ardupilot/ArduSub/mode_surftrak.cpp

166 lines
6.4 KiB
C++

#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);
}
}
}