diff --git a/drivers/sensors/gnss_uorb.c b/drivers/sensors/gnss_uorb.c index 4ca9e33ffc6..2353b786fcd 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 d7daa452195..5c1bfe81587 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