From c33b86a78322c2bcfb59deb3cf8ff7a27e23ff4a Mon Sep 17 00:00:00 2001 From: Caio Marcelo de Oliveira Filho Date: Fri, 20 Nov 2015 12:10:22 +0900 Subject: [PATCH] AP_GPS: use millis/micros/panic functions --- libraries/AP_GPS/AP_GPS.cpp | 10 +++++----- libraries/AP_GPS/AP_GPS_GSOF.cpp | 6 +++--- libraries/AP_GPS/AP_GPS_MTK19.cpp | 2 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 10 +++++----- libraries/AP_GPS/AP_GPS_PX4.cpp | 2 +- libraries/AP_GPS/AP_GPS_SBF.cpp | 8 ++++---- libraries/AP_GPS/AP_GPS_SBP.cpp | 12 ++++++------ libraries/AP_GPS/AP_GPS_UBLOX.cpp | 22 +++++++++++----------- libraries/AP_GPS/GPS_Backend.cpp | 2 +- 9 files changed, 37 insertions(+), 37 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index fecebd8971..f3af162ffd 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/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; } diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index 97246bf338..a558c8d564 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/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"); diff --git a/libraries/AP_GPS/AP_GPS_MTK19.cpp b/libraries/AP_GPS/AP_GPS_MTK19.cpp index 9f5f9b6beb..300985c752 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.cpp +++ b/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 diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 7472230887..98ef95a628 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS_PX4.cpp b/libraries/AP_GPS/AP_GPS_PX4.cpp index 0ee5ad5959..c19f636c42 100644 --- a/libraries/AP_GPS/AP_GPS_PX4.cpp +++ b/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); diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 1d3bb7e16c..86db3c70d7 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/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"); diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp index efeaedac10..9c8f391b52 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.cpp +++ b/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), diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 12856e3958..24f974f6bf 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/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; ilogging_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.last_gps_time_ms = hal.scheduler->millis(); + state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000; + 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 diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 5a6f6ca998..3ae9a49f3b 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/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; }