Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: update the surface/bottom detector

Browse Source
This commit is contained in:
jaxxzer 2016-02-28 20:16:10 -05:00 committed by Andrew Tridgell
parent 8ff479ca1e
commit 58d1e18614
1 changed files with 38 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
ArduSub/surface_bottom_detector.cpp
View File

@ -3,11 +3,6 @@
#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;
@ -18,66 +13,57 @@ static float start_depth = 0; // the depth when we first hit downward throttle l
// 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);
Vector3f velocity;
ahrs.get_velocity_NED(velocity);
// 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);
// check that we are not moving up or down
bool vel_stationary = velocity.z > -0.05 && velocity.z < 0.05;
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(motors.limit.throttle_lower && vel_stationary) {
// bottom 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);
}
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
set_bottomed(false);
}
// with no external baro, the only thing we have to go by is a vertical velocity estimate
} else if (vel_stationary) {
if(motors.limit.throttle_upper) {
// surface criteria met, increment counter and see if we've triggered
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) {
// bottom criteria met, increment counter and see if we've triggered
if( bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
bottom_detector_count++;
} else {
set_bottomed(true);
}
} else { // we're not at the limits of throttle, so reset both detectors
set_surfaced(false);
set_bottomed(false);
}
} else { // we're moving up or down, so reset both detectors
set_surfaced(false);
set_bottomed(false);
}
// 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) {