ArduCopter/surface_tracking.cpp

   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 void Copter::SurfaceTracking::update_surface_offset()
   8 {
   9     // check for timeout
  10     const uint32_t now_ms = millis();
  11     const bool timeout = (now_ms - last_update_ms) > SURFACE_TRACKING_TIMEOUT_MS;
  12
  13     // check tracking state and that range finders are healthy
  14     if (((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) ||
  15         ((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         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         copter.pos_control->set_pos_terrain_target_cm(rf_state.terrain_offset_cm);
  22         last_update_ms = now_ms;
  23         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         if (timeout ||
  29             reset_target ||
  30             (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) {
  31             copter.pos_control->init_pos_terrain_cm(rf_state.terrain_offset_cm);
  32             reset_target = false;
  33             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         if (timeout && !reset_target) {
  40             copter.pos_control->init_pos_terrain_cm(0);
  41             valid_for_logging = false;
  42             reset_target = true;
  43         }
  44     }
  45 }
  46
  47 // target has already been set by terrain following so do not initalise again
  48 // this should be called by flight modes when switching from terrain following to surface tracking (e.g. ZigZag)
  49 void Copter::SurfaceTracking::external_init()
  50 {
  51     if ((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) {
  52         last_update_ms = millis();
  53         reset_target = false;
  54     }
  55 }
  56
  57 bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) const
  58 {
  59     if (!valid_for_logging || (surface == Surface::NONE)) {
  60         return false;
  61     }
  62
  63     const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
  64     target_dist = dir * copter.pos_control->get_pos_desired_z_cm() * 0.01f;
  65     return true;
  66 }
  67
  68 float Copter::SurfaceTracking::get_dist_for_logging() const
  69 {
  70     return ((surface == Surface::CEILING) ? copter.rangefinder_up_state.alt_cm : copter.rangefinder_state.alt_cm) * 0.01f;
  71 }
  72
  73 // set direction
  74 void Copter::SurfaceTracking::set_surface(Surface new_surface)
  75 {
  76     if (surface == new_surface) {
  77         return;
  78     }
  79     // check we have a range finder in the correct direction
  80     if ((new_surface == Surface::GROUND) && !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
  81         copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no downward rangefinder");
  82         AP_Notify::events.user_mode_change_failed = 1;
  83         return;
  84     }
  85     if ((new_surface == Surface::CEILING) && !copter.rangefinder.has_orientation(ROTATION_PITCH_90)) {
  86         copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no upward rangefinder");
  87         AP_Notify::events.user_mode_change_failed = 1;
  88         return;
  89     }
  90     surface = new_surface;
  91     reset_target = true;
  92 }
  93
  94 #endif  // AP_RANGEFINDER_ENABLED