AntennaTracker/sensors.cpp

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