From 30151c8253f469b20ae0c5505dde178472efa6cc Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 4 Feb 2017 07:51:15 +0900 Subject: [PATCH] Global: Define MSEC_PER_SEC, MSEC_PER_WEEK, SEC_PER_WEEK and UNIX_OFFSET. --- libraries/AP_GPS/AP_GPS.cpp | 6 +++--- libraries/AP_GPS/AP_GPS.h | 2 ++ libraries/AP_GPS/GPS_Backend.cpp | 8 +++----- libraries/AP_HAL_SITL/sitl_gps.cpp | 4 ++-- libraries/AP_Math/definitions.h | 3 +++ 5 files changed, 13 insertions(+), 10 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 0b0e9a0388..1566e072d9 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -611,9 +611,9 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms, istate.hdop = hdop; istate.num_sats = _num_sats; istate.last_gps_time_ms = tnow; - uint64_t gps_time_ms = time_epoch_ms - (17000ULL*86400ULL + 52*10*7000ULL*86400ULL - GPS_LEAPSECONDS_MILLIS); - istate.time_week = gps_time_ms / (86400*7*(uint64_t)1000); - istate.time_week_ms = gps_time_ms - istate.time_week*(86400*7*(uint64_t)1000); + uint64_t gps_time_ms = time_epoch_ms - UNIX_OFFSET; + istate.time_week = gps_time_ms / MSEC_PER_WEEK; + istate.time_week_ms = gps_time_ms - istate.time_week * MSEC_PER_WEEK; timing[instance].last_message_time_ms = tnow; timing[instance].last_fix_time_ms = tnow; _type[instance].set(GPS_TYPE_HIL); diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 078bcfe93a..c97d4c7c6c 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -34,6 +34,8 @@ // the number of GPS leap seconds #define GPS_LEAPSECONDS_MILLIS 18000ULL +#define UNIX_OFFSET (17000ULL * 86400ULL + 52 * 10 * MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS) + class DataFlash_Class; class AP_GPS_Backend; diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 3e900ce268..997a2902b8 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -63,9 +63,7 @@ int16_t AP_GPS_Backend::swap_int16(int16_t v) const */ uint64_t AP_GPS::time_epoch_convert(uint16_t gps_week, uint32_t gps_ms) { - const uint64_t ms_per_week = 7000ULL*86400ULL; - const uint64_t unix_offset = 17000ULL*86400ULL + 52*10*7000ULL*86400ULL - GPS_LEAPSECONDS_MILLIS; - uint64_t fix_time_ms = unix_offset + gps_week*ms_per_week + gps_ms; + uint64_t fix_time_ms = UNIX_OFFSET + gps_week * MSEC_PER_WEEK + gps_ms; return fix_time_ms; } @@ -120,8 +118,8 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) ret -= 272764785UL; // get GPS week and time - state.time_week = ret / (7*86400UL); - state.time_week_ms = (ret % (7*86400UL)) * 1000; + state.time_week = ret / SEC_PER_WEEK; + state.time_week_ms = (ret % SEC_PER_WEEK) * MSEC_PER_SEC; state.time_week_ms += msec; } diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 6d6a2c28c4..f5c1cf276a 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -179,10 +179,10 @@ static void gps_time(uint16_t *time_week, uint32_t *time_week_ms) simulation_timeval(&tv); const uint32_t epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - (GPS_LEAPSECONDS_MILLIS / 1000ULL); uint32_t epoch_seconds = tv.tv_sec - epoch; - *time_week = epoch_seconds / (86400*7UL); + *time_week = epoch_seconds / SEC_PER_WEEK; uint32_t t_ms = tv.tv_usec / 1000; // round time to nearest 200ms - *time_week_ms = (epoch_seconds % (86400*7UL))*1000 + ((t_ms/200) * 200); + *time_week_ms = (epoch_seconds % SEC_PER_WEEK) * MSEC_PER_SEC + ((t_ms/200) * 200); } /* diff --git a/libraries/AP_Math/definitions.h b/libraries/AP_Math/definitions.h index 301158a8f3..1ee853d579 100644 --- a/libraries/AP_Math/definitions.h +++ b/libraries/AP_Math/definitions.h @@ -68,3 +68,6 @@ static const double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); #define NSEC_PER_USEC 1000ULL #define USEC_PER_SEC 1000000ULL #define USEC_PER_MSEC 1000ULL +#define MSEC_PER_SEC 1000UL +#define SEC_PER_WEEK (7UL * 86400UL) +#define MSEC_PER_WEEK (SEC_PER_WEEK * MSEC_PER_SEC)