Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : // read_inertia - read inertia in from accelerometers
       4     2180683 : void Sub::read_inertia()
       5             : {
       6             :     // inertial altitude estimates
       7     2180683 :     sub.pos_control.update_estimates();
       8             : 
       9             :     // pull position from ahrs
      10     2180683 :     Location loc;
      11             :     // AHRS provides a best-guess in case of failure
      12     2180683 :     UNUSED_RESULT(ahrs.get_location(loc));
      13     2180683 :     current_loc.lat = loc.lat;
      14     2180683 :     current_loc.lng = loc.lng;
      15             : 
      16             :     // exit immediately if we do not have an altitude estimate
      17     2180683 :     if (!AP::ahrs().has_status(AP_AHRS::Status::VERT_POS)) {
      18      105338 :         return;
      19             :     }
      20             : 
      21     2075345 :     current_loc.alt = pos_control.get_pos_estimate_U_m() * 100.0f;
      22             : }