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

Copter: refactor landing to reduce duplication, use vertical vel ff

This commit is contained in:
Jonathan Challinger 2016-07-07 17:41:38 -07:00 committed by Randy Mackay
parent e311139a21
commit e22220ab62
4 changed files with 109 additions and 223 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/Copter.h
View File

@ -813,7 +813,8 @@ private:
void land_run();
void land_gps_run();
void land_nogps_run();
float get_land_descent_speed();
void land_run_vertical_control(bool pause_descent = false);
void land_run_horizontal_control();
void land_do_not_use_GPS();
void set_mode_land_with_pause(mode_reason_t reason);
bool landing_with_GPS();

71
ArduCopter/control_auto.cpp
View File

@ -360,8 +360,9 @@ void Copter::auto_land_start(const Vector3f& destination)
// initialise loiter target destination
wp_nav.init_loiter_target(destination);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
@ -371,11 +372,8 @@ void Copter::auto_land_start(const Vector3f& destination)
// called by auto_run at 100hz or more
void Copter::auto_land_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || ap.land_complete) {
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
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(0, 0, 0, get_smoothing_gain());
@ -390,64 +388,11 @@ void Copter::auto_land_run()
return;
}
// relax loiter targets if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// process pilot's input
if (!failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
}
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = channel_roll->get_control_in();
pitch_control = channel_pitch->get_control_in();
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
#if PRECISION_LANDING == ENABLED
// run precision landing
if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) {
Vector3f target_pos;
precland.get_target_position(target_pos);
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.freeze_ff_xy();
precland_last_update_ms = precland.last_update_ms();
}
#endif
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
float cmb_rate = get_land_descent_speed();
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
land_run_horizontal_control();
land_run_vertical_control();
}
// auto_rtl_start - initialises RTL in AUTO flight mode

205
ArduCopter/control_land.cpp
View File

@ -23,9 +23,10 @@ bool Copter::land_init(bool ignore_checks)
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
pos_control.set_accel_z(wp_nav.get_accel_z());
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
land_start_time = millis();
land_pause = false;
@ -52,9 +53,6 @@ void Copter::land_run()
// should be called at 100hz or more
void Copter::land_gps_run()
{
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
@ -81,78 +79,17 @@ void Copter::land_gps_run()
#endif
return;
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// process pilot inputs
if (!failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
}
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = channel_roll->get_control_in();
pitch_control = channel_pitch->get_control_in();
// record if pilot has overriden roll or pitch
if (roll_control != 0 || pitch_control != 0) {
ap.land_repo_active = true;
}
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
#if PRECISION_LANDING == ENABLED
// run precision landing
if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) {
Vector3f target_pos;
precland.get_target_position(target_pos);
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.freeze_ff_xy();
precland_last_update_ms = precland.last_update_ms();
}
#endif
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
// pause 4 seconds before beginning land descent
float cmb_rate;
if(land_pause && millis()-land_start_time < 4000) {
cmb_rate = 0;
} else {
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
cmb_rate = get_land_descent_speed();
}
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
land_run_horizontal_control();
land_run_vertical_control(land_pause);
}
// land_nogps_run - runs the land controller
@ -215,56 +152,102 @@ void Copter::land_nogps_run()
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// pause 4 seconds before beginning land descent
float cmb_rate;
if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) {
cmb_rate = 0;
} else {
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
cmb_rate = get_land_descent_speed();
}
land_run_vertical_control(land_pause);
}
void Copter::land_run_vertical_control(bool pause_descent)
{
bool navigating = pos_control.is_active_xy();
// compute desired velocity
int32_t alt_above_ground;
if (rangefinder_alt_ok()) {
alt_above_ground = rangefinder_state.alt_cm_filt.get();
} else {
if (!navigating || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_above_ground);
}
}
float cmb_rate = 0;
if (!pause_descent) {
cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, g.p_alt_hold.kP(), pos_control.get_accel_z());
cmb_rate = constrain_float(cmb_rate, pos_control.get_speed_down(), -abs(g.land_speed));
}
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// call position controller
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
}
// get_land_descent_speed - high level landing logic
// returns climb rate (in cm/s) which should be passed to the position controller
// should be called at 100hz or higher
float Copter::get_land_descent_speed()
void Copter::land_run_horizontal_control()
{
#if RANGEFINDER_ENABLED == ENABLED
bool rangefinder_ok = rangefinder_state.enabled && rangefinder_state.alt_healthy;
#else
bool rangefinder_ok = false;
#endif
// get position controller's target altitude above terrain
Location_Class target_loc = pos_control.get_pos_target();
int32_t target_alt_cm;
// get altitude target above home by default
target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt_cm);
// try to use terrain if enabled
if (terrain_use() && !target_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, target_alt_cm)) {
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
int16_t roll_control = 0, pitch_control = 0;
float target_yaw_rate = 0;
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// if we are above 10m and the rangefinder does not sense anything perform regular alt hold descent
if ((target_alt_cm >= LAND_START_ALT) && !rangefinder_ok) {
if (g.land_speed_high > 0) {
// user has asked for a different landing speed than normal descent rate
return -abs(g.land_speed_high);
// process pilot inputs
if (!failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
}
return pos_control.get_speed_down();
}else{
return -abs(g.land_speed);
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = channel_roll->get_control_in();
pitch_control = channel_pitch->get_control_in();
// record if pilot has overriden roll or pitch
if (roll_control != 0 || pitch_control != 0) {
ap.land_repo_active = true;
}
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
#if PRECISION_LANDING == ENABLED
bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired();
// run precision landing
if (doing_precision_landing && precland_last_update_ms != precland.last_update_ms()) {
Vector3f target_pos;
precland.get_target_position(target_pos);
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.freeze_ff_xy();
precland_last_update_ms = precland.last_update_ms();
}
#else
bool doing_precision_landing = false;
#endif
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}
// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch

53
ArduCopter/control_rtl.cpp
View File

@ -349,8 +349,9 @@ void Copter::rtl_land_start()
// Set wp navigation target to above home
wp_nav.init_loiter_target(wp_nav.get_wp_destination());
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
@ -360,8 +361,6 @@ void Copter::rtl_land_start()
// called by rtl_run at 100hz or more
void Copter::rtl_land_run()
{
int16_t roll_control = 0, pitch_control = 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 (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
@ -393,53 +392,11 @@ void Copter::rtl_land_run()
return;
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav.loiter_soften_for_landing();
}
// process pilot's input
if (!failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
}
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
roll_control = channel_roll->get_control_in();
pitch_control = channel_pitch->get_control_in();
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// process pilot's roll and pitch input
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
float cmb_rate = get_land_descent_speed();
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
// record desired climb rate for logging
desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
land_run_horizontal_control();
land_run_vertical_control();
// check if we've completed this stage of RTL
rtl_state_complete = ap.land_complete;