From bc2ec5e94c67a286551a64eb7bc17498526479e7 Mon Sep 17 00:00:00 2001 From: wangjianyu3 Date: Sat, 30 Nov 2024 07:54:33 +0800 Subject: [PATCH 1/3] drivers/sensors/gnss: Different nbuffer for each topic Signed-off-by: wangjianyu3 --- drivers/sensors/gnss_uorb.c | 75 +++++++++++++++++++----------------- include/nuttx/sensors/gnss.h | 10 ++++- 2 files changed, 48 insertions(+), 37 deletions(-) diff --git a/drivers/sensors/gnss_uorb.c b/drivers/sensors/gnss_uorb.c index 4ca9e33ffc61a..2353b786fcdcd 100644 --- a/drivers/sensors/gnss_uorb.c +++ b/drivers/sensors/gnss_uorb.c @@ -45,13 +45,6 @@ #define GNSS_PATH_FMT "/dev/ttyGNSS%d" -#define GNSS_IDX 0 -#define GNSS_SATELLITE_IDX 1 -#define GNSS_MEASUREMENT_IDX 2 -#define GNSS_CLOCK_IDX 3 -#define GNSS_GEOFENCE_IDX 4 -#define GNSS_MAX_IDX 5 - #define GNSS_PARSE_BUFFERSIZE 256 #define GNSS_KNOT_TO_KMH 1.852f @@ -86,7 +79,7 @@ struct gnss_user_s struct gnss_upperhalf_s { - struct gnss_sensor_s dev[GNSS_MAX_IDX]; + struct gnss_sensor_s dev[SENSOR_GNSS_IDX_GNSS_MAX]; struct list_node userlist; FAR struct gnss_lowerhalf_s *lower; uint8_t crefs; @@ -537,7 +530,7 @@ static void gnss_parse_nmea(FAR struct gnss_upperhalf_s *upper, satellite.satellites = frame.total_sats; memcpy(satellite.info, frame.sats, sizeof(satellite.info[0]) * 4); - lower = &upper->dev[GNSS_SATELLITE_IDX].lower; + lower = &upper->dev[SENSOR_GNSS_IDX_GNSS_SATELLITE].lower; for (i = 0; i < nitems(g_gnss_constellation); i++) { @@ -572,7 +565,7 @@ static void gnss_parse_nmea(FAR struct gnss_upperhalf_s *upper, if (GNSS_FLAG_MARK == upper->flags) { upper->flags &= ~GNSS_FLAG_MARK; - lower = &upper->dev[GNSS_IDX].lower; + lower = &upper->dev[SENSOR_GNSS_IDX_GNSS].lower; lower->push_event(lower->priv, &upper->gnss, sizeof(upper->gnss)); gnss_init_data(&upper->gnss); } @@ -660,27 +653,27 @@ static void gnss_push_event(FAR void *priv, FAR const void *data, if (type == SENSOR_TYPE_GNSS) { - lower = &upper->dev[GNSS_IDX].lower; + lower = &upper->dev[SENSOR_GNSS_IDX_GNSS].lower; lower->push_event(lower->priv, data, bytes); } else if (type == SENSOR_TYPE_GNSS_SATELLITE) { - lower = &upper->dev[GNSS_SATELLITE_IDX].lower; + lower = &upper->dev[SENSOR_GNSS_IDX_GNSS_SATELLITE].lower; lower->push_event(lower->priv, data, bytes); } else if (type == SENSOR_TYPE_GNSS_MEASUREMENT) { - lower = &upper->dev[GNSS_MEASUREMENT_IDX].lower; + lower = &upper->dev[SENSOR_GNSS_IDX_GNSS_MEASUREMENT].lower; lower->push_event(lower->priv, data, bytes); } else if (type == SENSOR_TYPE_GNSS_CLOCK) { - lower = &upper->dev[GNSS_CLOCK_IDX].lower; + lower = &upper->dev[SENSOR_GNSS_IDX_GNSS_CLOCK].lower; lower->push_event(lower->priv, data, bytes); } else if (type == SENSOR_TYPE_GNSS_GEOFENCE) { - lower = &upper->dev[GNSS_GEOFENCE_IDX].lower; + lower = &upper->dev[SENSOR_GNSS_IDX_GNSS_GEOFENCE].lower; lower->push_event(lower->priv, data, bytes); } } @@ -704,6 +697,7 @@ static void gnss_push_event(FAR void *priv, FAR const void *data, * devno - The user specifies which device of this type, from 0. If the * devno alerady exists, -EEXIST will be returned. * nbuffer - The number of events that the circular buffer can hold. + * count - The array size of nbuffer. * * Returned Value: * OK if the driver was successfully register; A negated errno value is @@ -712,13 +706,18 @@ static void gnss_push_event(FAR void *priv, FAR const void *data, ****************************************************************************/ int gnss_register(FAR struct gnss_lowerhalf_s *lower, int devno, - uint32_t nbuffer) + uint32_t nbuffer[], size_t count) { FAR struct gnss_upperhalf_s *upper; FAR struct gnss_sensor_s *dev; FAR char *path; int ret; + if (count != SENSOR_GNSS_IDX_GNSS_MAX) + { + return -EINVAL; + } + upper = kmm_zalloc(sizeof(struct gnss_upperhalf_s)); if (upper == NULL) { @@ -745,10 +744,10 @@ int gnss_register(FAR struct gnss_lowerhalf_s *lower, int devno, /* GNSS register */ - dev = &upper->dev[GNSS_IDX]; + dev = &upper->dev[SENSOR_GNSS_IDX_GNSS]; dev->lower.ops = &g_gnss_sensor_ops; dev->lower.type = SENSOR_TYPE_GNSS; - dev->lower.nbuffer = nbuffer; + dev->lower.nbuffer = nbuffer[SENSOR_GNSS_IDX_GNSS]; dev->upper = upper; ret = sensor_register(&dev->lower, devno); if (ret < 0) @@ -758,10 +757,10 @@ int gnss_register(FAR struct gnss_lowerhalf_s *lower, int devno, /* Satellite register */ - dev = &upper->dev[GNSS_SATELLITE_IDX]; + dev = &upper->dev[SENSOR_GNSS_IDX_GNSS_SATELLITE]; dev->lower.ops = &g_gnss_sensor_ops; dev->lower.type = SENSOR_TYPE_GNSS_SATELLITE; - dev->lower.nbuffer = nbuffer; + dev->lower.nbuffer = nbuffer[SENSOR_GNSS_IDX_GNSS_SATELLITE]; dev->upper = upper; ret = sensor_register(&dev->lower, devno); if (ret < 0) @@ -771,10 +770,10 @@ int gnss_register(FAR struct gnss_lowerhalf_s *lower, int devno, /* GNSS Measurement register */ - dev = &upper->dev[GNSS_MEASUREMENT_IDX]; + dev = &upper->dev[SENSOR_GNSS_IDX_GNSS_MEASUREMENT]; dev->lower.ops = &g_gnss_sensor_ops; dev->lower.type = SENSOR_TYPE_GNSS_MEASUREMENT; - dev->lower.nbuffer = nbuffer; + dev->lower.nbuffer = nbuffer[SENSOR_GNSS_IDX_GNSS_MEASUREMENT]; dev->upper = upper; ret = sensor_register(&dev->lower, devno); if (ret < 0) @@ -784,10 +783,10 @@ int gnss_register(FAR struct gnss_lowerhalf_s *lower, int devno, /* GNSS Colck register */ - dev = &upper->dev[GNSS_CLOCK_IDX]; + dev = &upper->dev[SENSOR_GNSS_IDX_GNSS_CLOCK]; dev->lower.ops = &g_gnss_sensor_ops; dev->lower.type = SENSOR_TYPE_GNSS_CLOCK; - dev->lower.nbuffer = nbuffer; + dev->lower.nbuffer = nbuffer[SENSOR_GNSS_IDX_GNSS_CLOCK]; dev->upper = upper; ret = sensor_register(&dev->lower, devno); if (ret < 0) @@ -797,10 +796,10 @@ int gnss_register(FAR struct gnss_lowerhalf_s *lower, int devno, /* GNSS Geofence */ - dev = &upper->dev[GNSS_GEOFENCE_IDX]; + dev = &upper->dev[SENSOR_GNSS_IDX_GNSS_GEOFENCE]; dev->lower.ops = &g_gnss_sensor_ops; dev->lower.type = SENSOR_TYPE_GNSS_GEOFENCE; - dev->lower.nbuffer = nbuffer; + dev->lower.nbuffer = nbuffer[SENSOR_GNSS_IDX_GNSS_GEOFENCE]; dev->upper = upper; ret = sensor_register(&dev->lower, devno); if (ret < 0) @@ -828,15 +827,17 @@ int gnss_register(FAR struct gnss_lowerhalf_s *lower, int devno, driver_err: circbuf_uninit(&upper->buffer); circ_err: - sensor_unregister(&upper->dev[GNSS_GEOFENCE_IDX].lower, devno); + sensor_unregister(&upper->dev[SENSOR_GNSS_IDX_GNSS_GEOFENCE].lower, devno); gnss_geofence_err: - sensor_unregister(&upper->dev[GNSS_CLOCK_IDX].lower, devno); + sensor_unregister(&upper->dev[SENSOR_GNSS_IDX_GNSS_CLOCK].lower, devno); gnss_clock_err: - sensor_unregister(&upper->dev[GNSS_MEASUREMENT_IDX].lower, devno); + sensor_unregister(&upper->dev[SENSOR_GNSS_IDX_GNSS_MEASUREMENT].lower, + devno); gnss_measurement_err: - sensor_unregister(&upper->dev[GNSS_SATELLITE_IDX].lower, devno); + sensor_unregister(&upper->dev[SENSOR_GNSS_IDX_GNSS_SATELLITE].lower, + devno); satellite_err: - sensor_unregister(&upper->dev[GNSS_IDX].lower, devno); + sensor_unregister(&upper->dev[SENSOR_GNSS_IDX_GNSS].lower, devno); gnss_err: nxmutex_destroy(&upper->lock); nxmutex_destroy(&upper->bufferlock); @@ -872,11 +873,13 @@ void gnss_unregister(FAR struct gnss_lowerhalf_s *lower, int devno) return; } - sensor_unregister(&upper->dev[GNSS_IDX].lower, devno); - sensor_unregister(&upper->dev[GNSS_SATELLITE_IDX].lower, devno); - sensor_unregister(&upper->dev[GNSS_MEASUREMENT_IDX].lower, devno); - sensor_unregister(&upper->dev[GNSS_CLOCK_IDX].lower, devno); - sensor_unregister(&upper->dev[GNSS_GEOFENCE_IDX].lower, devno); + sensor_unregister(&upper->dev[SENSOR_GNSS_IDX_GNSS].lower, devno); + sensor_unregister(&upper->dev[SENSOR_GNSS_IDX_GNSS_SATELLITE].lower, + devno); + sensor_unregister(&upper->dev[SENSOR_GNSS_IDX_GNSS_MEASUREMENT].lower, + devno); + sensor_unregister(&upper->dev[SENSOR_GNSS_IDX_GNSS_CLOCK].lower, devno); + sensor_unregister(&upper->dev[SENSOR_GNSS_IDX_GNSS_GEOFENCE].lower, devno); snprintf(path, PATH_MAX, GNSS_PATH_FMT, devno); unregister_driver(path); nxsem_destroy(&upper->buffersem); diff --git a/include/nuttx/sensors/gnss.h b/include/nuttx/sensors/gnss.h index d7daa4521957d..5c1bfe8158740 100644 --- a/include/nuttx/sensors/gnss.h +++ b/include/nuttx/sensors/gnss.h @@ -33,6 +33,13 @@ * Pre-processor Definitions ****************************************************************************/ +#define SENSOR_GNSS_IDX_GNSS 0 +#define SENSOR_GNSS_IDX_GNSS_SATELLITE 1 +#define SENSOR_GNSS_IDX_GNSS_MEASUREMENT 2 +#define SENSOR_GNSS_IDX_GNSS_CLOCK 3 +#define SENSOR_GNSS_IDX_GNSS_GEOFENCE 4 +#define SENSOR_GNSS_IDX_GNSS_MAX 5 + /**************************************************************************** * Public Types ****************************************************************************/ @@ -202,6 +209,7 @@ extern "C" * devno - The user specifies which device of this type, from 0. If the * devno alerady exists, -EEXIST will be returned. * nbuffer - The number of events that the circular buffer can hold. + * count - The array size of nbuffer. * * Returned Value: * OK if the driver was successfully register; A negated errno value is @@ -210,7 +218,7 @@ extern "C" ****************************************************************************/ int gnss_register(FAR struct gnss_lowerhalf_s *dev, int devno, - uint32_t nbuffer); + uint32_t nbuffer[], size_t count); /**************************************************************************** * Name: gnss_unregister From 8039963e99728ca4f905e687676bcbafeaeca7cd Mon Sep 17 00:00:00 2001 From: wangjianyu3 Date: Sat, 30 Nov 2024 08:20:22 +0800 Subject: [PATCH 2/3] nrf91: Support using different nbuffer for each topic Signed-off-by: wangjianyu3 --- arch/arm/src/nrf91/nrf91_modem_gnss.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/arch/arm/src/nrf91/nrf91_modem_gnss.c b/arch/arm/src/nrf91/nrf91_modem_gnss.c index b88a1baaffcae..72d9c40eda8b2 100644 --- a/arch/arm/src/nrf91/nrf91_modem_gnss.c +++ b/arch/arm/src/nrf91/nrf91_modem_gnss.c @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -689,6 +690,13 @@ static int nrf91_gnss_thread(int argc, char** argv) int nrf91_gnss_register(int devno, uint32_t batch_number) { int ret = OK; + uint32_t nbuffer[] = { + [SENSOR_GNSS_IDX_GNSS] = batch_number, + [SENSOR_GNSS_IDX_GNSS_SATELLITE] = batch_number, + [SENSOR_GNSS_IDX_GNSS_MEASUREMENT] = batch_number, + [SENSOR_GNSS_IDX_GNSS_CLOCK] = batch_number, + [SENSOR_GNSS_IDX_GNSS_GEOFENCE] = batch_number, + }; if (!nrf_modem_is_initialized()) { @@ -728,5 +736,5 @@ int nrf91_gnss_register(int devno, uint32_t batch_number) g_nrf91_gnss.lower.ops = &g_nrf91_gnss_ops; - return gnss_register(&g_nrf91_gnss.lower, devno, batch_number); + return gnss_register(&g_nrf91_gnss.lower, devno, nbuffer, nitems(nbuffer)); } From 6ff177043cf50f07b18cf39c6be539ba1723076f Mon Sep 17 00:00:00 2001 From: wangjianyu3 Date: Sat, 30 Nov 2024 08:40:11 +0800 Subject: [PATCH 3/3] sensors/fakesensor_uorb: Using different nbuffer for each topic of GNSS Signed-off-by: wangjianyu3 --- drivers/sensors/fakesensor_uorb.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/drivers/sensors/fakesensor_uorb.c b/drivers/sensors/fakesensor_uorb.c index ef8e50fad1307..843347b6adf89 100644 --- a/drivers/sensors/fakesensor_uorb.c +++ b/drivers/sensors/fakesensor_uorb.c @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -401,6 +402,14 @@ int fakesensor_init(int type, FAR const char *file_name, FAR struct fakesensor_s *sensor; FAR char *argv[2]; char arg1[32]; + uint32_t nbuffer[] = { + [SENSOR_GNSS_IDX_GNSS] = batch_number, + [SENSOR_GNSS_IDX_GNSS_SATELLITE] = batch_number, + [SENSOR_GNSS_IDX_GNSS_MEASUREMENT] = batch_number, + [SENSOR_GNSS_IDX_GNSS_CLOCK] = batch_number, + [SENSOR_GNSS_IDX_GNSS_GEOFENCE] = batch_number, + }; + int ret; /* Alloc memory for sensor */ @@ -436,7 +445,7 @@ int fakesensor_init(int type, FAR const char *file_name, if (type == SENSOR_TYPE_GNSS || type == SENSOR_TYPE_GNSS_SATELLITE) { sensor->gnss.ops = &g_fakegnss_ops; - gnss_register(&sensor->gnss, devno, batch_number); + gnss_register(&sensor->gnss, devno, nbuffer, nitems(nbuffer)); } else {