Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : /*
       4             :  * SURFTRAK (surface tracking) -- a variation on ALT_HOLD (depth hold)
       5             :  *
       6             :  * SURFTRAK starts in the "reset" state (rangefinder_target_cm < 0). SURFTRAK exits the reset state when these
       7             :  * conditions are met:
       8             :  * -- There is a good rangefinder reading (the rangefinder is healthy, the reading is between min and max, etc.)
       9             :  * -- The sub is below SURFTRAK_DEPTH
      10             :  *
      11             :  * During normal operation, SURFTRAK sets the offset target to the current terrain altitude estimate and calls
      12             :  * AC_PosControl to do the rest.
      13             :  *
      14             :  * We generally do not want to reset SURFTRAK if the rangefinder glitches, since that will result in a new rangefinder
      15             :  * target. E.g., if a pilot is running 1m above the seafloor, there is a glitch, and the next rangefinder reading shows
      16             :  * 1.1m, the desired behavior is to move 10cm closer to the seafloor, vs setting a new target of 1.1m above the
      17             :  * seafloor.
      18             :  *
      19             :  * If the pilot takes control, SURFTRAK uses the change in depth readings to adjust the rangefinder target. This
      20             :  * minimizes the "bounce back" that can happen as the slower rangefinder catches up to the quicker barometer.
      21             :  */
      22             : 
      23             : #define INVALID_TARGET (-1)
      24             : #define HAS_VALID_TARGET (rangefinder_target_cm > 0)
      25             : 
      26          83 : ModeSurftrak::ModeSurftrak() :
      27          83 :         rangefinder_target_cm(INVALID_TARGET),
      28          83 :         pilot_in_control(false),
      29          83 :         pilot_control_start_z_cm(0)
      30          83 : { }
      31             : 
      32           4 : bool ModeSurftrak::init(bool ignore_checks)
      33             : {
      34           4 :     if (!ModeAlthold::init(ignore_checks)) {
      35           0 :         return false;
      36             :     }
      37             : 
      38           4 :     reset();
      39             : 
      40           4 :     if (!sub.rangefinder_alt_ok()) {
      41           3 :         sub.gcs().send_text(MAV_SEVERITY_INFO, "waiting for a rangefinder reading");
      42             : #if AP_RANGEFINDER_ENABLED
      43           1 :     } else if (position_control->get_pos_estimate_U_m() * 100.0f >= sub.g.surftrak_depth) {
      44           0 :         sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to hold range", sub.g.surftrak_depth * 0.01f);
      45             : #endif
      46             :     }
      47             : 
      48           4 :     return true;
      49             : }
      50             : 
      51       99757 : void ModeSurftrak::run()
      52             : {
      53       99757 :     run_pre();
      54             : 
      55       99757 :     if (!motors.armed()) {
      56             :         // Forget rangefinder target
      57        4767 :         reset();
      58             :     } else {
      59       94990 :         control_range();
      60             :     }
      61             : 
      62       99757 :     run_post();
      63       99757 : }
      64             : 
      65             : /*
      66             :  * Set the rangefinder target, return true if successful. This may be called from scripting so run a few extra checks.
      67             :  */
      68          38 : bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
      69             : {
      70          38 :     bool success = false;
      71             : 
      72             : #if AP_RANGEFINDER_ENABLED
      73          38 :     if (sub.control_mode != Number::SURFTRAK) {
      74           0 :         sub.gcs().send_text(MAV_SEVERITY_WARNING, "wrong mode, rangefinder target not set");
      75          38 :     } else if (position_control->get_pos_estimate_U_m() * 100.0f >= sub.g.surftrak_depth) {
      76           0 :         sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to set rangefinder target", sub.g.surftrak_depth * 0.01f);
      77          38 :     } else if (target_cm < sub.rangefinder_state.min*100) {
      78           0 :         sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target below minimum, ignored");
      79          38 :     } else if (target_cm > sub.rangefinder_state.max*100) {
      80           0 :         sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target above maximum, ignored");
      81             :     } else {
      82          38 :         success = true;
      83             :     }
      84             : 
      85          38 :     if (success) {
      86          38 :         rangefinder_target_cm = target_cm;
      87          38 :         sub.gcs().send_text(MAV_SEVERITY_INFO, "rangefinder target is %.2f meters", rangefinder_target_cm * 0.01f);
      88             : 
      89             :         // Initialize the terrain offset
      90          38 :         auto terrain_offset_cm = position_control->get_pos_estimate_U_m() * 100.0f - rangefinder_target_cm;
      91          38 :         sub.pos_control.init_pos_terrain_U_cm(terrain_offset_cm);
      92             : 
      93             :     } else {
      94           0 :         reset();
      95             :     }
      96             : #endif
      97             : 
      98          38 :     return success;
      99             : }
     100             : 
     101        4771 : void ModeSurftrak::reset()
     102             : {
     103        4771 :     rangefinder_target_cm = INVALID_TARGET;
     104             : 
     105             :     // Reset the terrain offset
     106        4771 :     sub.pos_control.init_pos_terrain_U_cm(0);
     107        4771 : }
     108             : 
     109             : /*
     110             :  * Main controller, call at 100hz+
     111             :  */
     112       94990 : void ModeSurftrak::control_range() {
     113       94990 :     float target_climb_rate_cms = sub.get_pilot_desired_climb_rate(channel_throttle->get_control_in());
     114       94990 :     target_climb_rate_cms = constrain_float(target_climb_rate_cms, -sub.get_pilot_speed_dn(), g.pilot_speed_up);
     115             : 
     116             :     // Desired_climb_rate returns 0 when within the deadzone
     117       94990 :     if (fabsf(target_climb_rate_cms) < 0.05f)  {
     118       84436 :         if (pilot_in_control) {
     119             :             // Pilot has released control; apply the delta to the rangefinder target
     120           1 :             set_rangefinder_target_cm(rangefinder_target_cm + position_control->get_pos_estimate_U_m() * 100.0f - pilot_control_start_z_cm);
     121           1 :             pilot_in_control = false;
     122             :         }
     123       84436 :         if (sub.ap.at_surface) {
     124             :             // Set target depth to 5 cm below SURFACE_DEPTH and reset
     125           0 :             position_control->set_pos_desired_U_cm(MIN(position_control->get_pos_desired_U_cm(), g.surface_depth - 5.0f));
     126           0 :             reset();
     127       84436 :         } else if (sub.ap.at_bottom) {
     128             :             // Set target depth to 10 cm above bottom and reset
     129           0 :             position_control->set_pos_desired_U_cm(MAX(position_control->get_pos_estimate_U_m() * 100.0f + 10.0f, position_control->get_pos_desired_U_cm()));
     130           0 :             reset();
     131             :         } else {
     132             :             // Typical operation
     133       84436 :             update_surface_offset();
     134             :         }
     135       10554 :     } else if (HAS_VALID_TARGET && !pilot_in_control) {
     136             :         // Pilot has taken control; note the current depth
     137           1 :         pilot_control_start_z_cm = position_control->get_pos_estimate_U_m() * 100.0f;
     138           1 :         pilot_in_control = true;
     139             :     }
     140             : 
     141             :     // Set the target altitude from the climb rate and the terrain offset
     142       94990 :     position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate_cms);
     143             : 
     144             :     // Run the PID controllers
     145       94990 :     position_control->D_update_controller();
     146       94990 : }
     147             : 
     148             : /*
     149             :  * Update the AC_PosControl terrain offset if we have a good rangefinder reading
     150             :  */
     151       84436 : void ModeSurftrak::update_surface_offset()
     152             : {
     153             : #if AP_RANGEFINDER_ENABLED
     154       84436 :     if (sub.rangefinder_alt_ok()) {
     155             :         // Get the latest terrain offset
     156       79725 :         float rangefinder_terrain_offset_cm = sub.rangefinder_state.rangefinder_terrain_offset_cm;
     157             : 
     158             :         // Handle the first reading or a reset
     159       79725 :         if (!HAS_VALID_TARGET && sub.rangefinder_state.inertial_alt_cm < sub.g.surftrak_depth) {
     160           3 :             set_rangefinder_target_cm(sub.rangefinder_state.inertial_alt_cm - rangefinder_terrain_offset_cm);
     161             :         }
     162             : 
     163       79725 :         if (HAS_VALID_TARGET) {
     164             :             // Will the new offset target cause the sub to ascend above SURFTRAK_DEPTH?
     165       79725 :             float desired_z_cm = rangefinder_terrain_offset_cm + rangefinder_target_cm;
     166       79725 :             if (desired_z_cm >= sub.g.surftrak_depth) {
     167             :                 // Adjust the terrain offset to stay below SURFTRAK_DEPTH, this should avoid "at_surface" events
     168           0 :                 rangefinder_terrain_offset_cm += sub.g.surftrak_depth - desired_z_cm;
     169             :             }
     170             : 
     171             :             // Set the offset target, AC_PosControl will do the rest
     172       79725 :             sub.pos_control.set_pos_terrain_target_U_cm(rangefinder_terrain_offset_cm);
     173             :         }
     174             :     }
     175             : #endif  // AP_RANGEFINDER_ENABLED
     176       84436 : }