Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: rename Loiter states and swap order within select statement

Browse Source
This commit is contained in:
Randy Mackay 2016-05-05 21:59:43 +09:00
parent 1c4db779a1
commit 2eb0484142
2 changed files with 24 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduCopter/control_loiter.cpp
View File

@ -73,10 +73,10 @@ void Copter::loiter_run()
} }
// Loiter State Machine Determination // Loiter State Machine Determination
if(!ap.auto_armed) { if (!motors.armed() || !motors.get_interlock()) {
loiter_state = Loiter_Disarmed; loiter_state = Loiter_MotorStopped;
} else if (!motors.get_interlock()){ } else if (!ap.auto_armed) {
loiter_state = Loiter_MotorStop; loiter_state = Loiter_NotAutoArmed;
} else if (takeoff_state.running || (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()))){ } else if (takeoff_state.running || (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()))){
loiter_state = Loiter_Takeoff; loiter_state = Loiter_Takeoff;
} else if (ap.land_complete){ } else if (ap.land_complete){
@ -88,23 +88,9 @@ void Copter::loiter_run()
// Loiter State Machine // Loiter State Machine
switch (loiter_state) { switch (loiter_state) {
case Loiter_Disarmed: case Loiter_MotorStopped:
wp_nav.init_loiter_target();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, 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);
// multicopters 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 Loiter_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
// run loiter controller // run loiter controller
@ -117,12 +103,26 @@ void Copter::loiter_run()
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 #else
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
wp_nav.init_loiter_target(); wp_nav.init_loiter_target();
// multicopters do not stabilize roll/pitch/yaw when motors are stopped // multicopters do not stabilize roll/pitch/yaw when motors 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 Loiter_NotAutoArmed:
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
wp_nav.init_loiter_target();
#if FRAME_CONFIG == HELI_FRAME
// Helicopters always stabilize roll/pitch/yaw
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, 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 Loiter_Takeoff: case Loiter_Takeoff:

4
ArduCopter/defines.h
View File

@ -225,8 +225,8 @@ enum AltHoldModeState {
// Loiter states // Loiter states
enum LoiterModeState { enum LoiterModeState {
Loiter_Disarmed, Loiter_MotorStopped,
Loiter_MotorStop, Loiter_NotAutoArmed,
Loiter_Takeoff, Loiter_Takeoff,
Loiter_Flying, Loiter_Flying,
Loiter_Landed Loiter_Landed