From 708ae8e8d857807eda42accf762fe4bd4d9b5d2b Mon Sep 17 00:00:00 2001 From: SPRESENSE <41312067+SPRESENSE@users.noreply.github.com> Date: Thu, 25 Jan 2024 20:22:30 +0900 Subject: [PATCH] boards: cxd56xx: Add geoid to cxd5610 gnss driver Support the output of geoidal heights for cxd5610 gnss driver. --- boards/arm/cxd56xx/drivers/sensors/cxd5610_gnss.c | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/boards/arm/cxd56xx/drivers/sensors/cxd5610_gnss.c b/boards/arm/cxd56xx/drivers/sensors/cxd5610_gnss.c index 8be65863e2e..b0c29804979 100644 --- a/boards/arm/cxd56xx/drivers/sensors/cxd5610_gnss.c +++ b/boards/arm/cxd56xx/drivers/sensors/cxd5610_gnss.c @@ -229,6 +229,7 @@ begin_packed_struct struct cmd_notify_pos_s int32_t lat32; int32_t lon32; int32_t alt32; + int16_t geo16; } end_packed_struct; begin_packed_struct struct cmd_notify_vel_s @@ -494,6 +495,7 @@ static int cxd5610_gnss_notify_pos(struct cxd5610_gnss_dev_s *priv, int len) int32_t lat32; int32_t lon32; int32_t alt32; + int16_t geo16 = 0; /* If the packet is invalid, do not update received data */ @@ -511,10 +513,15 @@ static int cxd5610_gnss_notify_pos(struct cxd5610_gnss_dev_s *priv, int len) cxd5610_store32(&lat32, ¶m->lat32); cxd5610_store32(&lon32, ¶m->lon32); cxd5610_store32(&alt32, ¶m->alt32); + if (GET_PACKET_VERSION(param->ver8) > 0) + { + cxd5610_store16(&geo16, ¶m->geo16); + } + receiver->latitude = (double)lat32 / 10000000.0; receiver->longitude = (double)lon32 / 10000000.0; receiver->altitude = (double)alt32 / 100.0; - receiver->geoid = 0.0; + receiver->geoid = (double)geo16 / 100.0; receiver->svtype = (param->mode >> 4); receiver->fix_indicator = (param->mode & 0xf); @@ -523,10 +530,12 @@ static int cxd5610_gnss_notify_pos(struct cxd5610_gnss_dev_s *priv, int len) receiver->pos_dataexist = 1; } - sninfo("lat=%.7lf[deg] lon=%.7lf[deg] alt=%.2lf[m] svtype=%d fix=%d\n", + sninfo("lat=%.7lf[deg] lon=%.7lf[deg] alt=%.2lf[m] " + "geo=%.2lf[m] svtype=%d fix=%d\n", receiver->latitude, receiver->longitude, receiver->altitude, + receiver->geoid, receiver->svtype, receiver->fix_indicator);