From f9db039dcc3b1c8e2a2c8030d927a266d6a01f0c Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Wed, 21 Feb 2024 12:12:34 -0800 Subject: [PATCH] Sub: add SURFTRAK mode --- ArduSub/Attitude.cpp | 40 ------ ArduSub/GCS_Mavlink.cpp | 3 + ArduSub/Log.cpp | 10 +- ArduSub/Parameters.cpp | 24 ++-- ArduSub/Parameters.h | 7 +- ArduSub/Sub.h | 22 ++-- ArduSub/config.h | 20 +-- ArduSub/joystick.cpp | 5 + ArduSub/mode.cpp | 3 + ArduSub/mode.h | 37 +++++- ArduSub/mode_althold.cpp | 15 ++- ArduSub/mode_surftrak.cpp | 165 +++++++++++++++++++++++++ ArduSub/sensors.cpp | 36 ++++-- Tools/autotest/ardusub.py | 71 ++++++++++- Tools/autotest/default_params/sub.parm | 1 + 15 files changed, 371 insertions(+), 88 deletions(-) create mode 100644 ArduSub/mode_surftrak.cpp diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp index 408d9bfa6b..de41b8833e 100644 --- a/ArduSub/Attitude.cpp +++ b/ArduSub/Attitude.cpp @@ -123,46 +123,6 @@ float Sub::get_pilot_desired_climb_rate(float throttle_control) return desired_rate; } -// get_surface_tracking_climb_rate - hold vehicle at the desired distance above the ground -// returns climb rate (in cm/s) which should be passed to the position controller -float Sub::get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt) -{ -#if RANGEFINDER_ENABLED == ENABLED - static uint32_t last_call_ms = 0; - float distance_error; - float velocity_correction; - float current_alt = inertial_nav.get_position_z_up_cm(); - - uint32_t now = AP_HAL::millis(); - - // reset target altitude if this controller has just been engaged - if (now - last_call_ms > RANGEFINDER_TIMEOUT_MS) { - target_rangefinder_alt = rangefinder_state.alt_cm + current_alt_target - current_alt; - } - last_call_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)) { - target_rangefinder_alt += target_rate * dt; - } - - // do not let target altitude get too far from current altitude above ground - target_rangefinder_alt = constrain_float(target_rangefinder_alt, - rangefinder_state.alt_cm - pos_control.get_pos_error_z_down_cm(), - rangefinder_state.alt_cm + pos_control.get_pos_error_z_up_cm()); - - // 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 = (target_rangefinder_alt - rangefinder_state.alt_cm) - (current_alt_target - current_alt); - velocity_correction = distance_error * g.rangefinder_gain; - velocity_correction = constrain_float(velocity_correction, -THR_SURFACE_TRACKING_VELZ_MAX, THR_SURFACE_TRACKING_VELZ_MAX); - - // return combined pilot climb rate + rate to correct rangefinder alt error - return (target_rate + velocity_correction); -#else - return (float)target_rate; -#endif -} - // rotate vector from vehicle's perspective to North-East frame void Sub::rotate_body_frame_to_NE(float &x, float &y) { diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 3b5fb95d79..a2be923671 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -144,6 +144,9 @@ bool GCS_MAVLINK_Sub::send_info() CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); send_named_float("RollPitch", sub.roll_pitch_flag); + CHECK_PAYLOAD_SIZE(NAMED_VALUE_FLOAT); + send_named_float("RFTarget", sub.mode_surftrak.get_rangefinder_target_cm() / 100.0f); + return true; } diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 45603f2d3a..e197ad3ba4 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -28,7 +28,13 @@ void Sub::Log_Write_Control_Tuning() // get terrain altitude float terr_alt = 0.0f; #if AP_TERRAIN_AVAILABLE - terrain.height_above_terrain(terr_alt, true); + if (terrain.enabled()) { + terrain.height_above_terrain(terr_alt, true); + } else { + terr_alt = rangefinder_state.rangefinder_terrain_offset_cm * 0.01f; + } +#else + terr_alt = rangefinder_state.rangefinder_terrain_offset_cm * 0.01f; #endif struct log_Control_Tuning pkt = { @@ -41,7 +47,7 @@ void Sub::Log_Write_Control_Tuning() desired_alt : pos_control.get_pos_target_z_cm() / 100.0f, inav_alt : inertial_nav.get_position_z_up_cm() * 0.01f, baro_alt : barometer.get_altitude(), - desired_rangefinder_alt : (int16_t)target_rangefinder_alt, + desired_rangefinder_alt : (int16_t)mode_surftrak.get_rangefinder_target_cm(), rangefinder_alt : rangefinder_state.alt_cm, terr_alt : terr_alt, target_climb_rate : (int16_t)pos_control.get_vel_target_z_cms(), diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 8d0fd2ef97..1a469c4c1e 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -75,16 +75,6 @@ const AP_Param::Info Sub::var_info[] = { // @Bitmask: 0:Roll,1:Pitch,2:Yaw GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), -#if RANGEFINDER_ENABLED == ENABLED - // @Param: RNGFND_GAIN - // @DisplayName: Rangefinder gain - // @Description: Used to adjust the speed with which the target altitude is changed when objects are sensed below the sub - // @Range: 0.01 2.0 - // @Increment: 0.01 - // @User: Standard - GSCALAR(rangefinder_gain, "RNGFND_GAIN", RANGEFINDER_GAIN_DEFAULT), -#endif - // @Param: FS_GCS_ENABLE // @DisplayName: Ground Station Failsafe Enable // @Description: Controls what action to take when GCS heartbeat is lost. @@ -647,6 +637,20 @@ const AP_Param::Info Sub::var_info[] = { // @Group: RNGFND // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp GOBJECT(rangefinder, "RNGFND", RangeFinder), + + // @Param: RNGFND_SQ_MIN + // @DisplayName: Rangefinder signal quality minimum + // @Description: Minimum signal quality for good rangefinder readings + // @Range: 0 100 + // @User: Advanced + GSCALAR(rangefinder_signal_min, "RNGFND_SQ_MIN", RANGEFINDER_SIGNAL_MIN_DEFAULT), + + // @Param: SURFTRAK_DEPTH + // @DisplayName: SURFTRAK minimum depth + // @Description: Minimum depth to engage SURFTRAK mode + // @Units: cm + // @User: Standard + GSCALAR(surftrak_depth, "SURFTRAK_DEPTH", SURFTRAK_DEPTH_DEFAULT), #endif #if AP_TERRAIN_AVAILABLE diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 6ea9608f8c..0a9c1aa5dd 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -190,7 +190,7 @@ public: // Misc Sub settings k_param_log_bitmask = 165, k_param_angle_max = 167, - k_param_rangefinder_gain, + k_param_rangefinder_gain, // deprecated k_param_wp_yaw_behavior = 170, k_param_xtrack_angle_limit, // Angle limit for crosstrack correction in Auto modes (degrees) k_param_pilot_speed_up, // renamed from k_param_pilot_velocity_z_max @@ -228,6 +228,8 @@ public: k_param_cam_slew_limit = 237, // deprecated k_param_lights_steps, k_param_pilot_speed_dn, + k_param_rangefinder_signal_min, + k_param_surftrak_depth, k_param_vehicle = 257, // vehicle common block of parameters }; @@ -242,7 +244,8 @@ public: AP_Float throttle_filt; #if RANGEFINDER_ENABLED == ENABLED - AP_Float rangefinder_gain; + AP_Int8 rangefinder_signal_min; // minimum signal quality for good rangefinder readings + AP_Float surftrak_depth; // surftrak will try to keep sub below this depth #endif AP_Int8 failsafe_leak; // leak detection failsafe behavior diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 69b43eb8a5..483425f791 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -111,6 +111,7 @@ public: friend class ModeStabilize; friend class ModeAcro; friend class ModeAlthold; + friend class ModeSurftrak; friend class ModeGuided; friend class ModePoshold; friend class ModeAuto; @@ -147,9 +148,13 @@ private: 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 + int16_t min_cm; // min rangefinder distance (in cm) + int16_t max_cm; // max rangefinder distance (in cm) uint32_t last_healthy_ms; - LowPassFilterFloat alt_cm_filt; // altitude filter - } rangefinder_state = { false, false, 0, 0 }; + float inertial_alt_cm; // inertial alt at time of last rangefinder sample + float rangefinder_terrain_offset_cm; // terrain height above EKF origin + LowPassFilterFloat alt_cm_filt; // altitude filter + } rangefinder_state = { false, false, 0, 0, 0, 0, 0, 0 }; #if AP_RPM_ENABLED AP_RPM rpm_sensor; @@ -268,7 +273,6 @@ private: // Altitude // The cm/s we are moving up or down based on filtered data - Positive = UP int16_t climb_rate; - float target_rangefinder_alt; // desired altitude in cm above the ground // Turn counter int32_t quarter_turn_count; @@ -391,7 +395,6 @@ private: float get_roi_yaw(); float get_look_ahead_yaw(); float get_pilot_desired_climb_rate(float throttle_control); - float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); void rotate_body_frame_to_NE(float &x, float &y); #if HAL_LOGGING_ENABLED // methods for AP_Vehicle: @@ -475,7 +478,6 @@ private: void read_barometer(void); void init_rangefinder(void); void read_rangefinder(void); - bool rangefinder_alt_ok(void) const; void terrain_update(); void terrain_logging(); void init_ardupilot() override; @@ -533,9 +535,6 @@ private: void translate_circle_nav_rp(float &lateral_out, float &forward_out); void translate_pos_control_rp(float &lateral_out, float &forward_out); - bool surface_init(void); - void surface_run(); - void stats_update(); uint16_t get_pilot_speed_dn() const; @@ -587,6 +586,7 @@ private: ModeCircle mode_circle; ModeSurface mode_surface; ModeMotordetect mode_motordetect; + ModeSurftrak mode_surftrak; // Auto AutoSubMode auto_mode; // controls which auto controller is run @@ -598,6 +598,7 @@ private: public: void mainloop_failsafe_check(); + bool rangefinder_alt_ok() const WARN_IF_UNUSED; static Sub *_singleton; @@ -611,6 +612,11 @@ public: // For Lua scripting, so index is 1..4, not 0..3 uint8_t get_and_clear_button_count(uint8_t index); + +#if RANGEFINDER_ENABLED == ENABLED + float get_rangefinder_target_cm() const WARN_IF_UNUSED { return mode_surftrak.get_rangefinder_target_cm(); } + bool set_rangefinder_target_cm(float new_target_cm) { return mode_surftrak.set_rangefinder_target_cm(new_target_cm); } +#endif // RANGEFINDER_ENABLED #endif // AP_SCRIPTING_ENABLED }; diff --git a/ArduSub/config.h b/ArduSub/config.h index d4cdc55bf9..673cd17ae8 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -54,14 +54,6 @@ # define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder #endif -#ifndef RANGEFINDER_GAIN_DEFAULT -# define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction) -#endif - -#ifndef THR_SURFACE_TRACKING_VELZ_MAX -# define THR_SURFACE_TRACKING_VELZ_MAX 150 // max vertical speed change while surface tracking with rangefinder -#endif - #ifndef RANGEFINDER_TIMEOUT_MS # define RANGEFINDER_TIMEOUT_MS 1000 // desired rangefinder alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt #endif @@ -70,8 +62,16 @@ # define RANGEFINDER_WPNAV_FILT_HZ 0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class #endif -#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF -# define RANGEFINDER_TILT_CORRECTION ENABLED +#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF +# define RANGEFINDER_TILT_CORRECTION DISABLED +#endif + +#ifndef RANGEFINDER_SIGNAL_MIN_DEFAULT +# define RANGEFINDER_SIGNAL_MIN_DEFAULT 90 // rangefinder readings with signal quality below this value are ignored +#endif + +#ifndef SURFTRAK_DEPTH_DEFAULT +# define SURFTRAK_DEPTH_DEFAULT -50.0f // surftrak will try to keep the sub below this depth #endif // Avoidance (relies on Proximity and Fence) diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 0eeafe94fb..ad2710c6c1 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -190,6 +190,11 @@ void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held) case JSButton::button_function_t::k_mode_poshold: set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND); break; +#if RANGEFINDER_ENABLED == ENABLED + case JSButton::button_function_t::k_mode_surftrak: + set_mode(Mode::Number::SURFTRAK, ModeReason::RC_COMMAND); + break; +#endif case JSButton::button_function_t::k_mount_center: #if HAL_MOUNT_ENABLED diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp index 9a945d85e4..29d314d72a 100644 --- a/ArduSub/mode.cpp +++ b/ArduSub/mode.cpp @@ -38,6 +38,9 @@ Mode *Sub::mode_from_mode_num(const Mode::Number mode) case Mode::Number::ALT_HOLD: ret = &mode_althold; break; + case Mode::Number::SURFTRAK: + ret = &mode_surftrak; + break; case Mode::Number::POSHOLD: ret = &mode_poshold; break; diff --git a/ArduSub/mode.h b/ArduSub/mode.h index 584f5b26d3..7e584120c2 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -49,7 +49,8 @@ public: SURFACE = 9, // automatically return to surface, pilot maintains horizontal control POSHOLD = 16, // automatic position hold with manual override, with automatic throttle MANUAL = 19, // Pass-through input with no stabilization - MOTOR_DETECT = 20 // Automatically detect motors orientation + MOTOR_DETECT = 20, // Automatically detect motors orientation + SURFTRAK = 21 // Track distance above seafloor (hold range) }; // constructor @@ -266,11 +267,45 @@ public: protected: + void run_pre(); + void run_post(); + const char *name() const override { return "ALT_HOLD"; } const char *name4() const override { return "ALTH"; } }; +class ModeSurftrak : public ModeAlthold +{ + +public: + // constructor + ModeSurftrak(); + + void run() override; + + bool init(bool ignore_checks) override; + + float get_rangefinder_target_cm() const WARN_IF_UNUSED { return rangefinder_target_cm; } + bool set_rangefinder_target_cm(float target_cm); + +protected: + + const char *name() const override { return "SURFTRAK"; } + const char *name4() const override { return "STRK"; } + +private: + + void reset(); + void control_range(); + void update_surface_offset(); + + float rangefinder_target_cm; + + bool pilot_in_control; // pilot is moving up/down + float pilot_control_start_z_cm; // alt when pilot took control +}; + class ModeGuided : public Mode { diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp index 9102ff9f80..cf5b2a952f 100644 --- a/ArduSub/mode_althold.cpp +++ b/ArduSub/mode_althold.cpp @@ -22,6 +22,13 @@ bool ModeAlthold::init(bool ignore_checks) { // althold_run - runs the althold controller // should be called at 100hz or more void ModeAlthold::run() +{ + run_pre(); + control_depth(); + run_post(); +} + +void ModeAlthold::run_pre() { uint32_t tnow = AP_HAL::millis(); @@ -84,13 +91,14 @@ void ModeAlthold::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold - } else { // call attitude controller holding absolute absolute bearing + } else { // call attitude controller holding absolute bearing attitude_control->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, sub.last_pilot_heading, true); } } +} - control_depth(); - +void ModeAlthold::run_post() +{ motors.set_forward(channel_forward->norm_input()); motors.set_lateral(channel_lateral->norm_input()); } @@ -111,5 +119,4 @@ void ModeAlthold::control_depth() { position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s); position_control->update_z_controller(); - } diff --git a/ArduSub/mode_surftrak.cpp b/ArduSub/mode_surftrak.cpp new file mode 100644 index 0000000000..23f90271c5 --- /dev/null +++ b/ArduSub/mode_surftrak.cpp @@ -0,0 +1,165 @@ +#include "Sub.h" + +/* + * SURFTRAK (surface tracking) -- a variation on ALT_HOLD (depth hold) + * + * SURFTRAK starts in the "reset" state (rangefinder_target_cm < 0). SURFTRAK exits the reset state when these + * conditions are met: + * -- There is a good rangefinder reading (the rangefinder is healthy, the reading is between min and max, etc.) + * -- The sub is below SURFTRAK_DEPTH + * + * During normal operation, SURFTRAK sets the offset target to the current terrain altitude estimate and calls + * AC_PosControl to do the rest. + * + * We generally do not want to reset SURFTRAK if the rangefinder glitches, since that will result in a new rangefinder + * target. E.g., if a pilot is running 1m above the seafloor, there is a glitch, and the next rangefinder reading shows + * 1.1m, the desired behavior is to move 10cm closer to the seafloor, vs setting a new target of 1.1m above the + * seafloor. + * + * If the pilot takes control, SURFTRAK uses the change in depth readings to adjust the rangefinder target. This + * minimizes the "bounce back" that can happen as the slower rangefinder catches up to the quicker barometer. + */ + +#define INVALID_TARGET (-1) +#define HAS_VALID_TARGET (rangefinder_target_cm > 0) + +ModeSurftrak::ModeSurftrak() : + rangefinder_target_cm(INVALID_TARGET), + pilot_in_control(false), + pilot_control_start_z_cm(0) +{ } + +bool ModeSurftrak::init(bool ignore_checks) +{ + if (!ModeAlthold::init(ignore_checks)) { + return false; + } + + reset(); + + if (!sub.rangefinder_alt_ok()) { + sub.gcs().send_text(MAV_SEVERITY_INFO, "waiting for a rangefinder reading"); + } else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) { + sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to hold range", sub.g.surftrak_depth * 0.01f); + } + + return true; +} + +void ModeSurftrak::run() +{ + run_pre(); + control_range(); + run_post(); +} + +/* + * Set the rangefinder target, return true if successful. This may be called from scripting so run a few extra checks. + */ +bool ModeSurftrak::set_rangefinder_target_cm(float target_cm) +{ + bool success = false; + + if (sub.control_mode != Number::SURFTRAK) { + sub.gcs().send_text(MAV_SEVERITY_WARNING, "wrong mode, rangefinder target not set"); + } else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) { + sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to set rangefinder target", sub.g.surftrak_depth * 0.01f); + } else if (target_cm < (float)sub.rangefinder_state.min_cm) { + sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target below minimum, ignored"); + } else if (target_cm > (float)sub.rangefinder_state.max_cm) { + sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target above maximum, ignored"); + } else { + success = true; + } + + if (success) { + rangefinder_target_cm = target_cm; + sub.gcs().send_text(MAV_SEVERITY_INFO, "rangefinder target is %.2f meters", rangefinder_target_cm * 0.01f); + + // Initialize the terrain offset + auto terrain_offset_cm = sub.inertial_nav.get_position_z_up_cm() - rangefinder_target_cm; + sub.pos_control.set_pos_offset_z_cm(terrain_offset_cm); + sub.pos_control.set_pos_offset_target_z_cm(terrain_offset_cm); + + } else { + reset(); + } + + return success; +} + +void ModeSurftrak::reset() +{ + rangefinder_target_cm = INVALID_TARGET; + + // Reset the terrain offset + sub.pos_control.set_pos_offset_z_cm(0); + sub.pos_control.set_pos_offset_target_z_cm(0); +} + +/* + * Main controller, call at 100hz+ + */ +void ModeSurftrak::control_range() { + float target_climb_rate_cm_s = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in()); + target_climb_rate_cm_s = constrain_float(target_climb_rate_cm_s, -sub.get_pilot_speed_dn(), g.pilot_speed_up); + + // Desired_climb_rate returns 0 when within the deadzone + if (fabsf(target_climb_rate_cm_s) < 0.05f) { + if (pilot_in_control) { + // Pilot has released control; apply the delta to the rangefinder target + set_rangefinder_target_cm(rangefinder_target_cm + inertial_nav.get_position_z_up_cm() - pilot_control_start_z_cm); + pilot_in_control = false; + } + if (sub.ap.at_surface) { + // Set target depth to 5 cm below SURFACE_DEPTH and reset + position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f)); + reset(); + } else if (sub.ap.at_bottom) { + // Set target depth to 10 cm above bottom and reset + position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm())); + reset(); + } else { + // Typical operation + update_surface_offset(); + } + } else if (HAS_VALID_TARGET && !pilot_in_control) { + // Pilot has taken control; note the current depth + pilot_control_start_z_cm = inertial_nav.get_position_z_up_cm(); + pilot_in_control = true; + } + + // Set the target altitude from the climb rate and the terrain offset + position_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate_cm_s); + + // Run the PID controllers + position_control->update_z_controller(); +} + +/* + * Update the AC_PosControl terrain offset if we have a good rangefinder reading + */ +void ModeSurftrak::update_surface_offset() +{ + if (sub.rangefinder_alt_ok()) { + // Get the latest terrain offset + float rangefinder_terrain_offset_cm = sub.rangefinder_state.rangefinder_terrain_offset_cm; + + // Handle the first reading or a reset + if (!HAS_VALID_TARGET && sub.rangefinder_state.inertial_alt_cm < sub.g.surftrak_depth) { + set_rangefinder_target_cm(sub.rangefinder_state.inertial_alt_cm - rangefinder_terrain_offset_cm); + } + + if (HAS_VALID_TARGET) { + // Will the new offset target cause the sub to ascend above SURFTRAK_DEPTH? + float desired_z_cm = rangefinder_terrain_offset_cm + rangefinder_target_cm; + if (desired_z_cm >= sub.g.surftrak_depth) { + // Adjust the terrain offset to stay below SURFTRAK_DEPTH, this should avoid "at_surface" events + rangefinder_terrain_offset_cm += sub.g.surftrak_depth - desired_z_cm; + } + + // Set the offset target, AC_PosControl will do the rest + sub.pos_control.set_pos_offset_target_z_cm(rangefinder_terrain_offset_cm); + } + } +} diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index 481930cd75..a8cb75c7fd 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -18,6 +18,7 @@ void Sub::read_barometer() void Sub::init_rangefinder() { #if RANGEFINDER_ENABLED == ENABLED + rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN); 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); @@ -30,7 +31,13 @@ void Sub::read_rangefinder() #if RANGEFINDER_ENABLED == ENABLED rangefinder.update(); - rangefinder_state.alt_healthy = ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) && (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX)); + // signal quality ranges from 0 (worst) to 100 (perfect), -1 means n/a + int8_t signal_quality_pct = rangefinder.signal_quality_pct_orient(ROTATION_PITCH_270); + + rangefinder_state.alt_healthy = + (rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) && + (rangefinder.range_valid_count_orient(ROTATION_PITCH_270) >= RANGEFINDER_HEALTH_MAX) && + (signal_quality_pct == -1 || signal_quality_pct >= g.rangefinder_signal_min); int16_t temp_alt = rangefinder.distance_cm_orient(ROTATION_PITCH_270); @@ -40,11 +47,13 @@ void Sub::read_rangefinder() #endif rangefinder_state.alt_cm = temp_alt; + rangefinder_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm(); + rangefinder_state.min_cm = rangefinder.min_distance_cm_orient(ROTATION_PITCH_270); + rangefinder_state.max_cm = rangefinder.max_distance_cm_orient(ROTATION_PITCH_270); - // filter rangefinder for use by AC_WPNav - uint32_t now = AP_HAL::millis(); - + // calculate rangefinder_terrain_offset_cm if (rangefinder_state.alt_healthy) { + uint32_t now = AP_HAL::millis(); if (now - rangefinder_state.last_healthy_ms > RANGEFINDER_TIMEOUT_MS) { // reset filter if we haven't used it within the last second rangefinder_state.alt_cm_filt.reset(rangefinder_state.alt_cm); @@ -52,22 +61,31 @@ void Sub::read_rangefinder() rangefinder_state.alt_cm_filt.apply(rangefinder_state.alt_cm, 0.05f); } rangefinder_state.last_healthy_ms = now; + rangefinder_state.rangefinder_terrain_offset_cm = + sub.rangefinder_state.inertial_alt_cm - sub.rangefinder_state.alt_cm_filt.get(); } // send rangefinder altitude and health to waypoint navigation library - const float terrain_offset_cm = inertial_nav.get_position_z_up_cm() - rangefinder_state.alt_cm_filt.get(); - wp_nav.set_rangefinder_terrain_offset(rangefinder_state.enabled, rangefinder_state.alt_healthy, terrain_offset_cm); - circle_nav.set_rangefinder_terrain_offset(rangefinder_state.enabled && wp_nav.rangefinder_used(), rangefinder_state.alt_healthy, terrain_offset_cm); - + wp_nav.set_rangefinder_terrain_offset( + rangefinder_state.enabled, + rangefinder_state.alt_healthy, + rangefinder_state.rangefinder_terrain_offset_cm); + circle_nav.set_rangefinder_terrain_offset( + rangefinder_state.enabled && wp_nav.rangefinder_used(), + rangefinder_state.alt_healthy, + rangefinder_state.rangefinder_terrain_offset_cm); #else rangefinder_state.enabled = false; rangefinder_state.alt_healthy = false; rangefinder_state.alt_cm = 0; + rangefinder_state.inertial_alt_cm = 0; + rangefinder_state.rangefinder_terrain_offset_cm = 0; #endif } // return true if rangefinder_alt can be used bool Sub::rangefinder_alt_ok() const { - return (rangefinder_state.enabled && rangefinder_state.alt_healthy); + uint32_t now = AP_HAL::millis(); + return (rangefinder_state.enabled && rangefinder_state.alt_healthy && now - rangefinder_state.last_healthy_ms < RANGEFINDER_TIMEOUT_MS); } diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 6d234c8ebf..73ef3e08c9 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -14,6 +14,7 @@ from pymavlink import mavutil import vehicle_test_suite from vehicle_test_suite import NotAchievedException from vehicle_test_suite import AutoTestTimeoutException +from vehicle_test_suite import PreconditionFailedException if sys.version_info[0] < 3: ConnectionResetError = AutoTestTimeoutException @@ -114,7 +115,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite): raise NotAchievedException("Did not get GLOBAL_POSITION_INT") pwm = 1300 if msg.relative_alt/1000.0 < -6.0: - # need to g`o up, not down! + # need to go up, not down! pwm = 1700 self.set_rc(Joystick.Throttle, pwm) self.wait_altitude(altitude_min=-6, altitude_max=-5) @@ -204,8 +205,69 @@ class AutoTestSub(vehicle_test_suite.TestSuite): if ex: raise ex + def watch_distance_maintained(self, delta=0.3, timeout=5.0): + """Watch and wait for the rangefinder reading to be maintained""" + tstart = self.get_sim_time_cached() + previous_distance = self.mav.recv_match(type='RANGEFINDER', blocking=True).distance + self.progress('Distance to be watched: %.2f' % previous_distance) + while True: + m = self.mav.recv_match(type='RANGEFINDER', blocking=True) + if self.get_sim_time_cached() - tstart > timeout: + self.progress('Distance hold done: %f' % previous_distance) + return + if abs(m.distance - previous_distance) > delta: + raise NotAchievedException( + "Distance not maintained: want %.2f (+/- %.2f) got=%.2f" % + (previous_distance, delta, m.distance)) + + def Surftrak(self): + """Test SURFTRAK mode""" + + if self.get_parameter('RNGFND1_MAX_CM') != 3000.0: + raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0) + + # Something closer to Bar30 noise + self.context_push() + self.set_parameter("SIM_BARO_RND", 0.01) + + self.wait_ready_to_arm() + self.arm_vehicle() + self.change_mode('MANUAL') + + # Dive to -5m, outside of rangefinder range, will act like ALT_HOLD + pwm = 1300 if self.get_altitude(relative=True) > -6 else 1700 + self.set_rc(Joystick.Throttle, pwm) + self.wait_altitude(altitude_min=-6, altitude_max=-5, relative=False, timeout=60) + self.set_rc(Joystick.Throttle, 1500) + self.delay_sim_time(1) + self.context_collect('STATUSTEXT') + self.change_mode(21) + self.wait_statustext('waiting for a rangefinder reading', check_context=True) + self.context_clear_collection('STATUSTEXT') + self.watch_altitude_maintained() + + # Move into range, should set a rangefinder target and maintain it + self.set_rc(Joystick.Throttle, 1300) + self.wait_altitude(altitude_min=-26, altitude_max=-25, relative=False, timeout=60) + self.set_rc(Joystick.Throttle, 1500) + self.delay_sim_time(4) + self.wait_statustext('rangefinder target is', check_context=True) + self.context_clear_collection('STATUSTEXT') + self.watch_distance_maintained() + + # Move a few meters, should apply a delta and maintain the new rangefinder target + self.set_rc(Joystick.Throttle, 1300) + self.wait_altitude(altitude_min=-31, altitude_max=-30, relative=False, timeout=60) + self.set_rc(Joystick.Throttle, 1500) + self.delay_sim_time(4) + self.wait_statustext('rangefinder target is', check_context=True) + self.watch_distance_maintained() + + self.disarm_vehicle() + self.context_pop() + def ModeChanges(self, delta=0.2): - """Check if alternating between ALTHOLD, STABILIZE and POSHOLD affects altitude""" + """Check if alternating between ALTHOLD, STABILIZE, POSHOLD and SURFTRAK (mode 21) affects altitude""" self.wait_ready_to_arm() self.arm_vehicle() # zero buoyancy and no baro noise @@ -225,12 +287,16 @@ class AutoTestSub(vehicle_test_suite.TestSuite): self.delay_sim_time(2) self.change_mode('STABILIZE') self.delay_sim_time(2) + self.change_mode(21) + self.delay_sim_time(2) self.change_mode('ALT_HOLD') self.delay_sim_time(2) self.change_mode('STABILIZE') self.delay_sim_time(2) self.change_mode('ALT_HOLD') self.delay_sim_time(2) + self.change_mode(21) + self.delay_sim_time(2) self.change_mode('MANUAL') self.disarm_vehicle() final_altitude = self.mav.recv_match(type='VFR_HUD', blocking=True).alt @@ -562,6 +628,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite): ret.extend([ self.DiveManual, self.AltitudeHold, + self.Surftrak, self.RngfndQuality, self.PositionHold, self.ModeChanges, diff --git a/Tools/autotest/default_params/sub.parm b/Tools/autotest/default_params/sub.parm index 3a173142cb..bef934263f 100644 --- a/Tools/autotest/default_params/sub.parm +++ b/Tools/autotest/default_params/sub.parm @@ -86,3 +86,4 @@ BARO_EXT_BUS 1 PILOT_ACCEL_Z 200 PILOT_SPEED_UP 200 PSC_JERK_Z 8 +LOG_BITMASK,180222