Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[gps] resurrect the gps time sync using the sys_time ticks (works on lpc and stm32)

Browse Source 
this is e.g. used for the external trigger sync
The resolution now depends on SYS_TIME_RESOLUTION
If real cpu ticks resolution is needed, a better arch independent solution is needed.
This commit is contained in:
Felix Ruess
2013-02-13 19:59:08 +01:00
parent b690fda8e8
commit 690af6a51f
8 changed files with 60 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/mcu_periph/sys_time.h
+3
View File
@@ -100,6 +100,8 @@ static inline bool_t sys_time_check_and_ack_timer(tid_t id) {
#define SIGNED_CPU_TICKS_OF_NSEC(us) SIGNED_CPU_TICKS_OF_SEC((us) * 1e-9)
/* paparazzi sys_time timers */
/** system time resolution in seconds */
#ifndef SYS_TIME_RESOLUTION
#define SYS_TIME_RESOLUTION ( 1./1024. )
#endif
@@ -110,6 +112,7 @@ static inline bool_t sys_time_check_and_ack_timer(tid_t id) {
#define SYS_TIME_TICKS_OF_NSEC(ns) SYS_TIME_TICKS_OF_SEC((ns) * 1e-9)
#define SEC_OF_SYS_TIME_TICKS(t) ((t) * SYS_TIME_RESOLUTION)
#define MSEC_OF_SYS_TIME_TICKS(t) ((t) * SYS_TIME_RESOLUTION / 1e-3)
#define USEC_OF_SYS_TIME_TICKS(t) ((t) * SYS_TIME_RESOLUTION / 1e-6)
#define NSEC_OF_SYS_TIME_TICKS(t) ((t) * SYS_TIME_RESOLUTION / 1e-9)
sw/airborne/modules/meteo/windturbine.c
+2 -2
View File
@@ -47,8 +47,8 @@ void windturbine_periodic( void ) {
uint8_t turb_id = TURBINE_ID;
uint32_t sync_itow, cycle_time;
sync_itow = gps_tow_from_ticks(trigger_t0);
cycle_time = MSEC_OF_CPU_TICKS(trigger_delta_t0);
sync_itow = gps_tow_from_sys_ticks(trigger_t0);
cycle_time = MSEC_OF_SYS_TIME_TICKS(trigger_delta_t0);
DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel, DefaultDevice,
&ac_id,
sw/airborne/modules/sensors/trigger_ext.c
+2 -2
View File
@@ -48,8 +48,8 @@ void trigger_ext_periodic( void ) {
uint8_t turb_id = TURBINE_ID;
uint32_t sync_itow, cycle_time;
sync_itow = gps_tow_from_ticks(trigger_t0);
cycle_time = MSEC_OF_CPU_TICKS(delta_t0);
sync_itow = gps_tow_from_sys_ticks(trigger_t0);
cycle_time = MSEC_OF_SYS_TIME_TICKS(delta_t0);
DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel, DefaultDevice,
&ac_id,
sw/airborne/subsystems/gps.c
+25 -3
View File
@@ -28,11 +28,11 @@
#include "led.h"
#define MSEC_PER_WEEK (1000*60*60*24*7)
struct GpsState gps;
#ifdef GPS_TIMESTAMP
struct GpsTimeSync gps_time;
#endif
struct GpsTimeSync gps_time_sync;
void gps_init(void) {
gps.fix = GPS_FIX_NONE;
@@ -44,3 +44,25 @@ void gps_init(void) {
gps_impl_init();
#endif
}
uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
{
uint32_t clock_delta;
uint32_t time_delta;
uint32_t itow_now;
if (sys_ticks < gps_time_sync.t0_ticks) {
clock_delta = (0xFFFFFFFF - sys_ticks) + gps_time_sync.t0_ticks + 1;
} else {
clock_delta = sys_ticks - gps_time_sync.t0_ticks;
}
time_delta = MSEC_OF_SYS_TIME_TICKS(clock_delta);
itow_now = gps_time_sync.t0_tow + time_delta;
if (itow_now > MSEC_PER_WEEK) {
itow_now %= MSEC_PER_WEEK;
}
return itow_now;
}
sw/airborne/subsystems/gps.h
+18 -43
View File
@@ -31,6 +31,7 @@
#include "std.h"
#include "math/pprz_geodetic_int.h"
#include "mcu_periph/sys_time.h"
/* GPS model specific implementation or sim */
#ifdef GPS_TYPE_H
@@ -86,17 +87,17 @@ struct GpsState {
uint16_t reset; ///< hotstart, warmstart, coldstart
};
/** data structure for GPS time sync */
struct GpsTimeSync {
uint32_t t0_tow; ///< for time sync: time of week in ms for time sync
int32_t t0_tow_frac; ///< for time sync: fractional ns remainder of tow [ms], range -500000 .. 500000
uint32_t t0; ///< for time sync: hw clock ticks when GPS message is received
uint32_t t0_tow; ///< GPS time of week in ms from last message
int32_t t0_tow_frac; ///< fractional ns remainder of tow [ms], range -500000 .. 500000
uint32_t t0_ticks; ///< hw clock ticks when GPS message is received
};
/** global GPS state */
extern struct GpsState gps;
/** initialize the global GPS state */
extern void gps_init(void);
/* GPS model specific init implementation */
@@ -108,7 +109,6 @@ extern void gps_impl_init(void);
#define GPS_TIMEOUT 5
#endif
#include "mcu_periph/sys_time.h"
inline bool_t GpsIsLost(void);
inline bool_t GpsIsLost(void) {
@@ -120,49 +120,24 @@ inline bool_t GpsIsLost(void) {
}
//TODO
// this still needs to call gps specific stuff
/*
/**
* GPS Reset
* @todo this still needs to call gps specific stuff
*/
#define CFG_RST_BBR_Hotstart 0x0000
#define CFG_RST_BBR_Warmstart 0x0001
#define CFG_RST_BBR_Coldstart 0xffff
#define gps_Reset(_val) { \
}
/*
* For GPS time synchronizaiton...
*/
extern struct GpsTimeSync gps_time_sync;
#ifdef GPS_TIMESTAMP
#ifndef PCLK
#error unknown PCLK frequency
#endif
extern struct GpsTimeSync gps_time;
uint32_t gps_tow_from_ticks(uint32_t clock_ticks)
{
uint32_t clock_delta;
uint32_t time_delta;
uint32_t itow_now;
if (clock_ticks < gps_t0) {
clock_delta = (0xFFFFFFFF - clock_ticks) + gps_time.t0 + 1;
} else {
clock_delta = clock_ticks - gps_time.t0;
}
time_delta = MSEC_OF_CPU_TICKS(clock_delta);
itow_now = gps_time.t0_tow + time_delta;
if (itow_now > MSEC_PER_WEEK) itow_now %= MSEC_PER_WEEK;
return itow_now;
}
#endif
/**
* Convert time in sys_time ticks to GPS time of week.
* The resolution depends on #SYS_TIME_RESOLUTION
* @return GPS tow in ms
*/
extern uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks);
#endif /* GPS_H */
sw/airborne/subsystems/gps/gps_mtk.c
+3 -5
View File
@@ -160,12 +160,10 @@ static void gps_mtk_time2itow(uint32_t gps_date, uint32_t gps_time,
void gps_mtk_read_message(void) {
if (gps_mtk.msg_class == MTK_DIY14_ID) {
if (gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
#ifdef GPS_TIMESTAMP
/* get hardware clock ticks */
SysTimeTimerStart(gps.t0);
gps.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
gps.t0_tow_frac = 0;
#endif
gps_time_sync.t0_ticks = sys_time.nb_tick;
gps_time_sync.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
gps_time_sync.t0_tow_frac = 0;
gps.lla_pos.lat = RadOfDeg(MTK_DIY14_NAV_LAT(gps_mtk.msg_buf))*10;
gps.lla_pos.lon = RadOfDeg(MTK_DIY14_NAV_LON(gps_mtk.msg_buf))*10;
// FIXME: with MTK you do not receive vertical speed
sw/airborne/subsystems/gps/gps_ubx.c
+3 -5
View File
@@ -93,12 +93,10 @@ void gps_ubx_read_message(void) {
if (gps_ubx.msg_class == UBX_NAV_ID) {
if (gps_ubx.msg_id == UBX_NAV_SOL_ID) {
#ifdef GPS_TIMESTAMP
/* get hardware clock ticks */
SysTimeTimerStart(gps.t0);
gps.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
gps.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf);
#endif
gps_time_sync.t0_ticks = sys_time.nb_tick;
gps_time_sync.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
gps_time_sync.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf);
gps.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
gps.week = UBX_NAV_SOL_week(gps_ubx.msg_buf);
gps.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf);
sw/airborne/subsystems/gps/gps_ubx.h
+4
View File
@@ -121,6 +121,10 @@ extern void gps_ubx_parse(uint8_t c);
#define CFG_RST_Reset_Controlled_GPS_stop 0x08
#define CFG_RST_Reset_Controlled_GPS_start 0x09
#define CFG_RST_BBR_Hotstart 0x0000
#define CFG_RST_BBR_Warmstart 0x0001
#define CFG_RST_BBR_Coldstart 0xffff
extern void ubxsend_cfg_rst(uint16_t, uint8_t);
#define gps_ubx_Reset(_val) { \