Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: fix althold take-off state 

Also abort takeoff when switching into AltHold
This commit is contained in:
Randy Mackay 2015-06-22 11:33:19 +09:00
parent 15c57342a5
commit f79ac46d60
1 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduCopter/control_althold.cpp
View File

@ -16,6 +16,9 @@ bool Copter::althold_init(bool ignore_checks)
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
// stop takeoff if running
takeoff_stop();
return true;
}
@ -43,7 +46,7 @@ void Copter::althold_run()
// Alt Hold State Machine Determination
if(!ap.auto_armed || !motors.get_interlock()) {
althold_state = AltHold_Disarmed;
} else if (ap.land_complete && (takeoff_state.running || (channel_throttle->control_in > get_takeoff_trigger_throttle()))){
} else if (takeoff_state.running || (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle()))){
althold_state = AltHold_Takeoff;
} else if (ap.land_complete){
althold_state = AltHold_Landed;
@ -68,17 +71,17 @@ void Copter::althold_run()
case AltHold_Takeoff:
// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// initiate take-off
if (!takeoff_state.running) {
takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
// indicate we are taking off
set_land_complete(false);
// clear i terms
set_throttle_takeoff();
}
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
// get take-off adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());