AP_GPS: use millis/micros/panic functions

libraries/AP_GPS/AP_GPS.cpp

@@ -184,7 +184,7 @@ AP_GPS::detect_instance(uint8_t instance)
 {
     AP_GPS_Backend *new_gps = NULL;
     struct detect_state *dstate = &detect_state[instance];
-    uint32_t now = hal.scheduler->millis();
+    uint32_t now = AP_HAL::millis();
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
     if (_type[instance] == GPS_TYPE_PX4) {
@@ -347,7 +347,7 @@ AP_GPS::update_instance(uint8_t instance)
 
     // we have an active driver for this instance
     bool result = drivers[instance]->read();
-    uint32_t tnow = hal.scheduler->millis();
+    uint32_t tnow = AP_HAL::millis();
 
     // if we did not get a message, and the idle timer of 1.2 seconds
     // has expired, re-initialise the GPS. This will cause GPS
@@ -401,7 +401,7 @@ AP_GPS::update(void)
 
             if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) {
 
-                uint32_t now = hal.scheduler->millis();
+                uint32_t now = AP_HAL::millis();
                 bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2);
 
                 if ( (another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) ||
@@ -435,7 +435,7 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms,
     if (instance >= GPS_MAX_INSTANCES) {
         return;
     }
-    uint32_t tnow = hal.scheduler->millis();
+    uint32_t tnow = AP_HAL::millis();
     GPS_State &istate = state[instance];
     istate.status = _status;
     istate.location = _location;
@@ -507,7 +507,7 @@ AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
         last_send_time_ms[chan] = last_message_time_ms(0);
     } else {
         // when we don't have a GPS then send at 1Hz
-        uint32_t now = hal.scheduler->millis();
+        uint32_t now = AP_HAL::millis();
         if (now - last_send_time_ms[chan] < 1000) {
             return;
         }

libraries/AP_GPS/AP_GPS_GSOF.cpp

@@ -50,7 +50,7 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
     // baud request for port 3
     requestBaud(3);
 
-    uint32_t now = hal.scheduler->millis();
+    uint32_t now = AP_HAL::millis();
     gsofmsg_time = now + 110;
 }
 
@@ -59,7 +59,7 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
 bool
 AP_GPS_GSOF::read(void)
 {
-    uint32_t now = hal.scheduler->millis();
+    uint32_t now = AP_HAL::millis();
 
     if (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
         if (now > gsofmsg_time) {
@@ -349,7 +349,7 @@ AP_GPS_GSOF::inject_data(uint8_t *data, uint8_t len)
 {
 
     if (port->txspace() > len) {
-        last_injected_data_ms = hal.scheduler->millis();
+        last_injected_data_ms = AP_HAL::millis();
         port->write(data, len);
     } else {
         Debug("GSOF: Not enough TXSPACE");

libraries/AP_GPS/AP_GPS_MTK19.cpp

@@ -159,7 +159,7 @@ restart:
                                         (unsigned)_mtk_revision);                                        
 #endif
                     make_gps_time(_buffer.msg.utc_date, bcd_time_ms);
-                    state.last_gps_time_ms = hal.scheduler->millis();
+                    state.last_gps_time_ms = AP_HAL::millis();
                 }
                 // the _fix_counter is to reduce the cost of the GPS
                 // BCD time conversion by only doing it every 10s

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -261,7 +261,7 @@ bool AP_GPS_NMEA::_have_new_message()
         _last_GPGGA_ms == 0) {
         return false;
     }
-    uint32_t now = hal.scheduler->millis();
+    uint32_t now = AP_HAL::millis();
     if (now - _last_GPRMC_ms > 150 ||
         now - _last_GPGGA_ms > 150) {
         return false;
@@ -297,7 +297,7 @@ bool AP_GPS_NMEA::_term_complete()
                     state.ground_speed     = _new_speed*0.01f;
                     state.ground_course_cd = wrap_360_cd(_new_course);
                     make_gps_time(_new_date, _new_time * 10);
-                    state.last_gps_time_ms = hal.scheduler->millis();
+                    state.last_gps_time_ms = AP_HAL::millis();
                     // To-Do: add support for proper reporting of 2D and 3D fix
                     state.status           = AP_GPS::GPS_OK_FIX_3D;
                     fill_3d_velocity();
@@ -337,15 +337,15 @@ bool AP_GPS_NMEA::_term_complete()
     if (_term_number == 0) {
         if (!strcmp(_term, _gprmc_string)) {
             _sentence_type = _GPS_SENTENCE_GPRMC;
-            _last_GPRMC_ms = hal.scheduler->millis();
+            _last_GPRMC_ms = AP_HAL::millis();
         } else if (!strcmp(_term, _gpgga_string)) {
             _sentence_type = _GPS_SENTENCE_GPGGA;
-            _last_GPGGA_ms = hal.scheduler->millis();
+            _last_GPGGA_ms = AP_HAL::millis();
         } else if (!strcmp(_term, _gpvtg_string)) {
             _sentence_type = _GPS_SENTENCE_GPVTG;
             // VTG may not contain a data qualifier, presume the solution is good
             // unless it tells us otherwise.
-            _last_GPVTG_ms = hal.scheduler->millis();
+            _last_GPVTG_ms = AP_HAL::millis();
             _gps_data_good = true;
         } else {
             _sentence_type = _GPS_SENTENCE_OTHER;

libraries/AP_GPS/AP_GPS_PX4.cpp

@@ -52,7 +52,7 @@ AP_GPS_PX4::read(void)
 
     if (updated) {
         if (OK == orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps_pos)) {
-            state.last_gps_time_ms = hal.scheduler->millis();
+            state.last_gps_time_ms = AP_HAL::millis();
             state.status  = (AP_GPS::GPS_Status) (_gps_pos.fix_type | AP_GPS::NO_FIX);
             state.num_sats = _gps_pos.satellites_used;
             state.hdop = uint16_t(_gps_pos.eph*100.0f + .5f);

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -53,7 +53,7 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
 bool
 AP_GPS_SBF::read(void)
 {
-	uint32_t now = hal.scheduler->millis();
+	uint32_t now = AP_HAL::millis();
 	
 	if (_init_blob_index < (sizeof(_initialisation_blob) / sizeof(_initialisation_blob[0]))) {
 		if (now > _init_blob_time) {
@@ -156,7 +156,7 @@ AP_GPS_SBF::log_ExtEventPVTGeodetic(const msg4007 &temp)
         return;
     }
 
-    uint64_t now = hal.scheduler->micros64();
+    uint64_t now = AP_HAL::micros64();
 
     struct log_GPS_SBF_EVENT header = {
         LOG_PACKET_HEADER_INIT(LOG_GPS_SBF_EVENT_MSG),
@@ -199,7 +199,7 @@ AP_GPS_SBF::process_message(void)
             state.time_week_ms = (uint32_t)(temp.TOW);
         }
 		
-		state.last_gps_time_ms = hal.scheduler->millis();
+		state.last_gps_time_ms = AP_HAL::millis();
 
         state.hdop = last_hdop;
 
@@ -289,7 +289,7 @@ AP_GPS_SBF::inject_data(uint8_t *data, uint8_t len)
 {
 
     if (port->txspace() > len) {
-        last_injected_data_ms = hal.scheduler->millis();
+        last_injected_data_ms = AP_HAL::millis();
         port->write(data, len);
     } else {
         Debug("SBF: Not enough TXSPACE");

libraries/AP_GPS/AP_GPS_SBP.cpp

@@ -66,7 +66,7 @@ AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state,
     //Externally visible state
     state.status = AP_GPS::NO_FIX;
     state.have_vertical_velocity = true;
-    state.last_gps_time_ms = last_heatbeat_received_ms = hal.scheduler->millis();
+    state.last_gps_time_ms = last_heatbeat_received_ms = AP_HAL::millis();
 
 }
 
@@ -92,7 +92,7 @@ AP_GPS_SBP::inject_data(uint8_t *data, uint8_t len)
 {
 
     if (port->txspace() > len) {
-        last_injected_data_ms = hal.scheduler->millis();
+        last_injected_data_ms = AP_HAL::millis();
         port->write(data, len);
     } else {
         Debug("PIKSI: Not enough TXSPACE");
@@ -188,7 +188,7 @@ void
 AP_GPS_SBP::_sbp_process_message() {
     switch(parser_state.msg_type) {
         case SBP_HEARTBEAT_MSGTYPE:
-            last_heatbeat_received_ms = hal.scheduler->millis();
+            last_heatbeat_received_ms = AP_HAL::millis();
             break;
 
         case SBP_GPS_TIME_MSGTYPE:
@@ -244,7 +244,7 @@ AP_GPS_SBP::_attempt_state_update()
     //
     // If we have a full update available, save it
     //
-    uint32_t now = hal.scheduler->millis();
+    uint32_t now = AP_HAL::millis();
     bool ret = false;
 
     if (now - last_heatbeat_received_ms > SBP_TIMEOUT_HEATBEAT) {
@@ -462,7 +462,7 @@ AP_GPS_SBP::logging_log_full_update()
 
     struct log_SbpHealth pkt = {
         LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH),
-        time_us                    : hal.scheduler->micros64(),
+        time_us                    : AP_HAL::micros64(),
         crc_error_counter          : crc_error_counter,
         last_injected_data_ms      : last_injected_data_ms,
         last_iar_num_hypotheses    : last_iar_num_hypotheses,
@@ -488,7 +488,7 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type,
 
     logging_write_headers();
 
-    uint64_t time_us = hal.scheduler->micros64();
+    uint64_t time_us = AP_HAL::micros64();
 
     struct log_SbpRAW1 pkt = {
         LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAW1),

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -156,7 +156,7 @@ AP_GPS_UBLOX::read(void)
     uint8_t data;
     int16_t numc;
     bool parsed = false;
-    uint32_t millis_now = hal.scheduler->millis();
+    uint32_t millis_now = AP_HAL::millis();
 
     if (need_rate_update) {
         send_next_rate_update();
@@ -283,7 +283,7 @@ void AP_GPS_UBLOX::log_mon_hw(void)
     }
     struct log_Ubx1 pkt = {
         LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX1_MSG)),
-        time_us    : hal.scheduler->micros64(),
+        time_us    : AP_HAL::micros64(),
         instance   : state.instance,
         noisePerMS : _buffer.mon_hw_60.noisePerMS,
         jamInd     : _buffer.mon_hw_60.jamInd,
@@ -307,7 +307,7 @@ void AP_GPS_UBLOX::log_mon_hw2(void)
 
     struct log_Ubx2 pkt = {
         LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX2_MSG)),
-        time_us   : hal.scheduler->micros64(),
+        time_us   : AP_HAL::micros64(),
         instance  : state.instance,
         ofsI      : _buffer.mon_hw2.ofsI,
         magI      : _buffer.mon_hw2.magI,
@@ -325,7 +325,7 @@ void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw)
     if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
         return;
     }
-    uint64_t now = hal.scheduler->micros64();
+    uint64_t now = AP_HAL::micros64();
     for (uint8_t i=0; i<raw.numSV; i++) {
         struct log_GPS_RAW pkt = {
             LOG_PACKET_HEADER_INIT(LOG_GPS_RAW_MSG),
@@ -350,7 +350,7 @@ void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw)
     if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
         return;
     }
-    uint64_t now = hal.scheduler->micros64();
+    uint64_t now = AP_HAL::micros64();
 
     struct log_GPS_RAWH header = {
         LOG_PACKET_HEADER_INIT(LOG_GPS_RAWH_MSG),
@@ -615,7 +615,7 @@ AP_GPS_UBLOX::_parse_gps(void)
         }
         state.num_sats    = _buffer.solution.satellites;
         if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
-            state.last_gps_time_ms = hal.scheduler->millis();
+            state.last_gps_time_ms = AP_HAL::millis();
             if (state.time_week == _buffer.solution.week &&
                 state.time_week_ms + 200 == _buffer.solution.time) {
                 // we got a 5Hz update. This relies on the way
@@ -630,8 +630,8 @@ AP_GPS_UBLOX::_parse_gps(void)
         next_fix = state.status;
         state.num_sats = 10;
         state.time_week = 1721;
-        state.time_week_ms = hal.scheduler->millis() + 3*60*60*1000 + 37000;
+        state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
-        state.last_gps_time_ms = hal.scheduler->millis();
+        state.last_gps_time_ms = AP_HAL::millis();
         state.hdop = 130;
 #endif
         break;
@@ -690,13 +690,13 @@ AP_GPS_UBLOX::_parse_gps(void)
     if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
         _new_speed = _new_position = false;
 		_fix_count++;
-        if ((hal.scheduler->millis() - _last_5hz_time) > 15000U && !need_rate_update) {
+        if ((AP_HAL::millis() - _last_5hz_time) > 15000U && !need_rate_update) {
             // the GPS is running slow. It possibly browned out and
             // restarted with incorrect parameters. We will slowly
             // send out new parameters to fix it
             need_rate_update = true;
             rate_update_step = 0;
-            _last_5hz_time = hal.scheduler->millis();
+            _last_5hz_time = AP_HAL::millis();
         }
 
 		if (_fix_count == 50 && gps._sbas_mode != 2) {
@@ -791,7 +791,7 @@ AP_GPS_UBLOX::_configure_gps(void)
 {
     // start the process of updating the GPS rates
     need_rate_update = true;
-    _last_5hz_time = hal.scheduler->millis();
+    _last_5hz_time = AP_HAL::millis();
     rate_update_step = 0;
 
     // ask for the current navigation settings

libraries/AP_GPS/GPS_Backend.cpp

@@ -71,7 +71,7 @@ uint64_t AP_GPS::time_epoch_usec(uint8_t instance)
     const uint64_t unix_offset = 17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL;
     uint64_t fix_time_ms = unix_offset + istate.time_week*ms_per_week + istate.time_week_ms;
     // add in the milliseconds since the last fix
-    return (fix_time_ms + (hal.scheduler->millis() - istate.last_gps_time_ms)) * 1000ULL;
+    return (fix_time_ms + (AP_HAL::millis() - istate.last_gps_time_ms)) * 1000ULL;
 }