Added altimeter offset. Started roll and pitch controllers