Copter: Helicopters to force descent when motor is shut off

This commit is contained in:
Robert Lefebvre 2015-11-11 22:19:15 -05:00 committed by Randy Mackay
parent 58f0abaf4c
commit 652283a570
3 changed files with 59 additions and 2 deletions

View File

@ -9,6 +9,13 @@
// althold_init - initialise althold controller
bool Copter::althold_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete
if (!ignore_checks && !motors.rotor_runup_complete()){
return false;
}
#endif
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
@ -56,8 +63,10 @@ void Copter::althold_run()
#endif
// Alt Hold State Machine Determination
if(!ap.auto_armed || !motors.get_interlock()) {
if(!ap.auto_armed) {
althold_state = AltHold_Disarmed;
} else if (!motors.get_interlock()){
althold_state = AltHold_MotorStop;
} else if (takeoff_state.running || takeoff_triggered){
althold_state = AltHold_Takeoff;
} else if (ap.land_complete){
@ -82,6 +91,22 @@ void Copter::althold_run()
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case AltHold_MotorStop:
#if FRAME_CONFIG == HELI_FRAME
// helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
pos_control.update_z_controller();
#else // Multicopter do not stabilize roll/pitch/yaw when motor are stopped
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);
#endif // HELI_FRAME
break;
case AltHold_Takeoff:
// initiate take-off

View File

@ -9,6 +9,13 @@
// loiter_init - initialise loiter controller
bool Copter::loiter_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter Loiter if the Rotor Runup is not complete
if (!ignore_checks && !motors.rotor_runup_complete()){
return false;
}
#endif
if (position_ok() || ignore_checks) {
// set target to current position
@ -66,8 +73,10 @@ void Copter::loiter_run()
}
// Loiter State Machine Determination
if(!ap.auto_armed || !motors.get_interlock()) {
if(!ap.auto_armed) {
loiter_state = Loiter_Disarmed;
} else if (!motors.get_interlock()){
loiter_state = Loiter_MotorStop;
} else if (takeoff_state.running || (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()))){
loiter_state = Loiter_Takeoff;
} else if (ap.land_complete){
@ -92,6 +101,27 @@ void Copter::loiter_run()
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
break;
case Loiter_MotorStop:
#if FRAME_CONFIG == HELI_FRAME
// helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// force descent rate and call position controller
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
pos_control.update_z_controller();
#else
wp_nav.init_loiter_target();
// multicopters do not stabilize roll/pitch/yaw when motors are stopped
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);
#endif // HELI_FRAME
break;
case Loiter_Takeoff:
if (!takeoff_state.running) {

View File

@ -200,6 +200,7 @@ enum RTLState {
// Alt_Hold states
enum AltHoldModeState {
AltHold_Disarmed,
AltHold_MotorStop,
AltHold_Takeoff,
AltHold_Flying,
AltHold_Landed
@ -208,6 +209,7 @@ enum AltHoldModeState {
// Loiter states
enum LoiterModeState {
Loiter_Disarmed,
Loiter_MotorStop,
Loiter_Takeoff,
Loiter_Flying,
Loiter_Landed