mirror of https://github.com/ArduPilot/ardupilot
Copter: rename AltHold states and swap order within select statement
No functional change
This commit is contained in:
parent
932cd907d4
commit
9c12b64ba5
|
@ -64,9 +64,9 @@ void Copter::althold_run()
|
||||||
|
|
||||||
// Alt Hold State Machine Determination
|
// Alt Hold State Machine Determination
|
||||||
if (!motors.armed() || !motors.get_interlock()) {
|
if (!motors.armed() || !motors.get_interlock()) {
|
||||||
althold_state = AltHold_MotorStop;
|
althold_state = AltHold_MotorStopped;
|
||||||
} else if (!ap.auto_armed){
|
} else if (!ap.auto_armed){
|
||||||
althold_state = AltHold_Disarmed;
|
althold_state = AltHold_NotAutoArmed;
|
||||||
} else if (takeoff_state.running || takeoff_triggered){
|
} else if (takeoff_state.running || takeoff_triggered){
|
||||||
althold_state = AltHold_Takeoff;
|
althold_state = AltHold_Takeoff;
|
||||||
} else if (ap.land_complete){
|
} else if (ap.land_complete){
|
||||||
|
@ -78,23 +78,9 @@ void Copter::althold_run()
|
||||||
// Alt Hold State Machine
|
// Alt Hold State Machine
|
||||||
switch (althold_state) {
|
switch (althold_state) {
|
||||||
|
|
||||||
case AltHold_Disarmed:
|
case AltHold_MotorStopped:
|
||||||
|
|
||||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
|
||||||
attitude_control.set_yaw_target_to_current_heading();
|
|
||||||
// call attitude controller
|
|
||||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
|
||||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
|
||||||
#else
|
|
||||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
|
||||||
// Multicopter do not stabilize roll/pitch/yaw when disarmed
|
|
||||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
|
||||||
#endif // HELI_FRAME
|
|
||||||
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
|
|
||||||
break;
|
|
||||||
|
|
||||||
case AltHold_MotorStop:
|
|
||||||
|
|
||||||
|
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||||
#if FRAME_CONFIG == HELI_FRAME
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
// helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
|
// helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying
|
||||||
// call attitude controller
|
// call attitude controller
|
||||||
|
@ -103,11 +89,26 @@ void Copter::althold_run()
|
||||||
// force descent rate and call position controller
|
// force descent rate and call position controller
|
||||||
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
|
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
|
||||||
pos_control.update_z_controller();
|
pos_control.update_z_controller();
|
||||||
#else // Multicopter do not stabilize roll/pitch/yaw when motor are stopped
|
#else
|
||||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
// Multicopters do not stabilize roll/pitch/yaw when motor are stopped
|
||||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
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);
|
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
|
||||||
#endif // HELI_FRAME
|
#endif
|
||||||
|
break;
|
||||||
|
|
||||||
|
case AltHold_NotAutoArmed:
|
||||||
|
|
||||||
|
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||||
|
#if FRAME_CONFIG == HELI_FRAME
|
||||||
|
// Helicopters always stabilize roll/pitch/yaw
|
||||||
|
attitude_control.set_yaw_target_to_current_heading();
|
||||||
|
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||||
|
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||||
|
#else
|
||||||
|
// Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
|
||||||
|
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||||
|
#endif
|
||||||
|
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case AltHold_Takeoff:
|
case AltHold_Takeoff:
|
||||||
|
|
|
@ -216,8 +216,8 @@ enum RTLState {
|
||||||
|
|
||||||
// Alt_Hold states
|
// Alt_Hold states
|
||||||
enum AltHoldModeState {
|
enum AltHoldModeState {
|
||||||
AltHold_Disarmed,
|
AltHold_MotorStopped,
|
||||||
AltHold_MotorStop,
|
AltHold_NotAutoArmed,
|
||||||
AltHold_Takeoff,
|
AltHold_Takeoff,
|
||||||
AltHold_Flying,
|
AltHold_Flying,
|
||||||
AltHold_Landed
|
AltHold_Landed
|
||||||
|
|
Loading…
Reference in New Issue