From b6a0237a632978d7177734abdf6248a025bcd2ab Mon Sep 17 00:00:00 2001 From: jaxxzer Date: Wed, 24 Feb 2016 17:18:48 -0500 Subject: [PATCH] Sub: Add surface and bottom detection capabilities --- ArduSub/ArduSub.cpp | 3 + ArduSub/Sub.h | 7 ++ ArduSub/control_althold.cpp | 10 ++- ArduSub/defines.h | 4 + ArduSub/surface_bottom_detector.cpp | 114 ++++++++++++++++++++++++++++ 5 files changed, 137 insertions(+), 1 deletion(-) create mode 100644 ArduSub/surface_bottom_detector.cpp diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 72c51f0b4e..e1cc8ed493 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -288,6 +288,9 @@ void Sub::fast_loop() // check if we've landed or crashed update_land_and_crash_detectors(); + // check if we've reached the surface or bottom + update_surface_and_bottom_detector(); + // camera mount's fast update camera_mount.update_fast(); diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index d30d5d5411..8c72bfad68 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -240,6 +240,9 @@ private: uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use uint8_t motor_emergency_stop: 1; // 21 // motor estop switch, shuts off motors when enabled uint8_t land_repo_active : 1; // 22 // true if the pilot is overriding the landing position + uint8_t at_bottom : 1; // true if we are at the bottom + uint8_t at_surface : 1; // true if we are at the surface + uint8_t depth_sensor_present: 1; // true if we have an external baro connected }; uint32_t value; } ap; @@ -390,6 +393,7 @@ private: int32_t baro_alt; // barometer altitude in cm above home float baro_climbrate; // barometer climbrate in cm/s LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests + LowPassFilterVector3f depth_accel_ef_filter; // accelerations for land and crash detector tests // 3D Location vectors // Current location of the Sub (altitude is relative to home) @@ -817,6 +821,9 @@ private: void update_land_and_crash_detectors(); void update_land_detector(); void update_throttle_thr_mix(); + void update_surface_and_bottom_detector(); + void set_surfaced(bool at_surface); + void set_bottomed(bool at_bottom); #if GNDEFFECT_COMPENSATION == ENABLED void update_ground_effect_detector(void); #endif // GNDEFFECT_COMPENSATION == ENABLED diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 324eef05e6..772d16ecef 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -121,7 +121,15 @@ void Sub::althold_run() } // call position controller - pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + if(ap.at_bottom) { + pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator + pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom + } else if(ap.at_surface) { + pos_control.relax_alt_hold_controllers(0.0); // clear velocity and position targets, and integrator + pos_control.set_alt_target(inertial_nav.get_altitude() - 10.0f); // set target to 10 cm below surface + } else { + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + } pos_control.update_z_controller(); break; } diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 4479699eab..0d3ce6ba74 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -333,6 +333,10 @@ enum FlipState { #define DATA_ROTOR_RUNUP_COMPLETE 58 // Heli only #define DATA_ROTOR_SPEED_BELOW_CRITICAL 59 // Heli only #define DATA_EKF_ALT_RESET 60 +#define DATA_SURFACED 61 // Sub only +#define DATA_NOT_SURFACED 62 // Sub only +#define DATA_BOTTOMED 63 // Sub only +#define DATA_NOT_BOTTOMED 64 // Sub only // Centi-degrees to radians #define DEGX100 5729.57795f diff --git a/ArduSub/surface_bottom_detector.cpp b/ArduSub/surface_bottom_detector.cpp new file mode 100644 index 0000000000..82bbc7b230 --- /dev/null +++ b/ArduSub/surface_bottom_detector.cpp @@ -0,0 +1,114 @@ +/// -*- 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.02 // 2cm 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"); + } +}