Line data    Source code

       1             : // Jacob Walser: [email protected]
       2             : 
       3             : #include "Sub.h"
       4             : 
       5             : // counter to verify contact with bottom
       6             : static uint32_t bottom_detector_count = 0;
       7             : static uint32_t surface_detector_count = 0;
       8             : static float current_depth = 0;
       9             : 
      10             : // checks if we have hit bottom or surface and updates the ap.at_bottom and ap.at_surface flags
      11             : // called at MAIN_LOOP_RATE
      12             : // ToDo: doesn't need to be called this fast
      13     2180683 : void Sub::update_surface_and_bottom_detector()
      14             : {
      15     2180683 :     if (!motors.armed()) { // only update when armed
      16      944196 :         set_surfaced(false);
      17      944196 :         set_bottomed(false);
      18      944196 :         return;
      19             :     }
      20             : 
      21     1236487 :     Vector3f velocity;
      22     1236487 :     UNUSED_RESULT(ahrs.get_velocity_NED(velocity));
      23             : 
      24             :     // check that we are not moving up or down
      25     1236487 :     bool vel_stationary = velocity.z > -0.05 && velocity.z < 0.05;
      26             : 
      27     1236487 :     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
      28     1214771 :         current_depth = barometer.get_altitude(); // cm
      29             : 
      30             : 
      31     1214771 :         if (ap.at_surface) {
      32      144416 :             set_surfaced(current_depth > g.surface_depth*0.01 - 0.05); // add a 5cm buffer so it doesn't trigger too often
      33             :         } else {
      34     1070355 :             set_surfaced(current_depth > g.surface_depth*0.01); // If we are above surface depth, we are surfaced
      35             :         }
      36             : 
      37             : 
      38     1214771 :         if (motors.limit.throttle_lower && vel_stationary) {
      39             :             // bottom criteria met - increment the counter and check if we've triggered
      40        6375 :             if (bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
      41        6372 :                 bottom_detector_count++;
      42             :             } else {
      43           3 :                 set_bottomed(true);
      44             :             }
      45             : 
      46             :         } else {
      47     1208396 :             set_bottomed(false);
      48             :         }
      49             : 
      50             :         // with no external baro, the only thing we have to go by is a vertical velocity estimate
      51       21716 :     } else if (vel_stationary) {
      52         650 :         if (motors.limit.throttle_upper) {
      53             : 
      54             :             // surface criteria met, increment counter and see if we've triggered
      55           0 :             if (surface_detector_count < ((float)SURFACE_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
      56           0 :                 surface_detector_count++;
      57             :             } else {
      58           0 :                 set_surfaced(true);
      59             :             }
      60             : 
      61         650 :         } else if (motors.limit.throttle_lower) {
      62             :             // bottom criteria met, increment counter and see if we've triggered
      63           0 :             if (bottom_detector_count < ((float)BOTTOM_DETECTOR_TRIGGER_SEC)*MAIN_LOOP_RATE) {
      64           0 :                 bottom_detector_count++;
      65             :             } else {
      66           0 :                 set_bottomed(true);
      67             :             }
      68             : 
      69             :         } else { // we're not at the limits of throttle, so reset both detectors
      70         650 :             set_surfaced(false);
      71         650 :             set_bottomed(false);
      72             :         }
      73             : 
      74             :     } else { // we're moving up or down, so reset both detectors
      75       21066 :         set_surfaced(false);
      76       21066 :         set_bottomed(false);
      77             :     }
      78             : }
      79             : 
      80     2180683 : void Sub::set_surfaced(bool at_surface)
      81             : {
      82             : 
      83             : 
      84     2180683 :     if (ap.at_surface == at_surface) { // do nothing if state unchanged
      85     2180613 :         return;
      86             :     }
      87             : 
      88          70 :     ap.at_surface = at_surface;
      89             : 
      90          70 :     surface_detector_count = 0;
      91             : 
      92          70 :     if (ap.at_surface) {
      93          35 :         LOGGER_WRITE_EVENT(LogEvent::SURFACED);
      94             :     } else {
      95          35 :         LOGGER_WRITE_EVENT(LogEvent::NOT_SURFACED);
      96             :     }
      97             : }
      98             : 
      99     2174311 : void Sub::set_bottomed(bool at_bottom)
     100             : {
     101             : 
     102     2174311 :     if (ap.at_bottom == at_bottom) { // do nothing if state unchanged
     103     2174305 :         return;
     104             :     }
     105             : 
     106           6 :     ap.at_bottom = at_bottom;
     107             : 
     108           6 :     bottom_detector_count = 0;
     109             : 
     110           6 :     if (ap.at_bottom) {
     111           3 :         LOGGER_WRITE_EVENT(LogEvent::BOTTOMED);
     112             :     } else {
     113           3 :         LOGGER_WRITE_EVENT(LogEvent::NOT_BOTTOMED);
     114             :     }
     115             : }