Browse Source

Copter: poshold uses AP_Motors set_desired_spool_state

master
Randy Mackay 9 years ago
parent
commit
965f3827b4
  1. 8
      ArduCopter/control_poshold.cpp

8
ArduCopter/control_poshold.cpp

@ -142,7 +142,7 @@ void Copter::poshold_run() @@ -142,7 +142,7 @@ void Copter::poshold_run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED);
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
wp_nav.init_loiter_target();
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
@ -186,9 +186,9 @@ void Copter::poshold_run() @@ -186,9 +186,9 @@ void Copter::poshold_run()
if (ap.land_complete) {
// if throttle zero reset attitude and exit immediately
if (ap.throttle_zero) {
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_SPIN_WHEN_ARMED);
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
}else{
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
wp_nav.init_loiter_target();
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
@ -397,7 +397,7 @@ void Copter::poshold_run() @@ -397,7 +397,7 @@ void Copter::poshold_run()
}
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
//
// Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER)

Loading…
Cancel
Save