From b0428f0fe87fa18042bcdb2789de550d2badd2b7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 11 Jun 2019 13:13:24 +1000 Subject: [PATCH] Copter: make surface_tracking a class, various functions methods --- ArduCopter/Attitude.cpp | 55 ++++++++++++++++++------------------ ArduCopter/Copter.h | 20 ++++++++++--- ArduCopter/Log.cpp | 8 +----- ArduCopter/mode.cpp | 2 +- ArduCopter/mode_althold.cpp | 2 +- ArduCopter/mode_circle.cpp | 2 +- ArduCopter/mode_flowhold.cpp | 2 +- ArduCopter/mode_loiter.cpp | 2 +- ArduCopter/mode_poshold.cpp | 2 +- ArduCopter/mode_sport.cpp | 2 +- ArduCopter/mode_zigzag.cpp | 6 ++-- 11 files changed, 55 insertions(+), 48 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index ff056d6456..c34ad8e0e2 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -119,9 +119,10 @@ float Copter::get_non_takeoff_throttle() return MAX(0,motors->get_throttle_hover()/2.0f); } -// get_surface_tracking_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::get_surface_tracking_climb_rate(int16_t target_rate) +// 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(int16_t target_rate) { #if RANGEFINDER_ENABLED == ENABLED if (!copter.rangefinder_alt_ok()) { @@ -129,24 +130,24 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate) return target_rate; } - const float current_alt = inertial_nav.get_altitude(); - const float current_alt_target = pos_control->get_alt_target(); + const float current_alt = copter.inertial_nav.get_altitude(); + const float current_alt_target = copter.pos_control->get_alt_target(); float distance_error; float velocity_correction; uint32_t now = millis(); - surface_tracking.valid_for_logging = true; + valid_for_logging = true; // reset target altitude if this controller has just been engaged - if (now - surface_tracking.last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) { - surface_tracking.target_alt_cm = rangefinder_state.alt_cm + current_alt_target - current_alt; + if (now - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) { + target_alt_cm = copter.rangefinder_state.alt_cm + current_alt_target - current_alt; } - surface_tracking.last_update_ms = now; + last_update_ms = now; // adjust rangefinder target alt if motors have not hit their limits - if ((target_rate<0 && !motors->limit.throttle_lower) || (target_rate>0 && !motors->limit.throttle_upper)) { - surface_tracking.target_alt_cm += target_rate * G_Dt; + 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; } /* @@ -157,27 +158,27 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate) row. When that happens we reset the target altitude to the new reading */ - int32_t glitch_cm = rangefinder_state.alt_cm - surface_tracking.target_alt_cm; + int32_t glitch_cm = copter.rangefinder_state.alt_cm - target_alt_cm; if (glitch_cm >= RANGEFINDER_GLITCH_ALT_CM) { - rangefinder_state.glitch_count = MAX(rangefinder_state.glitch_count+1,1); + copter.rangefinder_state.glitch_count = MAX(copter.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); + copter.rangefinder_state.glitch_count = MIN(copter.rangefinder_state.glitch_count-1,-1); } else { - rangefinder_state.glitch_count = 0; + copter.rangefinder_state.glitch_count = 0; } - if (abs(rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) { + if (abs(copter.rangefinder_state.glitch_count) >= RANGEFINDER_GLITCH_NUM_SAMPLES) { // shift to the new rangefinder reading - surface_tracking.target_alt_cm = rangefinder_state.alt_cm; - rangefinder_state.glitch_count = 0; + target_alt_cm = copter.rangefinder_state.alt_cm; + copter.rangefinder_state.glitch_count = 0; } - if (rangefinder_state.glitch_count != 0) { + if (copter.rangefinder_state.glitch_count != 0) { // we are currently glitching, just use the target rate return target_rate; } // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations) - distance_error = (surface_tracking.target_alt_cm - rangefinder_state.alt_cm) - (current_alt_target - current_alt); - velocity_correction = distance_error * g.rangefinder_gain; + distance_error = (target_alt_cm - copter.rangefinder_state.alt_cm) - (current_alt_target - current_alt); + velocity_correction = 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 @@ -189,21 +190,21 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate) // get surfacing tracking alt // returns true if there is a valid target -bool Copter::get_surface_tracking_target_alt_cm(float &target_alt_cm) const +bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const { // check target has been updated recently - if (AP_HAL::millis() - surface_tracking.last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) { + if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) { return false; } - target_alt_cm = surface_tracking.target_alt_cm; + _target_alt_cm = target_alt_cm; return true; } // set surface tracking target altitude -void Copter::set_surface_tracking_target_alt_cm(float target_alt_cm) +void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm) { - surface_tracking.target_alt_cm = target_alt_cm; - surface_tracking.last_update_ms = AP_HAL::millis(); + target_alt_cm = _target_alt_cm; + last_update_ms = AP_HAL::millis(); } // get target climb rate reduced to avoid obstacles and altitude fence diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index c8482fa54f..dfdd9873d6 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -287,7 +287,22 @@ private: int8_t glitch_count; } rangefinder_state; - struct { + class SurfaceTracking { + public: + float adjust_climb_rate(int16_t target_rate); + bool get_target_alt_cm(float &target_alt_cm) const; + void set_target_alt_cm(float target_alt_cm); + float logging_target_alt() const { + if (!valid_for_logging) { + return AP::logger().quiet_nan(); + } + return target_alt_cm * 0.01f; // cm->m + } + void invalidate_for_logging() { + valid_for_logging = false; + } + + private: float target_alt_cm; // desired altitude in cm above the ground uint32_t last_update_ms; // system time of last update to target_alt_cm bool valid_for_logging; // true if target_alt_cm is valid for logging @@ -646,9 +661,6 @@ private: void set_throttle_takeoff(); float get_pilot_desired_climb_rate(float throttle_control); float get_non_takeoff_throttle(); - float get_surface_tracking_climb_rate(int16_t target_rate); - bool get_surface_tracking_target_alt_cm(float &target_alt_cm) const; - void set_surface_tracking_target_alt_cm(float target_alt_cm); float get_avoidance_adjusted_climbrate(float target_rate); void set_accel_throttle_I_from_pilot_throttle(); void rotate_body_frame_to_NE(float &x, float &y); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 7434e82b72..f34375d855 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -39,12 +39,6 @@ void Copter::Log_Write_Control_Tuning() target_climb_rate_cms = pos_control->get_vel_target_z(); } - float surface_tracking_target_alt; - if (surface_tracking.valid_for_logging) { - surface_tracking_target_alt = surface_tracking.target_alt_cm * 0.01f; // cm->m - } else { - surface_tracking_target_alt = logger.quiet_nan(); - } struct log_Control_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG), time_us : AP_HAL::micros64(), @@ -55,7 +49,7 @@ 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_target_alt, + desired_rangefinder_alt : surface_tracking.logging_target_alt(), rangefinder_alt : rangefinder_state.alt_cm, terr_alt : terr_alt, target_climb_rate : target_climb_rate_cms, diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 6407b992eb..d3fc02ffa0 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -265,7 +265,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) // called at 100hz or more void Copter::update_flight_mode() { - surface_tracking.valid_for_logging = false; // invalidate surface tracking alt, flight mode will set to true if used + surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used flightmode->run(); } diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index 154dbdd26d..b8e4d875a3 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -92,7 +92,7 @@ void ModeAltHold::run() #endif // adjust climb rate using rangefinder - target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate); + 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); diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index e1e60686b3..587e77f8b5 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -44,7 +44,7 @@ void ModeCircle::run() // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking - target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate); + target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); } // if not armed set throttle to zero and exit immediately diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 308ccc24dd..11ffa56cdb 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -296,7 +296,7 @@ void ModeFlowHold::run() copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // adjust climb rate using rangefinder - target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate); + target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); // get avoidance adjusted climb rate target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate); diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 5a51eadb79..30a6e2acb9 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -183,7 +183,7 @@ void ModeLoiter::run() 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.get_surface_tracking_climb_rate(target_climb_rate); + 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); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index df13a8c46a..e7a5b0f480 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -177,7 +177,7 @@ void ModePosHold::run() // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking - target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate); + target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); } // get avoidance adjusted climb rate diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 9ada89d009..462402f004 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -117,7 +117,7 @@ void ModeSport::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // adjust climb rate using rangefinder - target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate); + 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); diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 7aa5ebc81a..7ea3de38cb 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -125,7 +125,7 @@ void ModeZigZag::return_to_manual_control(bool maintain_target) const Vector3f wp_dest = wp_nav->get_wp_destination(); loiter_nav->init_target(wp_dest); if (maintain_target && wp_nav->origin_and_destination_are_terrain_alt()) { - copter.set_surface_tracking_target_alt_cm(wp_dest.z); + copter.surface_tracking.set_target_alt_cm(wp_dest.z); } gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control"); } @@ -200,7 +200,7 @@ void ModeZigZag::manual_control() 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.get_surface_tracking_climb_rate(target_climb_rate); + 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); @@ -283,7 +283,7 @@ bool ModeZigZag::calculate_next_dest(uint8_t dest_num, bool use_wpnav_alt, Vecto // if we have a downward facing range finder then use terrain altitude targets terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used(); if (terrain_alt) { - if (!copter.get_surface_tracking_target_alt_cm(next_dest.z)) { + if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) { next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); } } else {