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