Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : // return barometric altitude in centimeters
       4      759512 : void Copter::read_barometer(void)
       5             : {
       6      759512 :     barometer.update();
       7             : 
       8      759512 :     baro_alt_m = barometer.get_altitude();
       9      759512 : }
      10             : 
      11             : #if AP_RANGEFINDER_ENABLED
      12         998 : void Copter::init_rangefinder(void)
      13             : {
      14         998 :    rangefinder.set_log_rfnd_bit(MASK_LOG_CTUN);
      15         998 :    rangefinder.init(ROTATION_PITCH_270);
      16         998 :    rangefinder_state.alt_m_filt.set_cutoff_frequency(g2.rangefinder_filt);
      17         998 :    rangefinder_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_270);
      18             : 
      19             :    // upward facing range finder
      20         998 :    rangefinder_up_state.alt_m_filt.set_cutoff_frequency(g2.rangefinder_filt);
      21         998 :    rangefinder_up_state.enabled = rangefinder.has_orientation(ROTATION_PITCH_90);
      22         998 : }
      23             : 
      24             : // return rangefinder altitude in centimeters
      25     1519878 : void Copter::read_rangefinder(void)
      26             : {
      27     1519878 :     rangefinder.update();
      28             : 
      29     1519878 :     rangefinder_state.update();
      30     1519878 :     rangefinder_up_state.update();
      31             : 
      32             : #if HAL_PROXIMITY_ENABLED
      33     1519878 :     if (rangefinder_state.enabled_and_healthy() || rangefinder_state.data_stale()) {
      34     1518828 :         g2.proximity.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_m_filt.get() * 100.0);
      35             :     }
      36             : #endif
      37     1519878 : }
      38             : #endif  // AP_RANGEFINDER_ENABLED
      39             : 
      40             : // return true if rangefinder_alt can be used
      41    53274204 : bool Copter::rangefinder_alt_ok() const
      42             : {
      43    53274204 :     return rangefinder_state.enabled_and_healthy();
      44             : }
      45             : 
      46             : // return true if rangefinder_alt can be used
      47           0 : bool Copter::rangefinder_up_ok() const
      48             : {
      49           0 :     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    30412448 : void Copter::update_rangefinder_terrain_offset()
      55             : {
      56    30412448 :     float terrain_u_m = rangefinder_state.ref_pos_u_m - rangefinder_state.alt_glitch_protected_m;
      57    30412448 :     rangefinder_state.terrain_u_m += (terrain_u_m - rangefinder_state.terrain_u_m) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt));
      58             : 
      59    30412448 :     terrain_u_m = rangefinder_up_state.ref_pos_u_m + rangefinder_up_state.alt_glitch_protected_m;
      60    30412448 :     rangefinder_up_state.terrain_u_m += (terrain_u_m - rangefinder_up_state.terrain_u_m) * (copter.G_Dt / MAX(copter.g2.surftrak_tc, copter.G_Dt));
      61             : 
      62    30412448 :     if (rangefinder_state.alt_healthy || rangefinder_state.data_stale()) {
      63    30392688 :         wp_nav->set_rangefinder_terrain_U_m(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.terrain_u_m);
      64             : #if MODE_CIRCLE_ENABLED
      65    30392688 :         circle_nav->set_rangefinder_terrain_U_m(rangefinder_state.enabled && wp_nav->rangefinder_used(), rangefinder_state.alt_healthy, rangefinder_state.terrain_u_m);
      66             : #endif
      67             :     }
      68    30412448 : }
      69             : 
      70             : // helper function to get inertially interpolated rangefinder height.
      71     9367196 : bool Copter::get_rangefinder_height_interpolated_m(float& height_m) const
      72             : {
      73             : #if AP_RANGEFINDER_ENABLED
      74     9367196 :     return rangefinder_state.get_rangefinder_height_interpolated_m(height_m);
      75             : #else
      76             :     return false;
      77             : #endif
      78             : }