watchrobot/ArduSub/surface_bottom_detector.cpp

// Jacob Walser: jacob@bluerobotics.com

#include "Sub.h"

// 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;

// 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
// ToDo: doesn't need to be called this fast
void Sub::update_surface_and_bottom_detector()
{
	static float last_alt = 0.0;//12.20
    if (!motors.armed()) { // only update when armed
        set_surfaced(false);
        set_bottomed(false);
		last_alt =  barometer.get_altitude();
        return;
    }

    Vector3f velocity;
    //ahrs.get_velocity_NED(velocity);//12.20
    float cur_alt = barometer.get_altitude(); // m
    
	float vel = AC_AttitudeControl::sqrt_controller(cur_alt-last_alt, pos_control._p_pos_z.kP(), pos_control._accel_z_cms/100, pos_control._dt);
	velocity.z = vel;//12.20
	static int f = 0;

	f++;
	if(f>600)
	{
	
		
		gcs().send_text(MAV_SEVERITY_INFO, " velocity.z %f %f %f ",cur_alt,last_alt,velocity.z);
		f=0;
	}

	static uint16_t i=0;
	uint16_t timecnt = (uint16_t)(0.25/MAIN_LOOP_SECONDS);
	i++;
	if (i>timecnt)
	{
		i = 0;
		last_alt = cur_alt;//12.19
	}

	


    // 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 && sensor_health.depth) { // we can use the external pressure sensor for a very accurate and current measure of our z axis position
        current_depth = barometer.get_altitude(); // cm


        if (ap.at_surface) {
            set_surfaced(current_depth > g.surface_depth/100.0 - 0.05); // add a 5cm buffer so it doesn't trigger too often
        } else {
            set_surfaced(current_depth > g.surface_depth/100.0); // If we are above surface depth, we are surfaced
        }


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

        } else {
            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);
    }
}

void Sub::set_surfaced(bool at_surface)
{


    if (ap.at_surface == at_surface) { // do nothing if state unchanged
        return;
    }

    ap.at_surface = at_surface;

    surface_detector_count = 0;

    if (ap.at_surface) {
        Log_Write_Event(DATA_SURFACED);
    } else {
        Log_Write_Event(DATA_NOT_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;

    bottom_detector_count = 0;

    if (ap.at_bottom) {
        Log_Write_Event(DATA_BOTTOMED);
    } else {
        Log_Write_Event(DATA_NOT_BOTTOMED);
    }
}