ArduCopter/sensors.cpp

   1 #include "Copter.h"
   2
   3 // return barometric altitude in centimeters
   4 void Copter::read_barometer(void)
   5 {
   6     barometer.update();
   7
   8     baro_alt = barometer.get_altitude() * 100.0f;
   9 }
  10
  11 #if AP_RANGEFINDER_ENABLED
  12 void Copter::init_rangefinder(void)
  13 {
  14    rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
  15    rangefinder.init(ROTATION_PITCH_270);
  16    rangefinder_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt);
  17    rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
  18
  19    // upward facing range finder
  20    rangefinder_up_state.alt_cm_filt.set_cutoff_frequency(g2.rangefinder_filt);
  21    rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90);
  22 }
  23
  24 // return rangefinder altitude in centimeters
  25 void Copter::read_rangefinder(void)
  26 {
  27     rangefinder.update();
  28
  29     rangefinder_state.update();
  30     rangefinder_up_state.update();
  31
  32 #if HAL_PROXIMITY_ENABLED
  33     if (rangefinder_state.enabled_and_healthy() || rangefinder_state.data_stale()) {
  34         g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
  35     }
  36 #endif
  37 }
  38 #endif  // AP_RANGEFINDER_ENABLED
  39
  40 // return true if rangefinder_alt can be used
  41 bool Copter::rangefinder_alt_ok() const
  42 {
  43     return rangefinder_state.enabled_and_healthy();
  44 }
  45
  46 // return true if rangefinder_alt can be used
  47 bool Copter::rangefinder_up_ok() const
  48 {
  49     return rangefinder_up_state.enabled_and_healthy();
  50 }
  51
  52 // update rangefinder based terrain offset
  53 // terrain offset is the terrain's height above the EKF origin
  54 void Copter::update_rangefinder_terrain_offset()
  55 {
  56     float terrain_offset_cm = rangefinder_state.inertial_alt_cm - rangefinder_state.alt_cm_glitch_protected;
  57     rangefinder_state.terrain_offset_cm += (terrain_offset_cm - rangefinder_state.terrain_offset_cm) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt));
  58
  59     terrain_offset_cm = rangefinder_up_state.inertial_alt_cm + rangefinder_up_state.alt_cm_glitch_protected;
  60     rangefinder_up_state.terrain_offset_cm += (terrain_offset_cm - rangefinder_up_state.terrain_offset_cm) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt));
  61
  62     if (rangefinder_state.alt_healthy || rangefinder_state.data_stale()) {
  63         wp_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm);
  64 #if MODE_CIRCLE_ENABLED
  65         circle_nav->set_rangefinder_terrain_offset(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.terrain_offset_cm);
  66 #endif
  67     }
  68 }
  69
  70 // helper function to get inertially interpolated rangefinder height.
  71 bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret) const
  72 {
  73 #if AP_RANGEFINDER_ENABLED
  74     return rangefinder_state.get_rangefinder_height_interpolated_cm(ret);
  75 #else
  76     return false;
  77 #endif
  78 }