From 1c4f47f88210ce3abac69f9056aae8f4e64a1231 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 24 Aug 2019 10:59:22 +0900 Subject: [PATCH] Copter: add upward facing surface tracking --- ArduCopter/Copter.h | 22 +++++-- ArduCopter/Log.cpp | 15 +++-- ArduCopter/RC_Channel.cpp | 15 +++++ ArduCopter/sensors.cpp | 113 ++++++++++++++++++++------------ ArduCopter/surface_tracking.cpp | 78 +++++++++++++++------- 5 files changed, 170 insertions(+), 73 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5b12a6028f..6e3bd67c44 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -282,7 +282,7 @@ private: AP_InertialSensor ins; RangeFinder rangefinder; - struct { + struct RangeFinderState { bool enabled:1; bool alt_healthy:1; // true if we can trust the altitude from the rangefinder int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder @@ -291,7 +291,7 @@ private: int16_t alt_cm_glitch_protected; // last glitch protected altitude int8_t glitch_count; // non-zero number indicates rangefinder is glitching uint32_t glitch_cleared_ms; // system time glitch cleared - } rangefinder_state; + } rangefinder_state, rangefinder_up_state; class SurfaceTracking { public: @@ -302,15 +302,26 @@ private: bool get_target_alt_cm(float &target_alt_cm) const; void set_target_alt_cm(float target_alt_cm); - // get target altitude (in cm) for logging purposes - float logging_target_alt() const; + // get target and actual distances (in m) for logging purposes + bool get_dist_for_logging(float &target_dist, float &actual_dist) const; void invalidate_for_logging() { valid_for_logging = false; } + // surface tracking states + enum class SurfaceTrackingState { + SURFACE_TRACKING_DISABLED = 0, + SURFACE_TRACKING_GROUND = 1, + SURFACE_TRACKING_CEILING = 2 + }; + // set direction + void set_state(SurfaceTrackingState state); + private: - float target_alt_cm; // desired altitude in cm above the ground + SurfaceTrackingState tracking_state = SurfaceTrackingState::SURFACE_TRACKING_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 + bool reset_target; // true if target should be reset because of change in tracking_state } surface_tracking; #if RPM_ENABLED == ENABLED @@ -829,6 +840,7 @@ private: void init_rangefinder(void); void read_rangefinder(void); bool rangefinder_alt_ok(); + bool rangefinder_up_ok(); void rpm_update(); void init_optflow(); void update_optical_flow(void); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index c814eb3245..1b5a1589ff 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -16,7 +16,7 @@ struct PACKED log_Control_Tuning { float inav_alt; int32_t baro_alt; float desired_rangefinder_alt; - int16_t rangefinder_alt; + float rangefinder_alt; float terr_alt; int16_t target_climb_rate; int16_t climb_rate; @@ -39,6 +39,13 @@ void Copter::Log_Write_Control_Tuning() target_climb_rate_cms = pos_control->get_vel_target_z(); } + // get surface tracking alts + float desired_rangefinder_alt, rangefinder_alt; + if (!surface_tracking.get_dist_for_logging(desired_rangefinder_alt, rangefinder_alt)) { + desired_rangefinder_alt = AP::logger().quiet_nan(); + rangefinder_alt = AP::logger().quiet_nan();; + } + struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), time_us : AP_HAL::micros64(), @@ -49,8 +56,8 @@ void Copter::Log_Write_Control_Tuning() desired_alt : des_alt_m, inav_alt : inertial_nav.get_altitude() / 100.0f, baro_alt : baro_alt, - desired_rangefinder_alt : surface_tracking.logging_target_alt(), - rangefinder_alt : rangefinder_state.alt_cm, + desired_rangefinder_alt : desired_rangefinder_alt, + rangefinder_alt : rangefinder_alt, terr_alt : terr_alt, target_climb_rate : target_climb_rate_cms, climb_rate : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t @@ -386,7 +393,7 @@ const struct LogStructure Copter::log_structure[] = { { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning), "PTUN", "QBfff", "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" }, { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning), - "CTUN", "Qffffffefcfhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B0BBBB" }, + "CTUN", "Qffffffefffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B00BBB" }, { LOG_MOTBATT_MSG, sizeof(log_MotBatt), "MOTB", "Qffff", "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" }, { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t), diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 6bff073030..49b2421a5b 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -77,6 +77,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_ case AUX_FUNC::PRECISION_LOITER: case AUX_FUNC::INVERTED: case AUX_FUNC::WINCH_ENABLE: + case AUX_FUNC::SURFACE_TRACKING: do_aux_function(ch_option, ch_flag); break; // the following functions do not need to be initialised: @@ -541,6 +542,20 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw #endif break; + case AUX_FUNC::SURFACE_TRACKING: + switch (ch_flag) { + case LOW: + copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_GROUND); + break; + case MIDDLE: + copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_DISABLED); + break; + case HIGH: + copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_CEILING); + break; + } + break; + default: RC_Channel::do_aux_function(ch_option, ch_flag); break; diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index e1de784f09..e9d51bd467 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -17,6 +17,10 @@ void Copter::init_rangefinder(void) rangefinder.init(ROTATION_PITCH_270); rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270); + + // upward facing range finder + rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); + rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90); #endif } @@ -26,62 +30,81 @@ void Copter::read_rangefinder(void) #if RANGEFINDER_ENABLED == ENABLED rangefinder.update(); - rangefinder_state.alt_healthy = ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX)); +#if RANGEFINDER_TILT_CORRECTION == ENABLED + const float tilt_correction = MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z); +#else + const float tile_correction = 1.0f; +#endif - int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270); + // iterate through downward and upward facing lidar + struct { + RangeFinderState &state; + enum Rotation orientation; + } rngfnd[2] = { {rangefinder_state, ROTATION_PITCH_270}, {rangefinder_up_state, ROTATION_PITCH_90}}; - #if RANGEFINDER_TILT_CORRECTION == ENABLED - // correct alt for angle of the rangefinder - temp_alt = (float)temp_alt * MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z); - #endif + for (uint8_t i=0; i < ARRAY_SIZE(rngfnd); i++) { + // local variables to make accessing simpler + RangeFinderState &rf_state = rngfnd[i].state; + enum Rotation rf_orient = rngfnd[i].orientation; - // tilt corrected but unfiltered, not glitch protected alt - rangefinder_state.alt_cm = temp_alt; + // update health + rf_state.alt_healthy = ((rangefinder.status_orient(rf_orient) == RangeFinder::RangeFinder_Good) && + (rangefinder.range_valid_count_orient(rf_orient) >= RANGEFINDER_HEALTH_MAX)); - // glitch handling. rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading - // are considered a glitch and glitch_count becomes non-zero - // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row. - // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset - const int32_t glitch_cm = rangefinder_state.alt_cm - rangefinder_state.alt_cm_glitch_protected; - if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) { - rangefinder_state.glitch_count = MAX(rangefinder_state.glitch_count+1, 1); - } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) { - rangefinder_state.glitch_count = MIN(rangefinder_state.glitch_count-1, -1); - } else { - rangefinder_state.glitch_count = 0; - rangefinder_state.alt_cm_glitch_protected = rangefinder_state.alt_cm; - } - if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) { - // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes - rangefinder_state.glitch_count = 0; - rangefinder_state.alt_cm_glitch_protected = rangefinder_state.alt_cm; - rangefinder_state.glitch_cleared_ms = AP_HAL::millis(); - } + // tilt corrected but unfiltered, not glitch protected alt + rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient); - // filter rangefinder for use by AC_WPNav - uint32_t now = AP_HAL::millis(); - - const bool timed_out = now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS; - - if (rangefinder_state.alt_healthy) { - if (timed_out) { - // reset filter if we haven't used it within the last second - rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm); + // glitch handling. rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading + // are considered a glitch and glitch_count becomes non-zero + // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row. + // glitch_cleared_ms is set so surface tracking (or other consumers) can trigger a target reset + const int32_t glitch_cm = rf_state.alt_cm - rf_state.alt_cm_glitch_protected; + if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) { + rf_state.glitch_count = MAX(rf_state.glitch_count+1, 1); + } else if (glitch_cm <= -RANGEFINDER_GLITCH_ALT_CM) { + rf_state.glitch_count = MIN(rf_state.glitch_count-1, -1); } else { - rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.02f); + rf_state.glitch_count = 0; + rf_state.alt_cm_glitch_protected = rf_state.alt_cm; + } + if (abs(rf_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) { + // clear glitch and record time so consumers (i.e. surface tracking) can reset their target altitudes + rf_state.glitch_count = 0; + rf_state.alt_cm_glitch_protected = rf_state.alt_cm; + rf_state.glitch_cleared_ms = AP_HAL::millis(); } - rangefinder_state.last_healthy_ms = now; - } - // send rangefinder altitude and health to waypoint navigation library - if (rangefinder_state.alt_healthy || timed_out) { - wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); + // filter rangefinder altitude + uint32_t now = AP_HAL::millis(); + const bool timed_out = now - rf_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS; + if (rf_state.alt_healthy) { + if (timed_out) { + // reset filter if we haven't used it within the last second + rf_state.alt_cm_filt.reset(rf_state.alt_cm); + } else { + rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.02f); + } + rf_state.last_healthy_ms = now; + } + + // send downward facing lidar altitude and health to waypoint navigation library + if (rf_orient == ROTATION_PITCH_270) { + if (rangefinder_state.alt_healthy || timed_out) { + wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get()); + } + } } #else + // downward facing rangefinder rangefinder_state.enabled = false; rangefinder_state.alt_healthy = false; rangefinder_state.alt_cm = 0; + + // upward facing rangefinder + rangefinder_up_state.enabled = false; + rangefinder_up_state.alt_healthy = false; + rangefinder_up_state.alt_cm = 0; #endif } @@ -91,6 +114,12 @@ bool Copter::rangefinder_alt_ok() return (rangefinder_state.enabled && rangefinder_state.alt_healthy); } +// return true if rangefinder_alt can be used +bool Copter::rangefinder_up_ok() +{ + return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy); +} + /* update RPM sensors */ diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index eb9348a344..b248b515c2 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -6,39 +6,42 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) { #if RANGEFINDER_ENABLED == ENABLED - // if rangefinder alt is not ok or glitching, do not do surface tracking - if (!copter.rangefinder_alt_ok() || - (copter.rangefinder_state.glitch_count != 0)) { + // check tracking state and that range finders are healthy + if ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_DISABLED) || + ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) && (!copter.rangefinder_alt_ok() || (copter.rangefinder_state.glitch_count != 0))) || + ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_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_alt_target() - copter.inertial_nav.get_altitude(); + // init based on tracking direction/state + RangeFinderState &rf_state = (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state; + const float dir = (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_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) { - target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_error; - last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms; + 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; // 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_alt_cm += target_rate * copter.G_Dt; + target_dist_cm += dir * target_rate * copter.G_Dt; } valid_for_logging = true; - // handle glitch recovery by resetting target - if (copter.rangefinder_state.glitch_cleared_ms != last_glitch_cleared_ms) { - // shift to the new rangefinder reading - target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_error; - last_glitch_cleared_ms = copter.rangefinder_state.glitch_cleared_ms; - } - // 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_alt_cm - copter.rangefinder_state.alt_cm) - current_alt_error; - float velocity_correction = distance_error * copter.g.rangefinder_gain; + 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 @@ -52,25 +55,56 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) // returns true if there is a valid target bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const { + // fail if we are not tracking downwards + if (tracking_state != SurfaceTrackingState::SURFACE_TRACKING_GROUND) { + return false; + } // check target has been updated recently if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) { return false; } - _target_alt_cm = target_alt_cm; + _target_alt_cm = target_dist_cm; return true; } // set target altitude (in cm) above ground void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm) { - target_alt_cm = _target_alt_cm; + // fail if we are not tracking downwards + if (tracking_state != SurfaceTrackingState::SURFACE_TRACKING_GROUND) { + return; + } + target_dist_cm = _target_alt_cm; last_update_ms = AP_HAL::millis(); } -float Copter::SurfaceTracking::logging_target_alt() const +bool Copter::SurfaceTracking::get_dist_for_logging(float &target_dist, float &actual_dist) const { - if (!valid_for_logging) { - return AP::logger().quiet_nan(); + if (!valid_for_logging || (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_DISABLED)) { + return false; } - return target_alt_cm * 0.01f; // cm->m + target_dist = target_dist_cm * 0.01f; + actual_dist = ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? copter.rangefinder_state.alt_cm : copter.rangefinder_up_state.alt_cm) * 0.01f; + return true; +} + +// set direction +void Copter::SurfaceTracking::set_state(SurfaceTrackingState state) +{ + if (tracking_state == state) { + return; + } + // check we have a range finder in the correct direction + if ((state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) && !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) { + copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no downward rangefinder"); + AP_Notify::events.user_mode_change_failed = 1; + return; + } + if ((state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) && !copter.rangefinder.has_orientation(ROTATION_PITCH_90)) { + copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no upward rangefinder"); + AP_Notify::events.user_mode_change_failed = 1; + return; + } + tracking_state = state; + reset_target = true; }