Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Surface tracking in ArduSub (SURFTRAK mode) #23435

Merged
merged 3 commits into from
Feb 21, 2024
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 0 additions & 40 deletions ArduSub/Attitude.cpp
Original file line number Diff line number Diff line change
@@ -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)
{
3 changes: 3 additions & 0 deletions ArduSub/GCS_Mavlink.cpp
Original file line number Diff line number Diff line change
@@ -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;
}

10 changes: 8 additions & 2 deletions ArduSub/Log.cpp
Original file line number Diff line number Diff line change
@@ -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(),
24 changes: 14 additions & 10 deletions ArduSub/Parameters.cpp
Original file line number Diff line number Diff line change
@@ -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
7 changes: 5 additions & 2 deletions ArduSub/Parameters.h
Original file line number Diff line number Diff line change
@@ -190,7 +190,7 @@ class Parameters {
// 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 @@ class Parameters {
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 @@ class Parameters {
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
22 changes: 14 additions & 8 deletions ArduSub/Sub.h
Original file line number Diff line number Diff line change
@@ -111,6 +111,7 @@ class Sub : public AP_Vehicle {
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 @@ class Sub : public AP_Vehicle {
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 @@ class Sub : public AP_Vehicle {
// 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 @@ class Sub : public AP_Vehicle {
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 @@ class Sub : public AP_Vehicle {
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 @@ class Sub : public AP_Vehicle {
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 @@ class Sub : public AP_Vehicle {
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 @@ class Sub : public AP_Vehicle {

public:
void mainloop_failsafe_check();
bool rangefinder_alt_ok() const WARN_IF_UNUSED;

static Sub *_singleton;

@@ -611,6 +612,11 @@ class Sub : public AP_Vehicle {

// 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
};

20 changes: 10 additions & 10 deletions ArduSub/config.h
Original file line number Diff line number Diff line change
@@ -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)
5 changes: 5 additions & 0 deletions ArduSub/joystick.cpp
Original file line number Diff line number Diff line change
@@ -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
3 changes: 3 additions & 0 deletions ArduSub/mode.cpp
Original file line number Diff line number Diff line change
@@ -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;
37 changes: 36 additions & 1 deletion ArduSub/mode.h
Original file line number Diff line number Diff line change
@@ -49,7 +49,8 @@ class Mode
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 @@ class ModeAlthold : public Mode

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
{

15 changes: 11 additions & 4 deletions ArduSub/mode_althold.cpp
Original file line number Diff line number Diff line change
@@ -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();

}
165 changes: 165 additions & 0 deletions ArduSub/mode_surftrak.cpp
Original file line number Diff line number Diff line change
@@ -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);
}
}
}
36 changes: 27 additions & 9 deletions ArduSub/sensors.cpp
Original file line number Diff line number Diff line change
@@ -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,34 +47,45 @@ 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);
} else {
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);
}
71 changes: 69 additions & 2 deletions Tools/autotest/ardusub.py
Original file line number Diff line number Diff line change
@@ -14,6 +14,7 @@
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 @@ def AltitudeHold(self):
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 @@ def RngfndQuality(self):
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 @@ def ModeChanges(self, delta=0.2):
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 @@ def tests(self):
ret.extend([
self.DiveManual,
self.AltitudeHold,
self.Surftrak,
self.RngfndQuality,
self.PositionHold,
self.ModeChanges,
1 change: 1 addition & 0 deletions Tools/autotest/default_params/sub.parm
Original file line number Diff line number Diff line change
@@ -86,3 +86,4 @@ BARO_EXT_BUS 1
PILOT_ACCEL_Z 200
PILOT_SPEED_UP 200
PSC_JERK_Z 8
LOG_BITMASK,180222
4 changes: 2 additions & 2 deletions libraries/AP_JSButton/AP_JSButton.cpp
Original file line number Diff line number Diff line change
@@ -5,14 +5,14 @@ const AP_Param::GroupInfo JSButton::var_info[] = {
// @Param: FUNCTION
// @DisplayName: Function for button
// @Description: Set to 0 to disable or choose a function
// @Values: 0:Disabled,1:shift,2:arm_toggle,3:arm,4:disarm,5:mode_manual,6:mode_stabilize,7:mode_depth_hold,8:mode_poshold,9:mode_auto,10:mode_circle,11:mode_guided,12:mode_acro,21:mount_center,22:mount_tilt_up,23:mount_tilt_down,24:camera_trigger,25:camera_source_toggle,26:mount_pan_right,27:mount_pan_left,31:lights1_cycle,32:lights1_brighter,33:lights1_dimmer,34:lights2_cycle,35:lights2_brighter,36:lights2_dimmer,41:gain_toggle,42:gain_inc,43:gain_dec,44:trim_roll_inc,45:trim_roll_dec,46:trim_pitch_inc,47:trim_pitch_dec,48:input_hold_set,49:roll_pitch_toggle,51:relay_1_on,52:relay_1_off,53:relay_1_toggle,54:relay_2_on,55:relay_2_off,56:relay_2_toggle,57:relay_3_on,58:relay_3_off,59:relay_3_toggle,61:servo_1_inc,62:servo_1_dec,63:servo_1_min,64:servo_1_max,65:servo_1_center,66:servo_2_inc,67:servo_2_dec,68:servo_2_min,69:servo_2_max,70:servo_2_center,71:servo_3_inc,72:servo_3_dec,73:servo_3_min,74:servo_3_max,75:servo_3_center,76:servo_1_min_momentary,77:servo_1_max_momentary,78:servo_1_min_toggle,79:servo_1_max_toggle,80:servo_2_min_momentary,81:servo_2_max_momentary,82:servo_2_min_toggle,83:servo_2_max_toggle,84:servo_3_min_momentary,85:servo_3_max_momentary,86:servo_3_min_toggle,87:servo_3_max_toggle,91:custom_1,92:custom_2,93:custom_3,94:custom_4,95:custom_5,96:custom_6,101:relay_4_on,102:relay_4_off,103:relay_4_toggle,104:relay_1_momentary,105:relay_2_momentary,106:relay_3_momentary,107:relay_4_momentary,108:script_1,109:script_2,110:script_3,111:script_4
// @Values: 0:Disabled,1:shift,2:arm_toggle,3:arm,4:disarm,5:mode_manual,6:mode_stabilize,7:mode_depth_hold,8:mode_poshold,9:mode_auto,10:mode_circle,11:mode_guided,12:mode_acro,13:mode_surftrak,21:mount_center,22:mount_tilt_up,23:mount_tilt_down,24:camera_trigger,25:camera_source_toggle,26:mount_pan_right,27:mount_pan_left,31:lights1_cycle,32:lights1_brighter,33:lights1_dimmer,34:lights2_cycle,35:lights2_brighter,36:lights2_dimmer,41:gain_toggle,42:gain_inc,43:gain_dec,44:trim_roll_inc,45:trim_roll_dec,46:trim_pitch_inc,47:trim_pitch_dec,48:input_hold_set,49:roll_pitch_toggle,51:relay_1_on,52:relay_1_off,53:relay_1_toggle,54:relay_2_on,55:relay_2_off,56:relay_2_toggle,57:relay_3_on,58:relay_3_off,59:relay_3_toggle,61:servo_1_inc,62:servo_1_dec,63:servo_1_min,64:servo_1_max,65:servo_1_center,66:servo_2_inc,67:servo_2_dec,68:servo_2_min,69:servo_2_max,70:servo_2_center,71:servo_3_inc,72:servo_3_dec,73:servo_3_min,74:servo_3_max,75:servo_3_center,76:servo_1_min_momentary,77:servo_1_max_momentary,78:servo_1_min_toggle,79:servo_1_max_toggle,80:servo_2_min_momentary,81:servo_2_max_momentary,82:servo_2_min_toggle,83:servo_2_max_toggle,84:servo_3_min_momentary,85:servo_3_max_momentary,86:servo_3_min_toggle,87:servo_3_max_toggle,91:custom_1,92:custom_2,93:custom_3,94:custom_4,95:custom_5,96:custom_6,101:relay_4_on,102:relay_4_off,103:relay_4_toggle,104:relay_1_momentary,105:relay_2_momentary,106:relay_3_momentary,107:relay_4_momentary,108:script_1,109:script_2,110:script_3,111:script_4
// @User: Standard
AP_GROUPINFO("FUNCTION", 1, JSButton, _function, 0),

// @Param: SFUNCTION
// @DisplayName: Function for button when the shift mode is toggled on
// @Description: Set to 0 to disable or choose a function
// @Values: 0:Disabled,1:shift,2:arm_toggle,3:arm,4:disarm,5:mode_manual,6:mode_stabilize,7:mode_depth_hold,8:mode_poshold,9:mode_auto,10:mode_circle,11:mode_guided,12:mode_acro,21:mount_center,22:mount_tilt_up,23:mount_tilt_down,24:camera_trigger,25:camera_source_toggle,26:mount_pan_right,27:mount_pan_left,31:lights1_cycle,32:lights1_brighter,33:lights1_dimmer,34:lights2_cycle,35:lights2_brighter,36:lights2_dimmer,41:gain_toggle,42:gain_inc,43:gain_dec,44:trim_roll_inc,45:trim_roll_dec,46:trim_pitch_inc,47:trim_pitch_dec,48:input_hold_set,49:roll_pitch_toggle,51:relay_1_on,52:relay_1_off,53:relay_1_toggle,54:relay_2_on,55:relay_2_off,56:relay_2_toggle,57:relay_3_on,58:relay_3_off,59:relay_3_toggle,61:servo_1_inc,62:servo_1_dec,63:servo_1_min,64:servo_1_max,65:servo_1_center,66:servo_2_inc,67:servo_2_dec,68:servo_2_min,69:servo_2_max,70:servo_2_center,71:servo_3_inc,72:servo_3_dec,73:servo_3_min,74:servo_3_max,75:servo_3_center,76:servo_1_min_momentary,77:servo_1_max_momentary,78:servo_1_min_toggle,79:servo_1_max_toggle,80:servo_2_min_momentary,81:servo_2_max_momentary,82:servo_2_min_toggle,83:servo_2_max_toggle,84:servo_3_min_momentary,85:servo_3_max_momentary,86:servo_3_min_toggle,87:servo_3_max_toggle,91:custom_1,92:custom_2,93:custom_3,94:custom_4,95:custom_5,96:custom_6,101:relay_4_on,102:relay_4_off,103:relay_4_toggle,104:relay_1_momentary,105:relay_2_momentary,106:relay_3_momentary,107:relay_4_momentary,108:script_1,109:script_2,110:script_3,111:script_4
// @Values: 0:Disabled,1:shift,2:arm_toggle,3:arm,4:disarm,5:mode_manual,6:mode_stabilize,7:mode_depth_hold,8:mode_poshold,9:mode_auto,10:mode_circle,11:mode_guided,12:mode_acro,13:mode_surftrak,21:mount_center,22:mount_tilt_up,23:mount_tilt_down,24:camera_trigger,25:camera_source_toggle,26:mount_pan_right,27:mount_pan_left,31:lights1_cycle,32:lights1_brighter,33:lights1_dimmer,34:lights2_cycle,35:lights2_brighter,36:lights2_dimmer,41:gain_toggle,42:gain_inc,43:gain_dec,44:trim_roll_inc,45:trim_roll_dec,46:trim_pitch_inc,47:trim_pitch_dec,48:input_hold_set,49:roll_pitch_toggle,51:relay_1_on,52:relay_1_off,53:relay_1_toggle,54:relay_2_on,55:relay_2_off,56:relay_2_toggle,57:relay_3_on,58:relay_3_off,59:relay_3_toggle,61:servo_1_inc,62:servo_1_dec,63:servo_1_min,64:servo_1_max,65:servo_1_center,66:servo_2_inc,67:servo_2_dec,68:servo_2_min,69:servo_2_max,70:servo_2_center,71:servo_3_inc,72:servo_3_dec,73:servo_3_min,74:servo_3_max,75:servo_3_center,76:servo_1_min_momentary,77:servo_1_max_momentary,78:servo_1_min_toggle,79:servo_1_max_toggle,80:servo_2_min_momentary,81:servo_2_max_momentary,82:servo_2_min_toggle,83:servo_2_max_toggle,84:servo_3_min_momentary,85:servo_3_max_momentary,86:servo_3_min_toggle,87:servo_3_max_toggle,91:custom_1,92:custom_2,93:custom_3,94:custom_4,95:custom_5,96:custom_6,101:relay_4_on,102:relay_4_off,103:relay_4_toggle,104:relay_1_momentary,105:relay_2_momentary,106:relay_3_momentary,107:relay_4_momentary,108:script_1,109:script_2,110:script_3,111:script_4
// @User: Standard
AP_GROUPINFO("SFUNCTION", 2, JSButton, _sfunction, 0),

3 changes: 2 additions & 1 deletion libraries/AP_JSButton/AP_JSButton.h
Original file line number Diff line number Diff line change
@@ -22,8 +22,9 @@ class JSButton {
k_mode_circle = 10, ///< enter circle mode
k_mode_guided = 11, ///< enter guided mode
k_mode_acro = 12, ///< enter acro mode
k_mode_surftrak = 13, ///< enter surftrak mode

// 12-20 reserved for future mode functions
// 14-20 reserved for future mode functions
k_mount_center = 21, ///< move mount to center
k_mount_tilt_up = 22, ///< tilt mount up
k_mount_tilt_down = 23, ///< tilt mount down
13 changes: 13 additions & 0 deletions libraries/AP_Scripting/docs/docs.lua
Original file line number Diff line number Diff line change
@@ -1691,6 +1691,19 @@ function sub:is_button_pressed(index) end
---@return integer
function sub:get_and_clear_button_count(index) end

-- Return true if rangefinder is healthy, includes a check for good signal quality
---@return boolean
function sub:rangefinder_alt_ok() end

-- SURFTRAK mode: return the rangefinder target in cm
---@return float
function sub:get_rangefinder_target_cm() end

-- SURFTRAK mode: set the rangefinder target in cm, return true if successful
---@param new_target_cm float
---@return boolean
function sub:set_rangefinder_target_cm(new_target_cm) end


-- desc
---@class quadplane
3 changes: 3 additions & 0 deletions libraries/AP_Scripting/generator/description/bindings.desc
Original file line number Diff line number Diff line change
@@ -539,6 +539,9 @@ singleton Sub rename sub
singleton Sub depends APM_BUILD_TYPE(APM_BUILD_ArduSub)
singleton Sub method get_and_clear_button_count uint8_t uint8_t 1 4
singleton Sub method is_button_pressed boolean uint8_t 1 4
singleton Sub method rangefinder_alt_ok boolean
singleton Sub method get_rangefinder_target_cm float
singleton Sub method set_rangefinder_target_cm boolean float'skip_check

include AP_Motors/AP_MotorsMatrix.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
singleton AP_MotorsMatrix depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI