mirror of https://github.com/ArduPilot/ardupilot
Sub: add SURFTRAK mode
This commit is contained in:
Clyde McQueen
Willian Galvani
parent
b486c1cb46
commit
f9db039dcc
|
|
@ -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)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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(),
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -86,3 +86,4 @@ BARO_EXT_BUS 1
|
|||
PILOT_ACCEL_Z 200
|
||||
PILOT_SPEED_UP 200
|
||||
PSC_JERK_Z 8
|
||||
LOG_BITMASK,180222
|
||||
|
|
|
|||
Loading…
Reference in New Issue