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

Copter: Fix helicopter ground stabilization in Auto, Brake, Circle, Guided, Land and RTL modes.

This commit is contained in:
Robert Lefebvre 2015-07-01 14:38:32 -04:00 committed by Randy Mackay
parent 14882bc6a8
commit 0a69c13b1d
6 changed files with 108 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ArduCopter/control_auto.cpp
View File

@ -115,8 +115,14 @@ void Copter::auto_takeoff_run()
if(!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
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// tell motors to do a slow start
motors.slow_start(true);
return;
@ -162,7 +168,13 @@ void Copter::auto_wp_run()
if(!ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// tell motors to do a slow start
motors.slow_start(true);
return;
@ -220,7 +232,13 @@ void Copter::auto_spline_run()
if(!ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// tell motors to do a slow start
motors.slow_start(true);
return;
@ -287,7 +305,13 @@ void Copter::auto_land_run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();
return;
@ -453,7 +477,13 @@ void Copter::auto_loiter_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!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.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
return;
}

6
ArduCopter/control_brake.cpp
View File

@ -37,7 +37,13 @@ void Copter::brake_run()
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average);
return;
}

6
ArduCopter/control_circle.cpp
View File

@ -37,7 +37,13 @@ void Copter::circle_run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
// To-Do: add some initialisation of position controllers
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
pos_control.set_alt_target_to_current_alt();
return;
}

12
ArduCopter/control_guided.cpp
View File

@ -163,7 +163,13 @@ void Copter::guided_run()
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
// To-Do: reset waypoint controller?
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true
return;
}
@ -201,8 +207,14 @@ void Copter::guided_takeoff_run()
if(!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
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// tell motors to do a slow start
motors.slow_start(true);
return;

47
ArduCopter/control_land.cpp
View File

@ -54,7 +54,13 @@ void Copter::land_gps_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 FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors 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();
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
@ -125,23 +131,6 @@ void Copter::land_nogps_run()
float target_roll = 0.0f, target_pitch = 0.0f;
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()) {
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
init_disarm_motors();
}
#else
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
}
#endif
return;
}
// process pilot inputs
if (!failsafe.radio) {
if (g.land_repositioning) {
@ -156,6 +145,30 @@ void Copter::land_nogps_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in);
}
// 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 FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
init_disarm_motors();
}
#else
// disarm when the landing detector says we've landed
if (ap.land_complete) {
init_disarm_motors();
}
#endif
return;
}
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());

24
ArduCopter/control_rtl.cpp
View File

@ -135,8 +135,14 @@ void Copter::rtl_climb_return_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets
return;
}
@ -191,8 +197,14 @@ void Copter::rtl_loiterathome_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets
return;
}
@ -261,7 +273,13 @@ void Copter::rtl_descent_run()
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if(!ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();
return;
@ -323,7 +341,13 @@ void Copter::rtl_land_run()
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(!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.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else // Multirotors do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// set target to current position
wp_nav.init_loiter_target();