|
|
|
@ -36,10 +36,6 @@ bool Sub::poshold_init(bool ignore_checks)
@@ -36,10 +36,6 @@ bool Sub::poshold_init(bool ignore_checks)
|
|
|
|
|
void Sub::poshold_run() |
|
|
|
|
{ |
|
|
|
|
uint32_t tnow = AP_HAL::millis(); |
|
|
|
|
// convert inertial nav earth-frame velocities to body-frame
|
|
|
|
|
// const Vector3f& vel = inertial_nav.get_velocity();
|
|
|
|
|
// float vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw();
|
|
|
|
|
// float vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw();
|
|
|
|
|
|
|
|
|
|
// if not auto armed set throttle to zero and exit immediately
|
|
|
|
|
if (!motors.armed() || !ap.auto_armed) { |
|
|
|
|