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

AP_GPS: added support for GPS time in week/millisec

Browse Source 
also adds time_epoch_usec() for MAVLink SYSTEM_TIME
This commit is contained in:
Andrew Tridgell 2013-10-23 22:13:48 +11:00
parent 6438be74e0
commit 6f5ac1d553
15 changed files with 143 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_GPS/AP_GPS_HIL.cpp
View File

@ -39,10 +39,11 @@ bool AP_GPS_HIL::read(void)
return result;
}
void AP_GPS_HIL::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude,
void AP_GPS_HIL::setHIL(uint64_t _time_epoch_ms, float _latitude, float _longitude, float _altitude,
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats)
{
time = _time;
time_week = _time_epoch_ms / (86400*7*(uint64_t)1000);
time_week_ms = _time_epoch_ms - time_week*(86400*7*(uint64_t)1000);
latitude = _latitude*1.0e7f;
longitude = _longitude*1.0e7f;
altitude_cm = _altitude*1.0e2f;

2
libraries/AP_GPS/AP_GPS_HIL.h
View File

@ -45,7 +45,7 @@ public:
* @param speed_3d - ground speed in meters/second
* @param altitude - altitude in meters
*/
virtual void setHIL(uint32_t time, float latitude, float longitude, float altitude,
virtual void setHIL(uint64_t time_epoch_ms, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
private:

16
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -50,9 +50,6 @@ AP_GPS_MTK::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
// Set Nav Threshold to 0 m/s
_port->print(MTK_NAVTHRES_OFF);
// set initial epoch code
_epoch = TIME_OF_DAY;
}
// Process bytes available from the stream
@ -162,14 +159,11 @@ restart:
ground_course_cd = _swapl(&_buffer.msg.ground_course) / 10000;
num_sats = _buffer.msg.satellites;
// time from gps is UTC, but convert here to msToD
int32_t time_utc = _swapl(&_buffer.msg.utc_time);
int32_t temp = (time_utc/10000000);
time_utc -= temp*10000000;
time = temp * 3600000;
temp = (time_utc/100000);
time_utc -= temp*100000;
time += temp * 60000 + time_utc;
if (fix >= GPS::FIX_2D) {
_make_gps_time(0, _swapl(&_buffer.msg.utc_time)*10);
}
// we don't change _last_gps_time as we don't know the
// full date
parsed = true;
}

37
libraries/AP_GPS/AP_GPS_MTK19.cpp
View File

@ -27,6 +27,8 @@
#include "AP_GPS_MTK19.h"
#include <stdint.h>
extern const AP_HAL::HAL& hal;
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_MTK19::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
@ -49,11 +51,6 @@ AP_GPS_MTK19::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
// Set Nav Threshold to 0 m/s
_port->print(MTK_NAVTHRES_OFF);
// set initial epoch code
_epoch = TIME_OF_DAY;
_time_offset = 0;
_offset_calculated = false;
}
// Process bytes available from the stream
@ -166,16 +163,28 @@ restart:
ground_course_cd = _buffer.msg.ground_course;
num_sats = _buffer.msg.satellites;
hdop = _buffer.msg.hdop;
date = _buffer.msg.utc_date;
if (fix >= GPS::FIX_2D) {
if (_fix_counter == 0) {
uint32_t bcd_time_ms;
if (_mtk_revision == MTK_GPS_REVISION_V16) {
bcd_time_ms = _buffer.msg.utc_time*10;
} else {
bcd_time_ms = _buffer.msg.utc_time;
}
_make_gps_time(_buffer.msg.utc_date, bcd_time_ms);
_last_gps_time = hal.scheduler->millis();
}
// the _fix_counter is to reduce the cost of the GPS
// BCD time conversion by only doing it every 10s
// between those times we use the HAL system clock as
// an offset from the last fix
_fix_counter++;
if (_fix_counter == 50) {
_fix_counter = 0;
}
}
// time from gps is UTC, but convert here to msToD
int32_t time_utc = _buffer.msg.utc_time;
int32_t temp = (time_utc/10000000);
time_utc -= temp*10000000;
time = temp * 3600000;
temp = (time_utc/100000);
time_utc -= temp*100000;
time += temp * 60000 + time_utc;
parsed = true;
#ifdef FAKE_GPS_LOCK_TIME

7
libraries/AP_GPS/AP_GPS_MTK19.h
View File

@ -37,7 +37,8 @@ public:
GPS(),
_step(0),
_payload_counter(0),
_mtk_revision(0)
_mtk_revision(0),
_fix_counter(0)
{}
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE);
@ -80,9 +81,7 @@ private:
uint8_t _payload_counter;
uint8_t _mtk_revision;
// Time from UNIX Epoch offset
long _time_offset;
bool _offset_calculated;
uint8_t _fix_counter;
// Receive buffer
union {

6
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -244,8 +244,8 @@ bool AP_GPS_NMEA::_term_complete()
if (_gps_data_good) {
switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC:
time = _new_time;
date = _new_date;
//time = _new_time;
//date = _new_date;
latitude = _new_latitude;
longitude = _new_longitude;
ground_speed_cm = _new_speed;
@ -254,7 +254,7 @@ bool AP_GPS_NMEA::_term_complete()
break;
case _GPS_SENTENCE_GPGGA:
altitude_cm = _new_altitude;
time = _new_time;
//time = _new_time;
latitude = _new_latitude;
longitude = _new_longitude;
num_sats = _new_satellite_count;

2
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -179,7 +179,7 @@ AP_GPS_SIRF::_parse_gps(void)
{
switch(_msg_id) {
case MSG_GEONAV:
time = _swapl(&_buffer.nav.time);
//time = _swapl(&_buffer.nav.time);
// parse fix type
if (_buffer.nav.fix_invalid) {
fix = GPS::FIX_NONE;

8
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -54,8 +54,6 @@ AP_GPS_UBLOX::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
_port->flush();
_epoch = TIME_OF_WEEK;
// configure the GPS for the messages we want
_configure_gps();
@ -275,7 +273,6 @@ AP_GPS_UBLOX::_parse_gps(void)
switch (_msg_id) {
case MSG_POSLLH:
Debug("MSG_POSLLH next_fix=%u", next_fix);
time = _buffer.posllh.time;
longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude;
altitude_cm = _buffer.posllh.altitude_msl / 10;
@ -328,6 +325,11 @@ AP_GPS_UBLOX::_parse_gps(void)
}
num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP;
if (next_fix >= GPS::FIX_2D) {
time_week_ms = _buffer.solution.time;
time_week = _buffer.solution.week;
_last_gps_time = hal.scheduler->millis();
}
#if UBLOX_FAKE_3DLOCK
next_fix = fix;
num_sats = 10;

69
libraries/AP_GPS/GPS.cpp
View File

@ -20,8 +20,8 @@ extern const AP_HAL::HAL& hal;
GPS::GPS(void) :
// ensure all the inherited fields are zeroed
time(0),
date(0),
time_week_ms(0),
time_week(0),
latitude(0),
longitude(0),
altitude_cm(0),
@ -35,6 +35,7 @@ GPS::GPS(void) :
valid_read(false),
last_fix_time(0),
_have_raw_velocity(false),
_last_gps_time(0),
_idleTimer(0),
_status(GPS::NO_FIX),
_last_ground_speed_cm(0),
@ -112,7 +113,7 @@ GPS::update(void)
}
void
GPS::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude,
GPS::setHIL(uint64_t _time_epoch_ms, float _latitude, float _longitude, float _altitude,
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats)
{
}
@ -221,3 +222,65 @@ int16_t GPS::_swapi(const void *bytes) const
return(u.v);
}
/**
current time since the unix epoch in microseconds
This costs about 60 usec on AVR2560
*/
uint64_t GPS::time_epoch_usec(void)
{
if (_last_gps_time == 0) {
return 0;
}
const uint64_t ms_per_week = 7000ULL*86400ULL;
const uint64_t unix_offset = 17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL;
uint64_t fix_time_ms = unix_offset + time_week*ms_per_week + time_week_ms;
// add in the milliseconds since the last fix
return (fix_time_ms + (hal.scheduler->millis() - _last_gps_time)) * 1000ULL;
}
/**
fill in time_week_ms and time_week from BCD date and time components
assumes MTK19 millisecond form of bcd_time
This function takes about 340 usec on the AVR2560
*/
void GPS::_make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
{
uint8_t year, mon, day, hour, min, sec;
uint16_t msec;
year = bcd_date % 100;
mon = (bcd_date / 100) % 100;
day = bcd_date / 10000;
msec = bcd_milliseconds % 1000;
uint32_t v = bcd_milliseconds;
msec = v % 1000; v /= 1000;
sec = v % 100; v /= 100;
min = v % 100; v /= 100;
hour = v % 100; v /= 100;
int8_t rmon = mon - 2;
if (0 >= rmon) {
rmon += 12;
year -= 1;
}
// get time in seconds since unix epoch
uint32_t ret = (year/4) - 15 + 367*rmon/12 + day;
ret += year*365 + 10501;
ret = ret*24 + hour;
ret = ret*60 + min;
ret = ret*60 + sec;
// convert to time since GPS epoch
ret -= 272764785UL;
// get GPS week and time
time_week = ret / (7*86400UL);
time_week_ms = (ret % (7*86400UL)) * 1000;
time_week_ms += msec;
}

36
libraries/AP_GPS/GPS.h
View File

@ -72,24 +72,6 @@ public:
return _status;
}
/// GPS time epoch codes
///
enum GPS_Time_Epoch {
TIME_OF_DAY = 0, ///<
TIME_OF_WEEK = 1, ///< Ublox
TIME_OF_YEAR = 2, ///< MTK, NMEA
UNIX_EPOCH = 3 ///< If available
}; ///< SIFR?
/// Query GPS time epoch
///
/// @returns Current GPS time epoch code
///
GPS_Time_Epoch epoch(void) {
return _epoch;
}
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required to set the
@ -100,8 +82,8 @@ public:
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting engine_setting = GPS_ENGINE_NONE) = 0;
// Properties
uint32_t time; ///< GPS time (milliseconds from epoch)
uint32_t date; ///< GPS date (FORMAT TBD)
uint32_t time_week_ms; ///< GPS time (milliseconds from start of GPS week)
uint16_t time_week; ///< GPS week number
int32_t latitude; ///< latitude in degrees * 10,000,000
int32_t longitude; ///< longitude in degrees * 10,000,000
int32_t altitude_cm; ///< altitude in cm
@ -123,7 +105,7 @@ public:
bool print_errors; ///< deprecated
// HIL support
virtual void setHIL(uint32_t time, float latitude, float longitude, float altitude,
virtual void setHIL(uint64_t time_epoch_ms, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
// components of velocity in 2D, in m/s
@ -157,6 +139,9 @@ public:
// the time we last processed a message in milliseconds
uint32_t last_message_time_ms(void) { return _idleTimer; }
// return last fix time since the 1/1/1970 in microseconds
uint64_t time_epoch_usec(void);
// return true if the GPS supports raw velocity values
@ -198,9 +183,6 @@ protected:
///
void _error(const char *msg);
/// Time epoch code for the gps in use
GPS_Time_Epoch _epoch;
enum GPS_Engine_Setting _nav_setting;
void _write_progstr_block(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size);
@ -218,6 +200,12 @@ protected:
// detected baudrate
uint16_t _baudrate;
// the time we got the last GPS timestamp
uint32_t _last_gps_time;
// return time in seconds since GPS epoch given time components
void _make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds);
private:

4
libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde
View File

@ -50,8 +50,10 @@ void loop()
hal.console->print(gps.num_sats, BASE_DEC);
hal.console->print(" FIX:");
hal.console->print(gps.fix, BASE_DEC);
hal.console->print(" WEEK:");
hal.console->print(gps.time_week, BASE_DEC);
hal.console->print(" TIM:");
hal.console->print(gps.time, BASE_DEC);
hal.console->print(gps.time_week_ms, BASE_DEC);
hal.console->println();
gps.new_data = 0; // We have readed the data
}

5
libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde
View File

@ -50,12 +50,13 @@ void loop()
print_latlon(hal.console,gps->latitude);
hal.console->print(" Lon: ");
print_latlon(hal.console,gps->longitude);
hal.console->printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu STATUS: %u\n",
hal.console->printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %u/%lu STATUS: %u\n",
(float)gps->altitude_cm / 100.0,
(float)gps->ground_speed_cm / 100.0,
(int)gps->ground_course_cd / 100,
gps->num_sats,
gps->time,
gps->time_week,
gps->time_week_ms,
gps->status());
} else {
hal.console->println("No fix");

4
libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde
View File

@ -53,8 +53,10 @@ void loop()
hal.console->print(gps.num_sats, BASE_DEC);
hal.console->print(" FIX:");
hal.console->print(gps.fix, BASE_DEC);
hal.console->print(" WEEK:");
hal.console->print(gps.time_week, BASE_DEC);
hal.console->print(" TIM:");
hal.console->print(gps.time, BASE_DEC);
hal.console->print(gps.time_week_ms, BASE_DEC);
hal.console->println();
gps.new_data = 0; // We have readed the data
}

17
libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde
View File

@ -61,14 +61,15 @@ void loop()
if (gps->fix) {
hal.console->printf_P(
PSTR("Lat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s "
"CoG: %d SAT: %d TIM: %lu\r\n"),
(float)gps->latitude / T7,
(float)gps->longitude / T7,
(float)gps->altitude_cm / 100.0,
(float)gps->ground_speed_cm / 100.0,
(int)gps->ground_course_cd / 100,
gps->num_sats,
gps->time);
"CoG: %d SAT: %d TIM: %u/%lu\r\n"),
(float)gps->latitude / T7,
(float)gps->longitude / T7,
(float)gps->altitude_cm / 100.0,
(float)gps->ground_speed_cm / 100.0,
(int)gps->ground_course_cd / 100,
gps->num_sats,
gps->time_week,
gps->time_week_ms);
} else {
hal.console->println_P(PSTR("No fix"));
}

4
libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde
View File

@ -58,8 +58,10 @@ void loop()
hal.console->print(gps.num_sats, BASE_DEC);
hal.console->print(" FIX:");
hal.console->print(gps.fix, BASE_DEC);
hal.console->print(" WEEK:");
hal.console->print(gps.time_week, BASE_DEC);
hal.console->print(" TIM:");
hal.console->print(gps.time, BASE_DEC);
hal.console->print(gps.time_week_ms, BASE_DEC);
hal.console->println();
gps.new_data = 0; // We have readed the data
}