zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_GPS: use millis/micros/panic functions

mission-4.1.18
Caio Marcelo de Oliveira Filho 9 years ago committed by Randy Mackay
parent
2b982d6391
commit
c33b86a783
9 changed files with 37 additions and 37 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      libraries/AP_GPS/AP_GPS.cpp
  2. 6
      libraries/AP_GPS/AP_GPS_GSOF.cpp
  3. 2
      libraries/AP_GPS/AP_GPS_MTK19.cpp
  4. 10
      libraries/AP_GPS/AP_GPS_NMEA.cpp
  5. 2
      libraries/AP_GPS/AP_GPS_PX4.cpp
  6. 8
      libraries/AP_GPS/AP_GPS_SBF.cpp
  7. 12
      libraries/AP_GPS/AP_GPS_SBP.cpp
  8. 22
      libraries/AP_GPS/AP_GPS_UBLOX.cpp
  9. 2
      libraries/AP_GPS/GPS_Backend.cpp

10
libraries/AP_GPS/AP_GPS.cpp
Unescape View File

@ -184,7 +184,7 @@ AP_GPS::detect_instance(uint8_t instance)
{ {
AP_GPS_Backend *new_gps = NULL; AP_GPS_Backend *new_gps = NULL;
struct detect_state *dstate = &detect_state[instance]; 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 CONFIG_HAL_BOARD == HAL_BOARD_PX4
if (_type[instance] == GPS_TYPE_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 // we have an active driver for this instance
bool result = drivers[instance]->read(); 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 // 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 // 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) { 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); 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) || 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) { if (instance >= GPS_MAX_INSTANCES) {
return; return;
} }
uint32_t tnow = hal.scheduler->millis(); uint32_t tnow = AP_HAL::millis();
GPS_State &istate = state[instance]; GPS_State &istate = state[instance];
istate.status = _status; istate.status = _status;
istate.location = _location; 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); last_send_time_ms[chan] = last_message_time_ms(0);
} else { } else {
// when we don't have a GPS then send at 1Hz // 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) { if (now - last_send_time_ms[chan] < 1000) {
return; return;
} }

6
libraries/AP_GPS/AP_GPS_GSOF.cpp
Unescape View File

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

2
libraries/AP_GPS/AP_GPS_MTK19.cpp
Unescape View File

@ -159,7 +159,7 @@ restart:
(unsigned)_mtk_revision); (unsigned)_mtk_revision);
#endif #endif
make_gps_time(_buffer.msg.utc_date, bcd_time_ms); 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 // the _fix_counter is to reduce the cost of the GPS
// BCD time conversion by only doing it every 10s // BCD time conversion by only doing it every 10s

10
libraries/AP_GPS/AP_GPS_NMEA.cpp
Unescape View File

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

2
libraries/AP_GPS/AP_GPS_PX4.cpp
Unescape View File

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

8
libraries/AP_GPS/AP_GPS_SBF.cpp
Unescape View File

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

12
libraries/AP_GPS/AP_GPS_SBP.cpp
Unescape View File

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

22
libraries/AP_GPS/AP_GPS_UBLOX.cpp
Unescape View File

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

2
libraries/AP_GPS/GPS_Backend.cpp
Unescape View File

@ -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; 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; 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 // 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;
} }

Write Preview
Loading…
Cancel
Save