Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 06:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: use millis/micros/panic functions

Browse Source
This commit is contained in:
Caio Marcelo de Oliveira Filho 2015-11-20 12:10:22 +09:00 committed by Randy Mackay
parent 2b982d6391
commit c33b86a783
9 changed files with 37 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_GPS/AP_GPS.cpp
View File

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

6
libraries/AP_GPS/AP_GPS_GSOF.cpp
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
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");

2
libraries/AP_GPS/AP_GPS_MTK19.cpp
View File

@ -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

10
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

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

2
libraries/AP_GPS/AP_GPS_PX4.cpp
View File

@ -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);

8
libraries/AP_GPS/AP_GPS_SBF.cpp
View File

@ -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");

12
libraries/AP_GPS/AP_GPS_SBP.cpp
View File

@ -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),

22
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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.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

2
libraries/AP_GPS/GPS_Backend.cpp
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;
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;
}