From 5fea74d4d06f65ce97cc983b38df447aacb5f1f2 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 31 Aug 2021 13:47:59 +0930 Subject: [PATCH] Copter: Separate landing and terrain following. --- ArduCopter/Copter.h | 6 +-- ArduCopter/mode.cpp | 2 +- ArduCopter/mode_althold.cpp | 9 ++-- ArduCopter/mode_auto.cpp | 7 ++- ArduCopter/mode_autorotate.cpp | 2 +- ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_circle.cpp | 9 ++-- ArduCopter/mode_flowhold.cpp | 9 ++-- ArduCopter/mode_guided.cpp | 2 +- ArduCopter/mode_loiter.cpp | 9 ++-- ArduCopter/mode_poshold.cpp | 12 ++--- ArduCopter/mode_sport.cpp | 9 ++-- ArduCopter/mode_throw.cpp | 4 +- ArduCopter/mode_zigzag.cpp | 9 ++-- ArduCopter/surface_tracking.cpp | 86 +++++++++++++-------------------- 15 files changed, 81 insertions(+), 96 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index d9f6f441d5111..c1688f636a7a5 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -268,8 +268,9 @@ class Copter : public AP_Vehicle { class SurfaceTracking { public: - // get desired climb rate (in cm/s) to achieve surface tracking - float adjust_climb_rate(float target_rate); + // update_surface_offset - manages the vertical offset of the position controller to follow the + // measured ground or ceiling level measured using the range finder. + void update_surface_offset(); // get/set target altitude (in cm) above ground bool get_target_alt_cm(float &target_alt_cm) const; @@ -291,7 +292,6 @@ class Copter : public AP_Vehicle { private: Surface surface = Surface::GROUND; - float target_dist_cm; // desired distance in cm from ground or ceiling uint32_t last_update_ms; // system time of last update to target_alt_cm uint32_t last_glitch_cleared_ms; // system time of last handle glitch recovery bool valid_for_logging; // true if target_alt_cm is valid for logging diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index b4022aea9fd5b..0ca6e7f42ae69 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -593,7 +593,7 @@ void Mode::land_run_vertical_control(bool pause_descent) } // update altitude target and call position controller - pos_control->set_pos_target_z_from_climb_rate_cm(cmb_rate, ignore_descent_limit); + pos_control->land_at_climb_rate_cm(cmb_rate, ignore_descent_limit); pos_control->update_z_controller(); } diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index a1e303f485e87..a1aa76f40c80a 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -84,13 +84,14 @@ void ModeAltHold::run() copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max); #endif - // adjust climb rate using rangefinder - target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); - // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); + // update the vertical offset based on the surface measurement + copter.surface_tracking.update_surface_offset(); + + // Send the commanded climb rate to the position controller + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); break; } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index dc6af3d3a3dde..13be10349a041 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1018,7 +1018,12 @@ void ModeAuto::loiter_to_alt_run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); + // update the vertical offset based on the surface measurement + copter.surface_tracking.update_surface_offset(); + + // Send the commanded climb rate to the position controller + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); + pos_control->update_z_controller(); } diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index 5b47f2fb78a00..17832679673c3 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -252,7 +252,7 @@ void ModeAutorotate::run() _pitch_target -= _target_pitch_adjust*G_Dt; } // Set position controller - pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z, false); + pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z); // Update controllers pos_control->update_z_controller(); diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 50652084090fd..e7a626194ee13 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -59,7 +59,7 @@ void ModeBrake::run() // call attitude controller attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f); - pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false); + pos_control->set_pos_target_z_from_climb_rate_cm(0.0f); pos_control->update_z_controller(); if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) { diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 43e2230893ddb..b20593d3c221f 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -89,11 +89,6 @@ void ModeCircle::run() // get pilot desired climb rate (or zero if in radio failsafe) float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - // adjust climb rate using rangefinder - if (copter.rangefinder_alt_ok()) { - // if rangefinder is ok, use surface tracking - target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); - } // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { @@ -104,7 +99,9 @@ void ModeCircle::run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // run circle controller + // update the vertical offset based on the surface measurement + copter.surface_tracking.update_surface_offset(); + copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate)); // call attitude controller diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 7ce771b6071b6..e2714a7df2a1b 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -297,13 +297,14 @@ void ModeFlowHold::run() case AltHold_Flying: copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // adjust climb rate using rangefinder - target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); - // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - copter.pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); + // update the vertical offset based on the surface measurement + copter.surface_tracking.update_surface_offset(); + + // Send the commanded climb rate to the position controller + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); break; } diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 865b1f74d8765..f5cf11ad41281 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -879,7 +879,7 @@ void ModeGuided::angle_control_run() if (guided_angle_state.use_thrust) { attitude_control->set_throttle_out(guided_angle_state.thrust, true, copter.g.throttle_filt); } else { - pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms, false); + pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms); pos_control->update_z_controller(); } } diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 1ee740d6130ae..a2acd4618ebb9 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -171,13 +171,14 @@ void ModeLoiter::run() // call attitude controller attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); - // adjust climb rate using rangefinder - target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); - // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); + // update the vertical offset based on the surface measurement + copter.surface_tracking.update_surface_offset(); + + // Send the commanded climb rate to the position controller + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); break; } diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index b6e031bf66955..981544c967ad8 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -158,16 +158,14 @@ void ModePosHold::run() case AltHold_Flying: motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // adjust climb rate using rangefinder - if (copter.rangefinder_alt_ok()) { - // if rangefinder is ok, use surface tracking - target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); - } - // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); + // update the vertical offset based on the surface measurement + copter.surface_tracking.update_surface_offset(); + + // Send the commanded climb rate to the position controller + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); break; } diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 1763c3df33d22..aedb88793c01b 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -105,13 +105,14 @@ void ModeSport::run() case AltHold_Flying: motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // adjust climb rate using rangefinder - target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); - // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); + // update the vertical offset based on the surface measurement + copter.surface_tracking.update_surface_offset(); + + // Send the commanded climb rate to the position controller + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); break; } diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 22d19070346c6..6e4bd999fc377 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -173,7 +173,7 @@ void ModeThrow::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); // call height controller - pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false); + pos_control->set_pos_target_z_from_climb_rate_cm(0.0f); pos_control->update_z_controller(); break; @@ -193,7 +193,7 @@ void ModeThrow::run() attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f); // call height controller - pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false); + pos_control->set_pos_target_z_from_climb_rate_cm(0.0f); pos_control->update_z_controller(); break; diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index cd666e7909562..12f785ff07efa 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -376,13 +376,14 @@ void ModeZigZag::manual_control() // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); - // adjust climb rate using rangefinder - target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); - // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); + // update the vertical offset based on the surface measurement + copter.surface_tracking.update_surface_offset(); + + // Send the commanded climb rate to the position controller + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate); break; } diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index b085f83348664..b2fcb29f2fc27 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -1,67 +1,46 @@ #include "Copter.h" -// adjust_climb_rate - hold copter at the desired distance above the -// ground; returns climb rate (in cm/s) which should be passed to -// the position controller -float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) +// update_surface_offset - manages the vertical offset of the position controller to follow the measured ground or ceiling +// level measured using the range finder. +void Copter::SurfaceTracking::update_surface_offset() { + float offset_cm = 0.0; #if RANGEFINDER_ENABLED == ENABLED // check tracking state and that range finders are healthy - if ((surface == Surface::NONE) || - ((surface == Surface::GROUND) && (!copter.rangefinder_alt_ok() || (copter.rangefinder_state.glitch_count != 0))) || - ((surface == Surface::CEILING) && !copter.rangefinder_up_ok()) || (copter.rangefinder_up_state.glitch_count != 0)) { - return target_rate; - } - - // calculate current ekf based altitude error - const float current_alt_error = copter.pos_control->get_pos_target_z_cm() - copter.inertial_nav.get_altitude(); - - // init based on tracking direction/state - RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state; - const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f; - - // reset target altitude if this controller has just been engaged - // target has been changed between upwards vs downwards - // or glitch has cleared - const uint32_t now = millis(); - if ((now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) || - reset_target || - (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) { - target_dist_cm = rf_state.alt_cm + (dir * current_alt_error); - reset_target = false; - last_glitch_cleared_ms = rf_state.glitch_cleared_ms;\ - } - last_update_ms = now; + if (((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) || + ((surface == Surface::CEILING) && copter.rangefinder_up_ok() && (copter.rangefinder_up_state.glitch_count == 0))) { - // adjust rangefinder target alt if motors have not hit their limits - if ((target_rate<0 && !copter.motors->limit.throttle_lower) || (target_rate>0 && !copter.motors->limit.throttle_upper)) { - target_dist_cm += dir * target_rate * copter.G_Dt; - } - valid_for_logging = true; + // init based on tracking direction/state + RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state; + const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f; + offset_cm = copter.inertial_nav.get_altitude() - dir * rf_state.alt_cm; -#if AC_AVOID_ENABLED == ENABLED - // upward facing terrain following never gets closer than avoidance margin - if (surface == Surface::CEILING) { - const float margin_cm = copter.avoid.enabled() ? copter.avoid.get_margin() * 100.0f : 0.0f; - target_dist_cm = MAX(target_dist_cm, margin_cm); + // reset target altitude if this controller has just been engaged + // target has been changed between upwards vs downwards + // or glitch has cleared + const uint32_t now = millis(); + if ((now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) || + reset_target || + (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) { + copter.pos_control->set_pos_offset_z_cm(offset_cm); + reset_target = false; + last_glitch_cleared_ms = rf_state.glitch_cleared_ms; + } + last_update_ms = now; + valid_for_logging = true; + } else { + copter.pos_control->set_pos_offset_z_cm(offset_cm); } -#endif - - // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations) - const float distance_error = (target_dist_cm - rf_state.alt_cm) - (dir * current_alt_error); - float velocity_correction = dir * distance_error * copter.g.rangefinder_gain; - velocity_correction = constrain_float(velocity_correction, -SURFACE_TRACKING_VELZ_MAX, SURFACE_TRACKING_VELZ_MAX); - - // return combined pilot climb rate + rate to correct rangefinder alt error - return (target_rate + velocity_correction); #else - return target_rate; + copter.pos_control->set_pos_offset_z_cm(offset_cm); #endif + copter.pos_control->set_pos_offset_target_z_cm(offset_cm); } + // get target altitude (in cm) above ground // returns true if there is a valid target -bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const +bool Copter::SurfaceTracking::get_target_alt_cm(float &target_alt_cm) const { // fail if we are not tracking downwards if (surface != Surface::GROUND) { @@ -71,7 +50,7 @@ bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) { return false; } - _target_alt_cm = target_dist_cm; + target_alt_cm = (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm()); return true; } @@ -82,7 +61,7 @@ void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm) if (surface != Surface::GROUND) { return; } - target_dist_cm = _target_alt_cm; + copter.pos_control->set_pos_offset_z_cm(copter.inertial_nav.get_altitude() - _target_alt_cm); last_update_ms = AP_HAL::millis(); } @@ -92,7 +71,8 @@ bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) co return false; } - target_dist = target_dist_cm * 0.01f; + const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f; + target_dist = dir * (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm()) * 0.01f; return true; }