watchrobot/ArduSub/surface_bottom_detector.cpp

// Jacob Walser: jacob@bluerobotics.com

#include "Sub.h"
uint8_t maxthree(float x1,float x2,float x3);

// 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;
bool bottomdetect = FALSE;
bool setZ = TRUE;
// 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
uint8_t maxthree(float x1,float x2,float x3){
	
	if(fabsf(x1)>fabsf(x2) || fabsf(fabsf(x1)-fabsf(x2))<0.0000001){//x1>=x3
		if(fabsf(x1)>fabsf(x3) || fabsf(fabsf(x1)-fabsf(x3))<0.0000001){//x1>=x3
		
			return 1;
		}else{//x3>x1
			return 3;
		}
	}
	if(fabsf(x2)>fabsf(x1) || fabsf(fabsf(x2)-fabsf(x1))<0.0000001){//x2>=x1
		if(fabsf(x2)>fabsf(x3) || fabsf(fabsf(x2)-fabsf(x3))<0.0000001){//x2>=x3
		
			return 2;
		}else{//x3>=x2
			return 3;
		}
	}
	return 0;
	

}
void Sub::bottom_detectorgain(void){
	float velocity_z = pos_control.alt_rate.get()/100;// m/s
	bool vel_stationary = velocity_z > -0.05 && velocity_z < 0.05;
	Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
	 Vector3f Xbx = mat_body*Vector3f(1,0,0);
	 Vector3f Xby = mat_body*Vector3f(0,1,0);
	 Vector3f Xbz = mat_body*Vector3f(0,0,1);
	 float dot1=Xbx*Vector3f(0,0,1);
	 float dot2=Xby*Vector3f(0,0,1);
	 float dot3=Xbz*Vector3f(0,0,1);
	 const AP_Motors6DOF &motors6dof = AP::motors6dof();//6自由度电机计算出来的PWM
	 static uint16_t countx=0;
	 static uint16_t county=0;
	 static uint16_t countz=0;
	  Vector3f velocity;
	velocity.z = pos_control.alt_rate.get()/100;//12.20 cm

    // check that we are not moving up or down

	if (vel_stationary)//速度很小
	{
	 if(maxthree(dot1,dot2,dot3)==1) {//x轴
	   
	 	 float x1 = (float)(motors6dof.motor_to_detect[0] - NETRULPWM)/(NETRULPWM-1000) ;
		 float x2 = (float)(motors6dof.motor_to_detect[1] - NETRULPWM)/(NETRULPWM-1000) ;
		 if (fabsf(x1)>0.95 && fabsf(x2)>0.95)
		 {
		 	  countz=0;
	 		  county=0;
			if (countx < 2*MAIN_LOOP_RATE) {
                countx++;
            } else{
				bottom_set = TRUE;
			}
		   
		 }else{
			 bottom_set = FALSE;
			 countx = 0;
		 }
	 }
	 else if (maxthree(dot1,dot2,dot3)==2 ){//y轴
	  	 float l = (float)(motors6dof.motor_to_detect[5] - NETRULPWM)/(NETRULPWM-1000) ;
		 countx=0;
	 	 countz=0;
		 if (fabsf(l)>0.9 )
		 {
		 	if (county < 2*MAIN_LOOP_RATE) {
                county++;
            } else{
				bottom_set = TRUE;
			}
		   
		 }else{
		 	 bottom_set = FALSE;
			 county = 0;
		 }
	 }
	 else if (maxthree(dot1,dot2,dot3)==3 ){//z轴
	 	countx=0;
	 	 county=0;
	 	 float z1 = (float)(motors6dof.motor_to_detect[2] - NETRULPWM)/(NETRULPWM-1000) ;
		 float z2 = (float)(motors6dof.motor_to_detect[3] - NETRULPWM)/(NETRULPWM-1000) ;
	 	 //float z3 = (float)(motors6dof.motor_to_detect[4] - NETRULPWM)/(NETRULPWM-1000) ;
		 if(fabsf(z1)>0.95 && fabsf(z2)>0.95){
			 if (countz < 2*MAIN_LOOP_RATE) {
					countz++;
			} else{
					bottom_set = TRUE;
			}
		 }else{
		 	 bottom_set = FALSE;
			 countz = 0;
		 }

		 
	 }else{
		
	 	 countx=0;
	     county=0;
	     countz=0;
		 bottom_set = FALSE; //这里忽略了三个或者2个相等的情况
	 }
	}
	 else{
		 countx=0;
	     county=0;
	     countz=0;
		 bottom_set = FALSE; //速度比较大
	 	}
	
	 

}

float Sub::surface_depth_use(void){

	Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
	float surface_depth_use;
	if (abs(mat_body.c.x) <0.2)
		{
		surface_depth_use = g.surface_depth/100.0;
		}else if(fabsf(mat_body.c.x) <0.45){
			surface_depth_use = -0.2;//m
		}else if(fabsf(mat_body.c.x) <0.7){
			surface_depth_use = -0.3;//m
		}else{
			surface_depth_use = -0.45;//m
		}
		return surface_depth_use;

}
void Sub::update_surface_and_bottom_detector()
{
	
    if (!motors.armed()) { // only update when armed
        set_surfaced(false);
        set_bottomed(false);
		
        return;
    }

    Vector3f velocity;
	//velocity.z = pos_control.alt_rate.get()/100;//12.20 cm
	velocity.z = velocity_z_filer/100;//03.24 cm

    // 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 > surface_depth_use() - 0.05); // add a 5cm buffer so it doesn't trigger too often
        } else {
            set_surfaced(current_depth > surface_depth_use()); // 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);//03.02
            }

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

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