aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: land sets desired spool state

Browse Source
This commit is contained in:
Leonard Hall 2016-01-13 16:11:01 +09:00 committed by Randy Mackay
parent f71157c508
commit 100fcf799e
1 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/control_land.cpp
View File

@ -56,12 +56,14 @@ void Copter::land_gps_run()
float target_yaw_rate = 0;
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#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 // multicopters do not stabilize roll/pitch/yaw when disarmed
#else
motors.set_desired_spool_state(AP_MotorsMulticopter::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
wp_nav.init_loiter_target();
@ -113,6 +115,9 @@ void Copter::land_gps_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
@ -175,12 +180,14 @@ void Copter::land_nogps_run()
}
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#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(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 disarmed
#else
motors.set_desired_spool_state(AP_MotorsMulticopter::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
@ -198,6 +205,9 @@ void Copter::land_nogps_run()
return;
}
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());