Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if AP_RANGEFINDER_ENABLED
       4             : 
       5             : // update_surface_offset - manages the vertical offset of the position controller to follow the measured ground or ceiling
       6             : //   level measured using the range finder.
       7     4459426 : void Copter::SurfaceTracking::update_surface_offset()
       8             : {
       9             :     // check for timeout
      10     4459426 :     const uint32_t now_ms = millis();
      11     4459426 :     const bool timeout = (now_ms - last_update_ms) > SURFACE_TRACKING_TIMEOUT_MS;
      12             : 
      13             :     // check tracking state and that range finders are healthy
      14     8577050 :     if (((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) ||
      15     4117624 :         ((surface == Surface::CEILING) && copter.rangefinder_up_ok() && (copter.rangefinder_up_state.glitch_count == 0))) {
      16             : 
      17             :         // Get the appropriate surface distance state, the terrain offset is calculated in the surface distance lib. 
      18      341802 :         AP_SurfaceDistance &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
      19             : 
      20             :         // update position controller target offset to the surface's alt above the EKF origin
      21      341802 :         copter.pos_control->set_pos_terrain_target_D_m(-rf_state.terrain_u_m);
      22      341802 :         last_update_ms = now_ms;
      23      341802 :         valid_for_logging = true;
      24             : 
      25             :         // reset target altitude if this controller has just been engaged
      26             :         // target has been changed between upwards vs downwards
      27             :         // or glitch has cleared
      28      341802 :         if (timeout ||
      29      341781 :             reset_target ||
      30      341781 :             (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) {
      31          23 :             copter.pos_control->init_pos_terrain_D_m(-rf_state.terrain_u_m);
      32          23 :             reset_target = false;
      33          23 :             last_glitch_cleared_ms = rf_state.glitch_cleared_ms;
      34             :         }
      35             : 
      36             :     } else {
      37             :         // reset position controller offsets if surface tracking is inactive
      38             :         // flag target should be reset when/if it next becomes active
      39     4117624 :         if (timeout && !reset_target) {
      40         170 :             copter.pos_control->init_pos_terrain_D_m(0);
      41         170 :             valid_for_logging = false;
      42         170 :             reset_target = true;
      43             :         }
      44             :     }
      45     4459426 : }
      46             : 
      47             : // target has already been set by terrain following so do not initialise again
      48             : // this should be called by flight modes when switching from terrain following to surface tracking (e.g. ZigZag)
      49           0 : void Copter::SurfaceTracking::external_init()
      50             : {
      51           0 :     if ((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) {
      52           0 :         last_update_ms = millis();
      53           0 :         reset_target = false;
      54             :     }
      55           0 : }
      56             : 
      57      728325 : bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist_m) const
      58             : {
      59      728325 :     if (!valid_for_logging || (surface == Surface::NONE)) {
      60      719773 :         return false;
      61             :     }
      62             : 
      63        8552 :     const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
      64        8552 :     target_dist_m = dir * copter.pos_control->get_pos_desired_U_m();
      65        8552 :     return true;
      66             : }
      67             : 
      68      728325 : float Copter::SurfaceTracking::get_dist_for_logging() const
      69             : {
      70      728325 :     return ((surface == Surface::CEILING) ? copter.rangefinder_up_state.alt_m : copter.rangefinder_state.alt_m);
      71             : }
      72             : 
      73             : // set direction
      74           0 : void Copter::SurfaceTracking::set_surface(Surface new_surface)
      75             : {
      76           0 :     if (surface == new_surface) {
      77           0 :         return;
      78             :     }
      79             :     // check we have a range finder in the correct direction
      80           0 :     if ((new_surface == Surface::GROUND) && !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
      81           0 :         copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no downward rangefinder");
      82           0 :         AP_Notify::events.user_mode_change_failed = 1;
      83           0 :         return;
      84             :     }
      85           0 :     if ((new_surface == Surface::CEILING) && !copter.rangefinder.has_orientation(ROTATION_PITCH_90)) {
      86           0 :         copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no upward rangefinder");
      87           0 :         AP_Notify::events.user_mode_change_failed = 1;
      88           0 :         return;
      89             :     }
      90           0 :     surface = new_surface;
      91           0 :     reset_target = true;
      92             : }
      93             : 
      94             : #endif  // AP_RANGEFINDER_ENABLED