Line data    Source code

       1             : #include "Tracker.h"
       2             : 
       3             : /*
       4             :   update INS and attitude
       5             :  */
       6       86527 : void Tracker::update_ahrs()
       7             : {
       8       86527 :     ahrs.update();
       9       86527 : }
      10             : 
      11             : /*
      12             :   read and update compass
      13             :  */
      14       17294 : void Tracker::update_compass(void)
      15             : {
      16       17294 :     compass.read();
      17       17294 : }
      18             : 
      19             : /*
      20             :   read the GPS
      21             :  */
      22       17294 : void Tracker::update_GPS(void)
      23             : {
      24       17294 :     gps.update();
      25             : 
      26             :     static uint32_t last_gps_msg_ms;
      27             :     static uint8_t ground_start_count = 5;
      28       24610 :     if (gps.last_message_time_ms() != last_gps_msg_ms && 
      29        7316 :         gps.status() >= AP_GPS_FixType::FIX_3D) {
      30        7261 :         last_gps_msg_ms = gps.last_message_time_ms();
      31             :         
      32        7261 :         if (ground_start_count > 1) {
      33         108 :             ground_start_count--;
      34        7153 :         } else if (ground_start_count == 1) {
      35             :             // We countdown N number of good GPS fixes
      36             :             // so that the altitude is more accurate
      37             :             // -------------------------------------
      38          27 :             if (current_loc.lat == 0 && current_loc.lng == 0) {
      39           0 :                 ground_start_count = 5;
      40             : 
      41             :             } else {
      42             :                 // Now have an initial GPS position
      43             :                 // use it as the HOME position in future startups
      44          27 :                 current_loc = gps.location();
      45          27 :                 IGNORE_RETURN(set_home(current_loc, false));
      46          27 :                 ground_start_count = 0;
      47             :             }
      48             :         }
      49             :     }
      50       17294 : }
      51             : 
      52           0 : void Tracker::handle_battery_failsafe(const char* type_str, const int8_t action)
      53             : {
      54             :     // NOP
      55             :     // useful failsafes in the future would include actually recalling the vehicle
      56             :     // that is tracked before the tracker loses power to continue tracking it
      57           0 : }