From e22220ab627858643a7fb5a134a6ba596089f45d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 7 Jul 2016 17:41:38 -0700 Subject: [PATCH] Copter: refactor landing to reduce duplication, use vertical vel ff --- ArduCopter/Copter.h | 3 +- ArduCopter/control_auto.cpp | 71 ++----------- ArduCopter/control_land.cpp | 205 +++++++++++++++++------------------- ArduCopter/control_rtl.cpp | 53 +--------- 4 files changed, 109 insertions(+), 223 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 74faecad0a..1546633096 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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(); diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 9a1c9e4cc6..f096073f06 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -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 diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 6d6bde5372..22df14b134 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -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 diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index 962770ad43..4713f471c9 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -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;