Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: rtl uses throttle in 0 to 1 range and sets motor spool state

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

36
ArduCopter/control_rtl.cpp
View File

@ -114,12 +114,14 @@ void Copter::rtl_return_start()
void Copter::rtl_climb_return_run() void Copter::rtl_climb_return_run()
{ {
// if not auto armed or motor interlock 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(!ap.auto_armed || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#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_smooth(0, 0, 0, get_smoothing_gain()); 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); 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
// reset attitude control targets // 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
@ -137,6 +139,9 @@ void Copter::rtl_climb_return_run()
} }
} }
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller // run waypoint controller
wp_nav.update_wpnav(); wp_nav.update_wpnav();
@ -176,12 +181,14 @@ void Copter::rtl_loiterathome_start()
void Copter::rtl_loiterathome_run() void Copter::rtl_loiterathome_run()
{ {
// if not auto armed or motor interlock 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(!ap.auto_armed || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#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_smooth(0, 0, 0, get_smoothing_gain()); 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); 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
// reset attitude control targets // 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
@ -199,6 +206,9 @@ void Copter::rtl_loiterathome_run()
} }
} }
// set motors to full range
motors.set_desired_spool_state(AP_MotorsMulticopter::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller // run waypoint controller
wp_nav.update_wpnav(); wp_nav.update_wpnav();
@ -252,12 +262,14 @@ void Copter::rtl_descent_run()
float target_yaw_rate = 0; float target_yaw_rate = 0;
// if not auto armed or motor interlock 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(!ap.auto_armed || !motors.get_interlock()) { if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#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_smooth(0, 0, 0, get_smoothing_gain()); 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); 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); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif #endif
// set target to current position // set target to current position
@ -288,6 +300,9 @@ void Copter::rtl_descent_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); 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 // process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
@ -328,12 +343,14 @@ void Copter::rtl_land_run()
int16_t roll_control = 0, pitch_control = 0; int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0; float target_yaw_rate = 0;
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately // if not auto armed or landing completed 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 #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_smooth(0, 0, 0, get_smoothing_gain()); 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); 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); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif #endif
// set target to current position // set target to current position
@ -384,6 +401,9 @@ void Copter::rtl_land_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); 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 pilot's roll and pitch input // process pilot's roll and pitch input
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);