Skip to content

Add gps time and reduce logging verbosity. #33

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jgoppert wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Add gps time and reduce logging verbosity. #33

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 41 additions & 2 deletions drivers/sense/gnss/main.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
* SPDX-License-Identifier: Apache-2.0
*/

#include <time.h>

#include <zephyr/drivers/gnss.h>
#include <zros/private/zros_node_struct.h>
#include <zros/private/zros_pub_struct.h>
Expand All @@ -16,13 +18,37 @@
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(sense_gnss, CONFIG_ZROS_SENSE_GNSS_LOG_LEVEL);

/**
* @brief Convert GNSS UTC time to Unix epoch seconds
* @param utc Pointer to GNSS time structure
* @return Unix epoch seconds, or 0 if conversion fails
*/
static int64_t gnss_time_to_epoch(const struct gnss_time *utc)
{
struct tm tm = {
.tm_sec = utc->millisecond / 1000,
.tm_min = utc->minute,
.tm_hour = utc->hour,
.tm_mday = utc->month_day,
.tm_mon = utc->month - 1, /* tm_mon is 0-11 */
.tm_year = utc->century_year + 100, /* tm_year is years since 1900 */
.tm_isdst = 0,
};
time_t t = mktime(&tm);
if (t == (time_t)-1) {
return 0;
}
return (int64_t)t;
}

typedef struct context {
struct zros_node node;
struct zros_pub pub;
synapse_pb_NavSatFix data;
int32_t gMeasurementPeriodMs;
bool running;
bool isAlive;
bool hadFix;
} context_t;

#define GNSS_MODEM DEVICE_DT_GET(DT_ALIAS(gnss))
Expand All @@ -42,6 +68,7 @@ static context_t g_ctx = {
},
.running = false,
.isAlive = false,
.hadFix = false,
};

static void gnss_data_cb(const struct device *dev, const struct gnss_data *data)
Expand All @@ -53,17 +80,29 @@ static void gnss_data_cb(const struct device *dev, const struct gnss_data *data)
return;
}

stamp_msg(&g_ctx.data.stamp, k_uptime_ticks());
g_ctx.isAlive = true;

if (data->info.fix_status != GNSS_FIX_STATUS_NO_FIX) {
if (!g_ctx.hadFix) {
LOG_INF("GNSS fix acquired. SV tracked: %d", data->info.satellites_cnt);
g_ctx.hadFix = true;
}
/* Use actual GPS UTC time for timestamp */
g_ctx.data.stamp.seconds = gnss_time_to_epoch(&data->utc);
g_ctx.data.stamp.nanos = (data->utc.millisecond % 1000) * 1000000;

g_ctx.data.latitude = data->nav_data.latitude / 1e9;
g_ctx.data.longitude = data->nav_data.longitude / 1e9;
g_ctx.data.altitude = data->nav_data.altitude / 1e3;
zros_pub_update(&g_ctx.pub);
LOG_DBG("lat %f long %f\n", g_ctx.data.latitude, g_ctx.data.longitude);
} else {
LOG_WRN("gnss update without fix. SV tracked: %d", data->info.satellites_cnt);
if (g_ctx.hadFix) {
LOG_WRN("GNSS fix lost. SV tracked: %d", data->info.satellites_cnt);
}
g_ctx.hadFix = false;
LOG_INF_RATELIMIT_RATE(30000, "waiting for fix. SV tracked: %d",
data->info.satellites_cnt);
}
}
GNSS_DATA_CALLBACK_DEFINE(GNSS_MODEM, gnss_data_cb);
Expand Down
25 changes: 10 additions & 15 deletions drivers/sense/stream/imu/main.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -173,41 +173,36 @@ static void feed_calibration(struct context *ctx,
accel_mean[2] * accel_mean[2]);

if (accel_magnitude < 8.0 || accel_magnitude > 11.0) {
LOG_WRN("%s: accel magnitude out of range: %10.4f (expected ~9.8)",
LOG_WRN_RATELIMIT_RATE(5000, "%s: accel magnitude out of range: %10.4f (expected ~9.8)",
ctx->name, accel_magnitude);
calibration_ok = false;
}

for (size_t i = 0 ; i < ARRAY_SIZE(gyro_std) ; i++) {
if (gyro_std[i] > 0.1) {
LOG_WRN("%s: gyro axis %d too noisy: std=%10.4f", ctx->name,
LOG_WRN_RATELIMIT_RATE(5000, "%s: gyro axis %d too noisy: std=%10.4f", ctx->name,
i, gyro_std[i]);
calibration_ok = false;
}
}

if (!calibration_ok) {
LOG_WRN("%s: Calibration failed. Retrying...", ctx->name);
LOG_WRN_RATELIMIT_RATE(5000, "%s: Calibration failed. Retrying...", ctx->name);
ctx->calibration.state = SENSE_IMU_STREAM_UNCALIBRATED;
return;
}

LOG_INF("%s: Calibration completed", ctx->name);
ctx->calibration.bias.accel[0] = accel_mean[0];
ctx->calibration.bias.accel[1] = accel_mean[1];
ctx->calibration.bias.accel[2] = 0;
ctx->calibration.accel_scale = accel_magnitude / ACCEL_G;
LOG_INF("%s: accel", ctx->name);
LOG_INF("%s: mean: %10.4f %10.4f %10.4f", ctx->name,
accel_mean[0], accel_mean[1], accel_mean[2]);
LOG_INF("%s: std: %10.4f %10.4f %10.4f", ctx->name,
accel_std[0], accel_std[1], accel_std[2]);
LOG_INF("%s: scale %10.4f", ctx->name, ctx->calibration.accel_scale);

LOG_INF("%s gyro", ctx->name);
LOG_INF("%s mean: %10.4f %10.4f %10.4f", ctx->name,
gyro_mean[0], gyro_mean[1], gyro_mean[2]);
LOG_INF("%s: std: %10.4f %10.4f %10.4f", ctx->name,
LOG_INF("%s: Calibration completed (scale=%.3f)", ctx->name, ctx->calibration.accel_scale);
LOG_DBG("%s: accel mean: %7.4f %7.4f %7.4f std: %7.4f %7.4f %7.4f", ctx->name,
accel_mean[0], accel_mean[1], accel_mean[2],
accel_std[0], accel_std[1], accel_std[2]);
LOG_DBG("%s: gyro mean: %7.4f %7.4f %7.4f std: %7.4f %7.4f %7.4f", ctx->name,
gyro_mean[0], gyro_mean[1], gyro_mean[2],
gyro_std[0], gyro_std[1], gyro_std[2]);

for (int i = 0; i < ARRAY_SIZE(gyro_mean); i++) {
Expand Down Expand Up @@ -397,7 +392,7 @@ static void imu_stream_thread(void *arg0)
K_MSEC(100));
if (err != 0 || !ctx->running) {
ctx->running = false;
LOG_ERR("Error during stream. Attempting recovery...");
LOG_ERR_RATELIMIT_RATE(5000, "Error during stream. Attempting recovery...");
do {
/* TODO: Decide when we've tried too much. */
k_sleep(K_MSEC(100));
Expand Down
2 changes: 1 addition & 1 deletion drivers/sense/stream/mag/main.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -199,7 +199,7 @@ static void mag_stream_thread(void *arg0)
K_MSEC(1000));
if (err != 0 || !ctx->running) {
ctx->running = false;
LOG_ERR("Error during stream. Attempting recovery...");
LOG_ERR_RATELIMIT_RATE(5000, "Error during stream. Attempting recovery...");
do {
/* TODO: Decide when we've tried too much. */
k_sleep(K_MSEC(1000));
Expand Down