mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
116 lines
3.8 KiB
C++
116 lines
3.8 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
// Code by Jacob Walser: jwalser90@gmail.com
|
|
|
|
#include "Sub.h"
|
|
|
|
#define SURFACE_DEPTH -0.05 // 5cm depends on where the external depth sensor is mounted
|
|
#define DIVE_DEPTH_CONSTRAINT 0.10 // 10cm if we cannot achieve this target, we are trying to dig into the bottom
|
|
#define BOTTOM_DETECTOR_TRIGGER_SEC 1.0
|
|
#define SURFACE_DETECTOR_TRIGGER_SEC 1.0
|
|
|
|
// counter to verify contact with bottom
|
|
static uint32_t bottom_detector_count = 0;
|
|
static uint32_t surface_detector_count = 0;
|
|
static float current_depth = 0;
|
|
static float start_depth = 0; // the depth when we first hit downward throttle limit
|
|
|
|
// checks if we have have hit bottom or surface and updates the ap.at_bottom and ap.at_surface flags
|
|
// called at MAIN_LOOP_RATE
|
|
void Sub::update_surface_and_bottom_detector()
|
|
{
|
|
// update 1hz filtered acceleration
|
|
Vector3f accel_ef = ahrs.get_accel_ef_blended();
|
|
accel_ef.z += GRAVITY_MSS;
|
|
depth_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS);
|
|
|
|
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
|
|
bool accel_stationary = (depth_accel_ef_filter.get().length() <= 1.0f);
|
|
|
|
if (ap.depth_sensor_present) { // we can use the external pressure sensor for a very accurate and current measure of our z axis position
|
|
current_depth = barometer.get_altitude();
|
|
|
|
|
|
set_surfaced(current_depth > SURFACE_DEPTH); // If we are above surface depth, we are surfaced
|
|
|
|
// ToDo maybe we can lighten the throttle check to a less extreme value, will depend on buoyancy and effective motor force
|
|
// it won't make a difference beyond informational purposes for non-auto control modes ie stabilize
|
|
if (motors.limit.throttle_lower) { // We can't predict where the bottom is, unless we have a sounder
|
|
if (bottom_detector_count == 0) {
|
|
start_depth = current_depth;
|
|
}
|
|
|
|
if (current_depth > start_depth - DIVE_DEPTH_CONSTRAINT) { // If we can't descend a short distance at full throttle, we are at the bottom
|
|
if( bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
|
|
bottom_detector_count++;
|
|
} else {
|
|
set_bottomed(true);
|
|
}
|
|
} else {
|
|
set_bottomed(false); // We are still moving down
|
|
}
|
|
} else {
|
|
set_bottomed(false); // If we are not trying to descend at lower throttle limit, we are not at the bottom
|
|
}
|
|
|
|
}
|
|
// else if (accel_stationary) {
|
|
// if(motors.limit.throttle_upper) {
|
|
// if( surface_detector_count < ((float)SURFACE_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
|
|
// surface_detector_count++;
|
|
// } else {
|
|
// set_surfaced(true);
|
|
// }
|
|
// } else if(motors.limit.throttle_lower) {
|
|
// // landed criteria met - increment the counter and check if we've triggered
|
|
// if( bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
|
|
// bottom_detector_count++;
|
|
// } else {
|
|
// set_bottomed(true);
|
|
// }
|
|
// } else {
|
|
// set_surfaced(false);
|
|
// set_bottomed(false);
|
|
// }
|
|
// } else {
|
|
// // we've sensed movement up or down so reset land_detector
|
|
// set_surfaced(false);
|
|
// set_bottomed(false);
|
|
// }
|
|
|
|
|
|
}
|
|
|
|
void Sub::set_surfaced(bool at_surface) {
|
|
if(ap.at_surface == at_surface) { // do nothing if state unchanged
|
|
return;
|
|
}
|
|
|
|
ap.at_surface = at_surface;
|
|
|
|
if(!ap.at_surface) {
|
|
surface_detector_count = 0;
|
|
Log_Write_Event(DATA_SURFACED);
|
|
gcs_send_text(MAV_SEVERITY_CRITICAL, "Off Surface");
|
|
} else {
|
|
Log_Write_Event(DATA_NOT_SURFACED);
|
|
gcs_send_text(MAV_SEVERITY_CRITICAL, "Surfaced");
|
|
}
|
|
}
|
|
|
|
void Sub::set_bottomed(bool at_bottom) {
|
|
if(ap.at_bottom == at_bottom) { // do nothing if state unchanged
|
|
return;
|
|
}
|
|
|
|
ap.at_bottom = at_bottom;
|
|
|
|
if(!ap.at_bottom) {
|
|
bottom_detector_count = 0;
|
|
Log_Write_Event(DATA_BOTTOMED);
|
|
gcs_send_text(MAV_SEVERITY_CRITICAL, "Off Bottom");
|
|
} else {
|
|
Log_Write_Event(DATA_NOT_BOTTOMED);
|
|
gcs_send_text(MAV_SEVERITY_CRITICAL, "Bottomed");
|
|
}
|
|
}
|