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

Copter : set land_complete to false during guided takeoff

Browse Source
This commit is contained in:
Clément 2017-02-07 10:50:08 +01:00 committed by Randy Mackay
parent 8a1962817c
commit 16f1af10ed
1 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/control_guided.cpp
View File

@ -333,17 +333,21 @@ void Copter::guided_run()
// called by guided_run at 100hz or more // called by guided_run at 100hz or more
void Copter::guided_takeoff_run() void Copter::guided_takeoff_run()
{ {
// if not auto armed or motors not enabled set throttle to zero and exit immediately // 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()) { if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
// initialise wpnav targets
wp_nav.shift_wp_origin_to_current_pos();
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller // call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt); attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else #else // multicopters do not stabilize roll/pitch/yaw when disarmed
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed // reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif #endif
// clear i term when we're taking off
set_throttle_takeoff();
return; return;
} }
@ -354,6 +358,18 @@ void Copter::guided_takeoff_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
} }
#if FRAME_CONFIG == HELI_FRAME
// helicopters stay in landed state until rotor speed runup has finished
if (motors.rotor_runup_complete()) {
set_land_complete(false);
} else {
// initialise wpnav targets
wp_nav.shift_wp_origin_to_current_pos();
}
#else
set_land_complete(false);
#endif
// set motors to full range // set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);