Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : // read_inertia - read inertia in from accelerometers
       4    30412448 : void Copter::read_inertia()
       5             : {
       6             :     // inertial altitude estimates. Use barometer climb rate during high vibrations
       7    30412448 :     pos_control->update_estimates(vibration_check.high_vibes);
       8             : #if MODE_FOLLOW_ENABLED
       9    30412448 :     g2.follow.update_estimates();
      10             : #endif
      11             : 
      12             :     // pull position from ahrs
      13    30412448 :     Location loc;
      14             :     // AHRS provides a best-guess in case of failure
      15    30412448 :     UNUSED_RESULT(ahrs.get_location(loc));
      16    30412448 :     current_loc.lat = loc.lat;
      17    30412448 :     current_loc.lng = loc.lng;
      18             : 
      19             :     // exit immediately if we do not have an altitude estimate
      20             :     float pos_d_m;
      21    30412448 :     if (!AP::ahrs().get_relative_position_D_origin_float(pos_d_m)) {
      22     1677408 :         return;
      23             :     }
      24             : 
      25             :     // current_loc.alt is alt-above-home, converted from AHRS's alt-above-ekf-origin
      26    29180050 :     const float alt_above_origin_m = -pos_d_m;
      27    29180050 :     current_loc.set_alt_m(alt_above_origin_m, Location::AltFrame::ABOVE_ORIGIN);
      28             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      29    29180050 :     if (!current_loc.initialised()) {
      30             :         // this can happen if we don't have lat/lng and our altitude
      31             :         // is *very* close the origin i.e.  lat=0, lng=0 and
      32             :         // int32_t(abs(pos_d_m)*100)==0
      33      445010 :         return;
      34             :     }
      35             : #endif  // CONFIG_HAL_BOARD == HAL_BOARD_SITL
      36             : 
      37    28735040 :     if (!ahrs.home_is_set() || !current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
      38             :         // if home has not been set yet we treat alt-above-origin as alt-above-home
      39     2183342 :         current_loc.set_alt_m(alt_above_origin_m, Location::AltFrame::ABOVE_HOME);
      40             :     }
      41             : }