Rate limit corrected UART telemetry
This commit is contained in:
@@ -44,6 +44,9 @@ endif
|
||||
IMU_ACCEL_FS_G ?= 0
|
||||
IMU_GYRO_FS_DPS ?= 0
|
||||
GPS_ESKF_ENABLE ?= 1
|
||||
# UART telemetry cadence only; ESKF prediction still runs for every IMU sample.
|
||||
# At the default 50 Hz ODR, 5 samples produce one corrected record every 100 ms.
|
||||
IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES ?= 5
|
||||
|
||||
APP_SRCS := \
|
||||
gps_system.c \
|
||||
@@ -81,6 +84,7 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \
|
||||
-DIMU_ACCEL_FS_G=$(IMU_ACCEL_FS_G) \
|
||||
-DIMU_GYRO_FS_DPS=$(IMU_GYRO_FS_DPS) \
|
||||
-DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \
|
||||
-DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \
|
||||
-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS)
|
||||
LDLIBS += -lm
|
||||
|
||||
|
||||
@@ -198,9 +198,9 @@ bagimsiz yuksek-ODR zamanlama gerekirse sensor FIFO timestamp'i kullanilmalidir.
|
||||
|
||||
### Level-1 konum UART protokolu
|
||||
|
||||
Her gecerli GPS navigation epoch'unda level 1'de iki konum satiri art arda
|
||||
yayinlanir. Ilk satir WLS GPS konumu, ikinci satir o anda mevcut IMU+ESKF
|
||||
duzeltilmis konumdur:
|
||||
Her gecerli GPS navigation epoch'unda ham WLS GPS konumu yayinlanir. ESKF
|
||||
duzeltilmis konumu ise GPS epoch'undan bagimsiz olarak IMU ornek sayaci
|
||||
uzerinden yayinlanir:
|
||||
|
||||
```text
|
||||
[LOCATION] source=gps lat=... lon=... height=... gdop=... pdop=... hdop=... vdop=... tdop=... tracked=... used=...
|
||||
@@ -208,10 +208,28 @@ duzeltilmis konumdur:
|
||||
```
|
||||
|
||||
`tracked`, donanimda calisan tracking kanal sayisini; `used`, ilgili WLS
|
||||
cozumunde kullanilan uydu sayisini belirtir. DOP alanlari ayni GPS epoch'unun
|
||||
geometrisidir ve iki satirda ayni tasinir. ESKF henuz kalibrasyon/origin
|
||||
asamasindaysa yalniz GPS satiri yayinlanir; ESKF valid olduktan sonra her GPS
|
||||
konumu geldiginde iki satir da yayinlanir. Eski `[NAV] position` ve periyodik
|
||||
cozumunde kullanilan uydu sayisini belirtir. ESKF henuz kalibrasyon/origin
|
||||
asamasindaysa yalniz GPS satiri yayinlanir. Origin kurulduktan sonra GPS
|
||||
reddedilse veya gecici olarak kesilse de corrected konum IMU ile ilerlemeye
|
||||
devam eder.
|
||||
|
||||
Varsayilan corrected UART periyodu bes IMU ornegidir. 50 Hz sensor hedefinde
|
||||
bu yaklasik 100 ms/10 Hz demektir. Bu bolucu yalniz UART logunu seyreltir;
|
||||
sensor okuma ve ESKF prediction her IMU orneginde calismaya devam eder:
|
||||
|
||||
```sh
|
||||
# Varsayilan: 20 ms x 5 ~= 100 ms
|
||||
make -C soc/software/gps_system IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=5
|
||||
|
||||
# 20 ms x 2 ~= 40 ms
|
||||
make -C soc/software/gps_system IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=2
|
||||
|
||||
# Parent software hedefi uzerinden ayni ayar
|
||||
make -C soc/software gps-system GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES=5
|
||||
```
|
||||
|
||||
Deger en az `1` olmalidir. Secilen bolucu startup satirinda
|
||||
`publish_every=` alaniyla gorulur. Eski `[NAV] position` ve periyodik
|
||||
`[FUSION] lat/lon` ayrintilari daha yuksek debug seviyesindedir; level 1
|
||||
parser'inin ayni konumu birden fazla kez gormesi engellenir.
|
||||
|
||||
|
||||
@@ -18,6 +18,14 @@
|
||||
#error "GPS_ESKF_ENABLE must be 0 or 1"
|
||||
#endif
|
||||
|
||||
#ifndef IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES
|
||||
#define IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES 5u
|
||||
#endif
|
||||
|
||||
#if (IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES < 1)
|
||||
#error "IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES must be at least one"
|
||||
#endif
|
||||
|
||||
#define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u
|
||||
#define IMU_SAMPLES_AFTER_GPS_SERVICE 3u
|
||||
|
||||
@@ -67,10 +75,20 @@ static unsigned int feed_pending_imu_to_eskf(
|
||||
(system->fused_solution.timestamp_ticks !=
|
||||
system->last_fused_location_tick))
|
||||
{
|
||||
gps_system_uart_publish_fused_location(
|
||||
&system->fused_solution);
|
||||
system->last_fused_location_tick =
|
||||
system->fused_solution.timestamp_ticks;
|
||||
system->fused_publish_sample_count++;
|
||||
bool first_location =
|
||||
(system->last_fused_location_tick == 0u);
|
||||
bool publish_due =
|
||||
(system->fused_publish_sample_count >=
|
||||
IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES);
|
||||
if(first_location || publish_due)
|
||||
{
|
||||
gps_system_uart_publish_fused_location(
|
||||
&system->fused_solution);
|
||||
system->last_fused_location_tick =
|
||||
system->fused_solution.timestamp_ticks;
|
||||
system->fused_publish_sample_count = 0u;
|
||||
}
|
||||
}
|
||||
processed++;
|
||||
}
|
||||
@@ -93,7 +111,8 @@ int main(void)
|
||||
gps_receiver_start_acquisition();
|
||||
gps_system_uart_publish_startup(
|
||||
system.fusion_enabled, system.fusion_initialized,
|
||||
system.imu_initialized, system.imu_running);
|
||||
system.imu_initialized, system.imu_running,
|
||||
IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES);
|
||||
|
||||
bool imu_backlog_may_remain = false;
|
||||
for(;;)
|
||||
|
||||
@@ -21,7 +21,8 @@ void gps_system_uart_publish_startup(
|
||||
bool fusion_enabled,
|
||||
bool fusion_initialized,
|
||||
bool imu_initialized,
|
||||
bool imu_running);
|
||||
bool imu_running,
|
||||
unsigned int fused_publish_every_samples);
|
||||
|
||||
void gps_system_uart_publish_periodic_fusion_status(
|
||||
navigation_fusion_output_t* output,
|
||||
|
||||
@@ -19,6 +19,7 @@ typedef struct {
|
||||
uint64_t gps_measurement_ticks;
|
||||
uint64_t last_fused_location_tick;
|
||||
uint64_t last_status_tick;
|
||||
uint32_t fused_publish_sample_count;
|
||||
bool gps_epoch_ready;
|
||||
bool fusion_enabled;
|
||||
bool imu_initialized;
|
||||
|
||||
@@ -47,8 +47,8 @@ void gps_system_uart_publish_fused_location(
|
||||
(output->gps_accepted > 0u) && (age_ms <= GPS_AIDED_MAX_AGE_MS)
|
||||
? "gps_aided" : "dead_reckoning";
|
||||
|
||||
/* At 50 Hz this compact record uses about 14% of the 460800-baud link,
|
||||
* leaving headroom for navigation work and lower-rate diagnostics. */
|
||||
/* The application rate-limits this compact record by IMU sample count,
|
||||
* leaving UART headroom without reducing the ESKF prediction rate. */
|
||||
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
|
||||
"source=imu_corrected mode=%s lat=%.9lf lon=%.9lf "
|
||||
"height=%.2lf gps_age_ms=%lu\r\n",
|
||||
@@ -60,13 +60,15 @@ void gps_system_uart_publish_startup(
|
||||
bool fusion_enabled,
|
||||
bool fusion_initialized,
|
||||
bool imu_initialized,
|
||||
bool imu_running)
|
||||
bool imu_running,
|
||||
unsigned int fused_publish_every_samples)
|
||||
{
|
||||
imu_sensor_diagnostics_t imu;
|
||||
imu_sensor_get_diagnostics(&imu);
|
||||
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
|
||||
"init eskf_enabled=%u eskf_ready=%u imu=%u sensor=%s who=0x%02x "
|
||||
"running=%u target_hz=%lu actual_hz=%lu nav_ms=%u timebase_hz=%lu\r\n",
|
||||
"running=%u target_hz=%lu actual_hz=%lu publish_every=%u "
|
||||
"nav_ms=%u timebase_hz=%lu\r\n",
|
||||
fusion_enabled ? 1u : 0u,
|
||||
fusion_initialized ? 1u : 0u,
|
||||
imu_initialized ? 1u : 0u,
|
||||
@@ -75,6 +77,7 @@ void gps_system_uart_publish_startup(
|
||||
imu_running ? 1u : 0u,
|
||||
(unsigned long)imu.requested_odr_hz,
|
||||
(unsigned long)imu.configured_odr_hz,
|
||||
fused_publish_every_samples,
|
||||
GPS_NAV_SOLVE_PERIOD_MS,
|
||||
(unsigned long)BSP_CLINT_TIMEBASE_HZ);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user