From bf83b3fafc03b41b2a034cab5a0895a2c1fab7d9 Mon Sep 17 00:00:00 2001 From: wangjianyu3 Date: Thu, 20 Jun 2024 23:44:12 +0800 Subject: [PATCH] driver/sensors: Support GNSS Measurement & Clock Signed-off-by: wangjianyu3 --- drivers/sensors/gnss_uorb.c | 46 +++++++++- drivers/sensors/sensor.c | 72 ++++++++------- include/nuttx/sensors/gnss.h | 1 + include/nuttx/uorb.h | 173 ++++++++++++++++++++++++++++++++++- 4 files changed, 255 insertions(+), 37 deletions(-) diff --git a/drivers/sensors/gnss_uorb.c b/drivers/sensors/gnss_uorb.c index 082ca031eb1..1085af352e4 100644 --- a/drivers/sensors/gnss_uorb.c +++ b/drivers/sensors/gnss_uorb.c @@ -43,7 +43,9 @@ #define GNSS_IDX 0 #define GNSS_SATELLITE_IDX 1 -#define GNSS_MAX_IDX 2 +#define GNSS_MEASUREMENT_IDX 2 +#define GNSS_CLOCK_IDX 3 +#define GNSS_MAX_IDX 4 #define GNSS_PARSE_BUFFERSIZE 256 @@ -643,6 +645,16 @@ static void gnss_push_event(FAR void *priv, FAR const void *data, lower = &upper->dev[GNSS_SATELLITE_IDX].lower; lower->push_event(lower->priv, data, bytes); } + else if (type == SENSOR_TYPE_GNSS_MEASUREMENT) + { + lower = &upper->dev[GNSS_MEASUREMENT_IDX].lower; + lower->push_event(lower->priv, data, bytes); + } + else if (type == SENSOR_TYPE_GNSS_CLOCK) + { + lower = &upper->dev[GNSS_CLOCK_IDX].lower; + lower->push_event(lower->priv, data, bytes); + } nxmutex_unlock(&upper->lock); } @@ -722,6 +734,32 @@ int gnss_register(FAR struct gnss_lowerhalf_s *lower, int devno, goto satellite_err; } + /* GNSS Measurement register */ + + dev = &upper->dev[GNSS_MEASUREMENT_IDX]; + dev->lower.ops = &g_gnss_sensor_ops; + dev->lower.type = SENSOR_TYPE_GNSS_MEASUREMENT; + dev->lower.nbuffer = nbuffer; + dev->upper = upper; + ret = sensor_register(&dev->lower, devno); + if (ret < 0) + { + goto gnss_measurement_err; + } + + /* GNSS Colck register */ + + dev = &upper->dev[GNSS_CLOCK_IDX]; + dev->lower.ops = &g_gnss_sensor_ops; + dev->lower.type = SENSOR_TYPE_GNSS_CLOCK; + dev->lower.nbuffer = nbuffer; + dev->upper = upper; + ret = sensor_register(&dev->lower, devno); + if (ret < 0) + { + goto gnss_clock_err; + } + ret = circbuf_init(&upper->buffer, NULL, CONFIG_SENSORS_GNSS_RECV_BUFFERSIZE); if (ret < 0) @@ -741,6 +779,10 @@ int gnss_register(FAR struct gnss_lowerhalf_s *lower, int devno, driver_err: circbuf_uninit(&upper->buffer); circ_err: + sensor_unregister(&upper->dev[GNSS_CLOCK_IDX].lower, devno); +gnss_clock_err: + sensor_unregister(&upper->dev[GNSS_MEASUREMENT_IDX].lower, devno); +gnss_measurement_err: sensor_unregister(&upper->dev[GNSS_SATELLITE_IDX].lower, devno); satellite_err: sensor_unregister(&upper->dev[GNSS_IDX].lower, devno); @@ -772,6 +814,8 @@ void gnss_unregister(FAR struct gnss_lowerhalf_s *lower, int devno) 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); snprintf(path, PATH_MAX, GNSS_PATH_FMT, devno); unregister_driver(path); nxsem_destroy(&upper->buffersem); diff --git a/drivers/sensors/sensor.c b/drivers/sensors/sensor.c index 3603cffd6ff..647e11f449e 100644 --- a/drivers/sensors/sensor.c +++ b/drivers/sensors/sensor.c @@ -158,41 +158,43 @@ static const struct sensor_axis_map_s g_remap_tbl[] = static const struct sensor_meta_s g_sensor_meta[] = { - {0, NULL}, - {sizeof(struct sensor_accel), "accel"}, - {sizeof(struct sensor_mag), "mag"}, - {sizeof(struct sensor_gyro), "gyro"}, - {sizeof(struct sensor_light), "light"}, - {sizeof(struct sensor_baro), "baro"}, - {sizeof(struct sensor_prox), "prox"}, - {sizeof(struct sensor_humi), "humi"}, - {sizeof(struct sensor_temp), "temp"}, - {sizeof(struct sensor_rgb), "rgb"}, - {sizeof(struct sensor_hall), "hall"}, - {sizeof(struct sensor_ir), "ir"}, - {sizeof(struct sensor_uv), "uv"}, - {sizeof(struct sensor_noise), "noise"}, - {sizeof(struct sensor_pm25), "pm25"}, - {sizeof(struct sensor_pm1p0), "pm1p0"}, - {sizeof(struct sensor_pm10), "pm10"}, - {sizeof(struct sensor_co2), "co2"}, - {sizeof(struct sensor_hcho), "hcho"}, - {sizeof(struct sensor_tvoc), "tvoc"}, - {sizeof(struct sensor_ph), "ph"}, - {sizeof(struct sensor_dust), "dust"}, - {sizeof(struct sensor_hrate), "hrate"}, - {sizeof(struct sensor_hbeat), "hbeat"}, - {sizeof(struct sensor_ecg), "ecg"}, - {sizeof(struct sensor_ppgd), "ppgd"}, - {sizeof(struct sensor_ppgq), "ppgq"}, - {sizeof(struct sensor_impd), "impd"}, - {sizeof(struct sensor_ots), "ots"}, - {sizeof(struct sensor_wake_gesture), "wake_gesture"}, - {sizeof(struct sensor_cap), "cap"}, - {sizeof(struct sensor_gas), "gas"}, - {sizeof(struct sensor_force), "force"}, - {sizeof(struct sensor_gnss), "gnss"}, - {sizeof(struct sensor_gnss_satellite), "gnss_satellite"}, + {0, NULL}, + {sizeof(struct sensor_accel), "accel"}, + {sizeof(struct sensor_mag), "mag"}, + {sizeof(struct sensor_gyro), "gyro"}, + {sizeof(struct sensor_light), "light"}, + {sizeof(struct sensor_baro), "baro"}, + {sizeof(struct sensor_prox), "prox"}, + {sizeof(struct sensor_humi), "humi"}, + {sizeof(struct sensor_temp), "temp"}, + {sizeof(struct sensor_rgb), "rgb"}, + {sizeof(struct sensor_hall), "hall"}, + {sizeof(struct sensor_ir), "ir"}, + {sizeof(struct sensor_uv), "uv"}, + {sizeof(struct sensor_noise), "noise"}, + {sizeof(struct sensor_pm25), "pm25"}, + {sizeof(struct sensor_pm1p0), "pm1p0"}, + {sizeof(struct sensor_pm10), "pm10"}, + {sizeof(struct sensor_co2), "co2"}, + {sizeof(struct sensor_hcho), "hcho"}, + {sizeof(struct sensor_tvoc), "tvoc"}, + {sizeof(struct sensor_ph), "ph"}, + {sizeof(struct sensor_dust), "dust"}, + {sizeof(struct sensor_hrate), "hrate"}, + {sizeof(struct sensor_hbeat), "hbeat"}, + {sizeof(struct sensor_ecg), "ecg"}, + {sizeof(struct sensor_ppgd), "ppgd"}, + {sizeof(struct sensor_ppgq), "ppgq"}, + {sizeof(struct sensor_impd), "impd"}, + {sizeof(struct sensor_ots), "ots"}, + {sizeof(struct sensor_wake_gesture), "wake_gesture"}, + {sizeof(struct sensor_cap), "cap"}, + {sizeof(struct sensor_gas), "gas"}, + {sizeof(struct sensor_force), "force"}, + {sizeof(struct sensor_gnss), "gnss"}, + {sizeof(struct sensor_gnss_satellite), "gnss_satellite"}, + {sizeof(struct sensor_gnss_measurement), "gnss_measurement"}, + {sizeof(struct sensor_gnss_clock), "gnss_clock"}, }; static const struct file_operations g_sensor_fops = diff --git a/include/nuttx/sensors/gnss.h b/include/nuttx/sensors/gnss.h index a3304a81e62..f926f19d8ec 100644 --- a/include/nuttx/sensors/gnss.h +++ b/include/nuttx/sensors/gnss.h @@ -163,6 +163,7 @@ struct gnss_lowerhalf_s * It is provided by upper half driver to lower half driver. * lower half can use type to description the data type, eg: * SENSOR_TYPE_GNSS, SENSOR_TYPE_GNSS_SATELLITE + * SENSOR_TYPE_GNSS_MEASUREMENT, SENSOR_TYPE_GNSS_CLOCK */ gnss_push_event_t push_event; diff --git a/include/nuttx/uorb.h b/include/nuttx/uorb.h index 0407a8e99da..9025f25bb26 100644 --- a/include/nuttx/uorb.h +++ b/include/nuttx/uorb.h @@ -305,9 +305,17 @@ #define SENSOR_TYPE_GNSS_SATELLITE 34 +/* GNSS Measurement */ + +#define SENSOR_TYPE_GNSS_MEASUREMENT 35 + +/* GNSS Clock */ + +#define SENSOR_TYPE_GNSS_CLOCK 36 + /* The total number of sensor */ -#define SENSOR_TYPE_COUNT 35 +#define SENSOR_TYPE_COUNT 37 /* The additional sensor open flags */ @@ -326,6 +334,48 @@ #define SENSOR_EVENT_FLUSH_COMPLETE 0x01 +/* GNSS Clock Flags, see `flags` of `struct sensor_gnss_clock` + * Refs: https://android.googlesource.com/platform/hardware/libhardware/+/ + * refs/heads/android14-release/include/hardware/gps.h#140 + */ + +#define SENSOR_GNSS_CLOCK_HAS_LEAP_SECOND (1 << 0) +#define SENSOR_GNSS_CLOCK_HAS_TIME_UNCERTAINTY (1 << 1) +#define SENSOR_GNSS_CLOCK_HAS_FULL_BIAS (1 << 2) +#define SENSOR_GNSS_CLOCK_HAS_BIAS (1 << 3) +#define SENSOR_GNSS_CLOCK_HAS_BIAS_UNCERTAINTY (1 << 4) +#define SENSOR_GNSS_CLOCK_HAS_DRIFT (1 << 5) +#define SENSOR_GNSS_CLOCK_HAS_DRIFT_UNCERTAINTY (1 << 6) + +/* GNSS Measurement Flags */ + +#define SENSOR_GNSS_MEASUREMENT_HAS_SNR (1 << 0) +#define SENSOR_GNSS_MEASUREMENT_HAS_CARRIER_FREQUENCY (1 << 9) +#define SENSOR_GNSS_MEASUREMENT_HAS_CARRIER_CYCLES (1 << 10) +#define SENSOR_GNSS_MEASUREMENT_HAS_CARRIER_PHASE (1 << 11) +#define SENSOR_GNSS_MEASUREMENT_HAS_CARRIER_PHASE_UNCERTAINTY (1 << 12) +#define SENSOR_GNSS_MEASUREMENT_HAS_AUTOMATIC_GAIN_CONTROL (1 << 13) + +/* GNSS Measurement States */ + +#define SENSOR_GNSS_MEASUREMENT_STATE_UNKNOWN (0) +#define SENSOR_GNSS_MEASUREMENT_STATE_CODE_LOCK (1 << 0) +#define SENSOR_GNSS_MEASUREMENT_STATE_BIT_SYNC (1 << 1) +#define SENSOR_GNSS_MEASUREMENT_STATE_SUBFRAME_SYNC (1 << 2) +#define SENSOR_GNSS_MEASUREMENT_STATE_TOW_DECODED (1 << 3) +#define SENSOR_GNSS_MEASUREMENT_STATE_MSEC_AMBIGUOUS (1 << 4) +#define SENSOR_GNSS_MEASUREMENT_STATE_SYMBOL_SYNC (1 << 5) +#define SENSOR_GNSS_MEASUREMENT_STATE_GLO_STRING_SYNC (1 << 6) +#define SENSOR_GNSS_MEASUREMENT_STATE_GLO_TOD_DECODED (1 << 7) +#define SENSOR_GNSS_MEASUREMENT_STATE_BDS_D2_BIT_SYNC (1 << 8) +#define SENSOR_GNSS_MEASUREMENT_STATE_BDS_D2_SUBFRAME_SYNC (1 << 9) +#define SENSOR_GNSS_MEASUREMENT_STATE_GAL_E1BC_CODE_LOCK (1 << 10) +#define SENSOR_GNSS_MEASUREMENT_STATE_GAL_E1C_2ND_CODE_LOCK (1 << 11) +#define SENSOR_GNSS_MEASUREMENT_STATE_GAL_E1B_PAGE_SYNC (1 << 12) +#define SENSOR_GNSS_MEASUREMENT_STATE_SBAS_SYNC (1 << 13) +#define SENSOR_GNSS_MEASUREMENT_STATE_TOW_KNOWN (1 << 14) +#define SENSOR_GNSS_MEASUREMENT_STATE_GLO_TOD_KNOWN (1 << 15) + /**************************************************************************** * Public Types ****************************************************************************/ @@ -754,4 +804,125 @@ struct sensor_device_info_s char vendor[SENSOR_INFO_NAME_SIZE]; }; +struct sensor_gnss_clock +{ + /* Indicating what fields are valid. + * See SENSOR_GNSS_CLOCK_HAS_*. + */ + + uint32_t flags; + + /* Leap second data. + * flags: SENSOR_GNSS_CLOCK_HAS_LEAP_SECOND + */ + + int32_t leap_second; + + /* The GNSS receiver internal local hardware clock value. + * flags: + * SENSOR_GNSS_CLOCK_HAS_TIME_UNCERTAINTY + */ + + int64_t time_ns; + float time_uncertainty_ns; + + /* Discontinuities in the HW clock. */ + + uint32_t hw_clock_discontinuity_count; + + /* The difference between hardware clock ('time' field) inside + * GPS receiver and the true GPS time since 0000Z, January 6, 1980, in + * nanoseconds. + * flags: + * SENSOR_GNSS_CLOCK_HAS_FULL_BIAS + * SENSOR_GNSS_CLOCK_HAS_BIAS + * SENSOR_GNSS_CLOCK_HAS_BIAS_UNCERTAINTY + */ + + int64_t full_bias_ns; + float bias_ns; /* Sub-nanosecond bias */ + float bias_uncertainty_ns; + + /* The clock's drift in nanoseconds (per second). + * A positive value means that the frequency is higher than + * the nominal frequency. + * flags: + * SENSOR_GNSS_CLOCK_HAS_DRIFT + * SENSOR_GNSS_CLOCK_HAS_DRIFT_UNCERTAINTY + */ + + float drift_nsps; + float drift_uncertainty_nsps; +}; + +struct sensor_gnss_measurement +{ + /* Indicating what fields are valid. + * See SENSOR_GNSS_MEASUREMENT_HAS_*. + */ + + uint32_t flags; + + /* Space vehicle ID. */ + + int32_t svid; + + /* Constellation of the given SV, see GNSS_CONSTELLATION_*. */ + + uint32_t constellation; + + /* Offset between clock and time at which the measurement was taken in + * nanoseconds. + */ + + float time_offset_ns; + + /* The received GNSS Time-of-Week at the measurement time, in + * nanoseconds. + */ + + int64_t received_sv_time_in_ns; + int64_t received_sv_time_uncertainty_in_ns; + + /* GNSS measurement state, see SENSOR_GNSS_MEASUREMENT_STATE_*. */ + + uint32_t state; + + /* dBHz, Carrier-to-noise density. */ + + float c_n0_dbhz; + + /* Pseudorange rate(m/s) at the timestamp. */ + + float pseudorange_rate_mps; + float pseudorange_rate_uncertainty_mps; + + /* Accumulated delta range. */ + + uint32_t accumulated_delta_range_state; + float accumulated_delta_range_m; + float accumulated_delta_range_uncertainty_m; + + /* Carrier related between the satellite and the receiver. + * flags: + * SENSOR_GNSS_MEASUREMENT_HAS_CARRIER_CYCLES + * SENSOR_GNSS_MEASUREMENT_HAS_CARRIER_FREQUENCY + * SENSOR_GNSS_MEASUREMENT_HAS_CARRIER_PHASE + * SENSOR_GNSS_MEASUREMENT_HAS_CARRIER_PHASE_UNCERTAINTY + */ + + float carrier_frequency_hz; + int64_t carrier_cycles; + float carrier_phase; + float carrier_phase_uncertainty; + + uint32_t multipath_indicator; + + /* dBHz, Signal to noise ratio of satellite C/N0. + * flags: SENSOR_GNSS_MEASUREMENT_HAS_SNR + */ + + uint32_t snr; +}; + #endif /* __INCLUDE_NUTTX_SENSORS_SENSOR_H */