From c21405d2d5cdc4b75c88d06194676247cff3618a Mon Sep 17 00:00:00 2001 From: chenzihan1 Date: Tue, 21 Jan 2025 16:30:10 +0800 Subject: [PATCH] drivers/sensors/goldfish: goldfish support more sensors add sensors type to align android Signed-off-by: chenzihan1 --- .../arm/goldfish/goldfish_sensor.rst | 224 +++++ drivers/sensors/goldfish_sensor_uorb.c | 773 ++++++++++-------- 2 files changed, 663 insertions(+), 334 deletions(-) create mode 100644 Documentation/platforms/arm/goldfish/goldfish_sensor.rst diff --git a/Documentation/platforms/arm/goldfish/goldfish_sensor.rst b/Documentation/platforms/arm/goldfish/goldfish_sensor.rst new file mode 100644 index 00000000000..f4a568628ad --- /dev/null +++ b/Documentation/platforms/arm/goldfish/goldfish_sensor.rst @@ -0,0 +1,224 @@ +======================= +Goldfish Sensor Driver +======================= + +Introduction +============ + +The Goldfish Sensor driver provides a virtual multi-sensor interface for +Android Goldfish/QEMU emulator environments. This driver enables NuttX to +interact with the QEMU sensor emulation infrastructure, allowing applications +to access various simulated sensor types without requiring physical hardware. + +The driver supports multiple sensor types through a unified interface based +on the NuttX sensor framework (uORB), making it suitable for development, +testing, and debugging of sensor-dependent applications in virtualized +environments. + +Supported Sensor Types +======================= + +The Goldfish Sensor driver supports the following sensor types: + +Inertial Sensors +---------------- + +- **Accelerometer** (``SENSOR_TYPE_ACCELEROMETER``) + + - Measures linear acceleration in three axes (x, y, z) + - Range: ±2.8 m/s² + - Resolution: 1.0/4032.0 m/s² + - Power consumption: 3.0 mW + +- **Gyroscope** (``SENSOR_TYPE_GYROSCOPE``) + + - Measures angular velocity in three axes + - Range: ±11.11 rad/s + - Resolution: 1.0/1000.0 rad/s + - Power consumption: 3.0 mW + +- **Uncalibrated Accelerometer** (``SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED``) + + - Raw accelerometer data without calibration + - Range: ±39.3 m/s² + - Resolution: 1.0/4032.0 m/s² + +- **Uncalibrated Gyroscope** (``SENSOR_TYPE_GYROSCOPE_UNCALIBRATED``) + + - Raw gyroscope data without drift compensation + - Range: ±16.46 rad/s + - Resolution: 1.0/1000.0 rad/s + +Magnetic Sensors +---------------- + +- **Magnetometer** (``SENSOR_TYPE_MAGNETIC_FIELD``) + + - Measures magnetic field strength in three axes + - Range: ±2000.0 μT + - Resolution: 1.0 μT + - Power consumption: 6.7 mW + +- **Uncalibrated Magnetometer** (``SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED``) + + - Raw magnetic field data without hard/soft iron correction + - Range: ±2000.0 μT + - Resolution: 1.0 μT + +Environmental Sensors +--------------------- + +- **Ambient Temperature** (``SENSOR_TYPE_AMBIENT_TEMPERATURE``) + + - Measures ambient temperature + - Range: 0-80°C + - Resolution: 1.0°C + - Power consumption: 0.0 mW + +- **Barometric Pressure** (``SENSOR_TYPE_BAROMETER``) + + - Measures atmospheric pressure + - Range: 0-800 hPa + - Resolution: 1.0 hPa + - Power consumption: 20.0 mW + +- **Relative Humidity** (``SENSOR_TYPE_RELATIVE_HUMIDITY``) + + - Measures relative humidity + - Range: 0-100% + - Resolution: 1.0% + - Power consumption: 20.0 mW + +Optical Sensors +--------------- + +- **Ambient Light** (``SENSOR_TYPE_LIGHT``) + + - Measures ambient light intensity + - Range: 0-40000 lux + - Resolution: 1.0 lux + - Power consumption: 20.0 mW + +- **Proximity** (``SENSOR_TYPE_PROXIMITY``) + + - Detects nearby objects + - Range: 0-1.0 (binary detection) + - Resolution: 1.0 + - Power consumption: 20.0 mW + +Orientation Sensors +------------------- + +- **Orientation** (``SENSOR_TYPE_ORIENTATION``) + + - Provides device orientation (azimuth, pitch, roll) + - Range: 0-360° + - Resolution: 1.0° + - Power consumption: 9.7 mW + +Position Sensors +---------------- + +- **Hinge Angle 0/1/2** (``SENSOR_TYPE_HINGE_ANGLE``) + + - Measures foldable device hinge angles + - Range: 0-360° + - Resolution: 1.0° + - Power consumption: 3.0 mW + - Note: Supports up to 3 hinges (for multi-fold devices) + +Biometric Sensors +----------------- + +- **Heart Rate** (``SENSOR_TYPE_HEART_RATE``) + + - Measures heart rate in beats per minute + - Range: 0-500 bpm + - Resolution: 1.0 bpm + - Power consumption: 20.0 mW + +Gesture Sensors +--------------- + +- **Wrist Tilt** (``SENSOR_TYPE_WRIST_TILT_GESTURE``) + + - Detects wrist tilt gesture events + - Range: 0-1.0 (event trigger) + - Resolution: 1.0 + - Power consumption: 20.0 mW + +Usage +===== + +Kernel Configuration +-------------------- + +Enable the Goldfish Sensor driver in your NuttX configuration:: + + CONFIG_SENSORS=y + CONFIG_SENSORS_GOLDFISH=y + CONFIG_GOLDFISH_PIPE=y + +Driver Initialization +--------------------- + +The driver is typically initialized during board-level setup:: + + #include + + int board_sensors_initialize(void) + { + int ret; + + /* Initialize Goldfish sensors with device number 0 + * and batch buffer size of 1 + */ + ret = goldfish_sensor_init(0, 1); + if (ret < 0) + { + snerr("ERROR: Failed to initialize Goldfish sensors: %d\n", ret); + return ret; + } + + return OK; + } + +Application Usage +----------------- + +Applications can access sensors through the standard NuttX sensor interface:: + + #include + #include + + int main(int argc, char *argv[]) + { + struct sensor_accel accel_data; + int fd; + int ret; + + /* Open accelerometer sensor */ + fd = open("/dev/uorb/sensor_accel0", O_RDONLY); + if (fd < 0) + { + printf("Failed to open accelerometer\n"); + return -1; + } + + /* Read sensor data */ + while (1) + { + ret = read(fd, &accel_data, sizeof(accel_data)); + if (ret == sizeof(accel_data)) + { + printf("Accel: x=%.2f y=%.2f z=%.2f (timestamp=%llu)\n", + accel_data.x, accel_data.y, accel_data.z, + accel_data.timestamp); + } + + usleep(100000); /* 100ms */ + } + + close(fd); + return 0; + } diff --git a/drivers/sensors/goldfish_sensor_uorb.c b/drivers/sensors/goldfish_sensor_uorb.c index f108e7a5e3c..73e5fcfb259 100644 --- a/drivers/sensors/goldfish_sensor_uorb.c +++ b/drivers/sensors/goldfish_sensor_uorb.c @@ -25,7 +25,9 @@ ****************************************************************************/ #include +#include #include +#include #include #include #include @@ -42,47 +44,40 @@ * Pre-processor Definitions ****************************************************************************/ -#define GOLDFISH_ACCELERATION 0 -#define GOLDFISH_GYROSCOPE 1 -#define GOLDFISH_MAGNETIC_FIELD 2 -#define GOLDFISH_ORIENTATION 3 -#define GOLDFISH_AMBIENT_TEMPERATURE 4 -#define GOLDFISH_PROXIMITY 5 -#define GOLDFISH_LIGHT 6 -#define GOLDFISH_PRESSURE 7 -#define GOLDFISH_RELATIVE_HUMIDITY 8 -#define GOLDFISH_MAGNETIC_FIELD_UNCALIBRATED 9 -#define GOLDFISH_GYROSCOPE_UNCALIBRATED 10 -#define GOLDFISH_HINGE_ANGLE0 11 -#define GOLDFISH_HINGE_ANGLE1 12 -#define GOLDFISH_HINGE_ANGLE2 13 -#define GOLDFISH_HEART_RATE 14 -#define GOLDFISH_RGBC_LIGHT 15 -#define GOLDFISH_WRIST_TILT 16 -#define GOLDFISH_ACCELERATION_UNCALIBRATED 17 - #define GOLDFISH_LIST_SENSOR_CMD "list-sensors" /**************************************************************************** * Private Types ****************************************************************************/ +struct goldfish_lowerhalf_s +{ + struct sensor_lowerhalf_s lower; + FAR const struct sensor_device_info_s *info; + intptr_t offset; +}; + struct goldfish_sensor_s { int64_t time_bias_ns; struct file pipe; - struct sensor_lowerhalf_s lower_accel; - struct sensor_lowerhalf_s lower_mag; - struct sensor_lowerhalf_s lower_gyro; - struct sensor_lowerhalf_s lower_accel_uncalibrated; - struct sensor_lowerhalf_s lower_mag_uncalibrated; - struct sensor_lowerhalf_s lower_gyro_uncalibrated; - struct sensor_lowerhalf_s lower_prox; - struct sensor_lowerhalf_s lower_light; - struct sensor_lowerhalf_s lower_baro; - struct sensor_lowerhalf_s lower_humi; - struct sensor_lowerhalf_s lower_temp; - struct sensor_lowerhalf_s lower_hrate; + struct goldfish_lowerhalf_s lower_accel; + struct goldfish_lowerhalf_s lower_gyro; + struct goldfish_lowerhalf_s lower_mag; + struct goldfish_lowerhalf_s lower_orient; + struct goldfish_lowerhalf_s lower_temp; + struct goldfish_lowerhalf_s lower_prox; + struct goldfish_lowerhalf_s lower_light; + struct goldfish_lowerhalf_s lower_baro; + struct goldfish_lowerhalf_s lower_humi; + struct goldfish_lowerhalf_s lower_mag_uncalibrated; + struct goldfish_lowerhalf_s lower_gyro_uncalibrated; + struct goldfish_lowerhalf_s lower_hinge_angle0; + struct goldfish_lowerhalf_s lower_hinge_angle1; + struct goldfish_lowerhalf_s lower_hinge_angle2; + struct goldfish_lowerhalf_s lower_hrate; + struct goldfish_lowerhalf_s lower_wrist_tilt; + struct goldfish_lowerhalf_s lower_accel_uncalibrated; uint32_t interval; }; @@ -90,12 +85,13 @@ struct goldfish_sensor_s * Private Function Prototypes ****************************************************************************/ -static int goldfish_sensor_activate(FAR struct sensor_lowerhalf_s *lower, +static int goldfish_sensor_activate(FAR struct sensor_lowerhalf_s *lower_, FAR struct file *filep, bool enabled); -static int goldfish_sensor_set_interval(FAR struct sensor_lowerhalf_s *lower, - FAR struct file *filep, - FAR uint32_t *period_us); -static int goldfish_sensor_get_info(FAR struct sensor_lowerhalf_s *lower, +static int +goldfish_sensor_set_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us); +static int goldfish_sensor_get_info(FAR struct sensor_lowerhalf_s *lower_, FAR struct file *filep, FAR struct sensor_device_info_s *info); static int goldfish_sensor_thread(int argc, FAR char** argv); @@ -111,31 +107,8 @@ static const struct sensor_ops_s g_goldfish_sensor_ops = .get_info = goldfish_sensor_get_info, }; -FAR static const char *const g_goldfish_sensor_name[] = +static const struct sensor_device_info_s g_goldfish_sensor_accel = { - "acceleration", - "gyroscope", - "magnetic-field", - "orientation", - "temperature", - "proximity", - "light", - "pressure", - "humidity", - "magnetic-field-uncalibrated", - "gyroscope-uncalibrated", - "hinge-angle0", - "hinge-angle1", - "hinge-angle2", - "heart-rate", - "rgbc-light", - "wrist-tilt", - "acceleration-uncalibrated", -}; - -static struct sensor_device_info_s g_goldfish_sensor_info[] = -{ - { .version = 1, .power = 3.0f, .max_range = 2.8f, @@ -146,8 +119,10 @@ static struct sensor_device_info_s g_goldfish_sensor_info[] = .fifo_max_event_count = 0, .name = "acceleration", .vendor = "The Android Open Source Project", - }, - { +}; + +static const struct sensor_device_info_s g_goldfish_sensor_gyro = +{ .version = 1, .power = 3.0f, .max_range = 11.1111111, @@ -156,8 +131,10 @@ static struct sensor_device_info_s g_goldfish_sensor_info[] = .max_delay = 500 * 1000, .name = "gyroscope", .vendor = "The Android Open Source Project", - }, - { +}; + +static const struct sensor_device_info_s g_goldfish_sensor_mag = +{ .version = 1, .power = 6.7f, .max_range = 2000.0f, @@ -168,8 +145,10 @@ static struct sensor_device_info_s g_goldfish_sensor_info[] = .fifo_max_event_count = 0, .name = "magnetic-field", .vendor = "The Android Open Source Project", - }, - { +}; + +static const struct sensor_device_info_s g_goldfish_sensor_orient = +{ .version = 1, .power = 9.7f, .max_range = 360.0f, @@ -180,8 +159,10 @@ static struct sensor_device_info_s g_goldfish_sensor_info[] = .fifo_max_event_count = 0, .name = "orientation", .vendor = "The Android Open Source Project", - }, - { +}; + +static const struct sensor_device_info_s g_goldfish_sensor_temp = +{ .version = 1, .power = 0.0f, .max_range = 80.0f, @@ -192,8 +173,10 @@ static struct sensor_device_info_s g_goldfish_sensor_info[] = .fifo_max_event_count = 0, .name = "temperature", .vendor = "The Android Open Source Project", - }, - { +}; + +static const struct sensor_device_info_s g_goldfish_sensor_prox = +{ .version = 1, .power = 20.0f, .max_range = 1.0f, @@ -204,8 +187,10 @@ static struct sensor_device_info_s g_goldfish_sensor_info[] = .fifo_max_event_count = 0, .name = "proximity", .vendor = "The Android Open Source Project", - }, - { +}; + +static const struct sensor_device_info_s g_goldfish_sensor_light = +{ .version = 1, .power = 20.0f, .max_range = 40000.0f, @@ -216,8 +201,10 @@ static struct sensor_device_info_s g_goldfish_sensor_info[] = .fifo_max_event_count = 0, .name = "light", .vendor = "The Android Open Source Project", - }, - { +}; + +static const struct sensor_device_info_s g_goldfish_sensor_baro = +{ .version = 1, .power = 20.0f, .max_range = 800.0f, @@ -228,8 +215,10 @@ static struct sensor_device_info_s g_goldfish_sensor_info[] = .fifo_max_event_count = 0, .name = "pressure", .vendor = "The Android Open Source Project", - }, - { +}; + +static const struct sensor_device_info_s g_goldfish_sensor_humi = +{ .version = 1, .power = 20.0f, .max_range = 100.0f, @@ -240,8 +229,10 @@ static struct sensor_device_info_s g_goldfish_sensor_info[] = .fifo_max_event_count = 0, .name = "humidity", .vendor = "The Android Open Source Project", - }, - { +}; + +static const struct sensor_device_info_s g_goldfish_sensor_mag_uncal = +{ .version = 1, .power = 6.7f, .max_range = 2000.0f, @@ -250,7 +241,90 @@ static struct sensor_device_info_s g_goldfish_sensor_info[] = .max_delay = 500 * 1000, .name = "magnetic-field-uncalibrated", .vendor = "The Android Open Source Project", - }, +}; + +static const struct sensor_device_info_s g_goldfish_sensor_gyro_uncal = +{ + .version = 1, + .power = 3.0, + .max_range = 16.46, + .resolution = 1.0 / 1000.0, + .min_delay = 10000, + .max_delay = 500 * 1000, + .name = "gyroscope-uncalibrated", + .vendor = "The Android Open Source Project", +}; + +static const struct sensor_device_info_s g_goldfish_sensor_hinge_angle0 = +{ + .version = 1, + .power = 3.0, + .max_range = 360, + .resolution = 1.0, + .min_delay = 0, + .max_delay = 0, + .name = "hinge-angle0", + .vendor = "The Android Open Source Project", +}; + +static const struct sensor_device_info_s g_goldfish_sensor_hinge_angle1 = +{ + .version = 1, + .power = 3.0, + .max_range = 360, + .resolution = 1.0, + .min_delay = 0, + .max_delay = 0, + .name = "hinge-angle1", + .vendor = "The Android Open Source Project", +}; + +static const struct sensor_device_info_s g_goldfish_sensor_hinge_angle2 = +{ + .version = 1, + .power = 3.0, + .max_range = 360, + .resolution = 1.0, + .min_delay = 0, + .max_delay = 0, + .name = "hinge-angle2", + .vendor = "The Android Open Source Project", +}; + +static const struct sensor_device_info_s g_goldfish_sensor_hrate = +{ + .version = 1, + .power = 20.0, + .max_range = 500, + .resolution = 1.0, + .min_delay = 0, + .max_delay = 500 * 1000, + .name = "heart-rate", + .vendor = "The Android Open Source Project", +}; + +static const struct sensor_device_info_s g_goldfish_sensor_wrist_tilt = +{ + .version = 1, + .power = 20.0, + .max_range = 1.0, + .resolution = 1.0, + .min_delay = 0, + .max_delay = 500 * 1000, + .name = "wrist-tilt", + .vendor = "The Android Open Source Project", +}; + +static const struct sensor_device_info_s g_goldfish_sensor_accel_uncal = +{ + .version = 1, + .power = 3.0, + .max_range = 39.3, + .resolution = 1.0 / 4032.0, + .min_delay = 10000, + .max_delay = 500 * 1000, + .name = "acceleration-uncalibrated", + .vendor = "The Android Open Source Project", }; /**************************************************************************** @@ -392,11 +466,6 @@ static inline int goldfish_sensor_open_pipe(FAR struct file *filep, return ret; } -static inline FAR const char *goldfish_sensor_get_name(int h) -{ - return g_goldfish_sensor_name[h]; -} - static FAR const char * goldfish_sensor_match(FAR const char *s, FAR const char *p) { @@ -411,21 +480,6 @@ goldfish_sensor_weigthed_average(int64_t a, int64_t aw, return (a * aw + b * bw) / (aw + bw); } -static int goldfish_sensor_do_activate(FAR struct file *pipe, - int handle, - bool enabled) -{ - char buffer[64]; - int len; - - len = snprintf(buffer, sizeof(buffer), - "set:%s:%d", - goldfish_sensor_get_name(handle), - enabled); - - return goldfish_sensor_send(pipe, buffer, len); -} - static void goldfish_sensor_parse_event(FAR struct goldfish_sensor_s *sensor) { FAR const char *value; @@ -451,9 +505,9 @@ static void goldfish_sensor_parse_event(FAR struct goldfish_sensor_s *sensor) { accel.temperature = NAN; accel.timestamp = now_ns + sensor->time_bias_ns; - sensor->lower_accel.push_event(sensor->lower_accel.priv, - &accel, - sizeof(struct sensor_accel)); + sensor->lower_accel.lower.push_event( + sensor->lower_accel.lower.priv, + &accel, sizeof(struct sensor_accel)); } } else if ((value = goldfish_sensor_match(buf, "gyroscope:")) != NULL) @@ -464,9 +518,9 @@ static void goldfish_sensor_parse_event(FAR struct goldfish_sensor_s *sensor) { gyro.temperature = NAN; gyro.timestamp = now_ns + sensor->time_bias_ns; - sensor->lower_gyro.push_event(sensor->lower_gyro.priv, - &gyro, - sizeof(struct sensor_gyro)); + sensor->lower_gyro.lower.push_event( + sensor->lower_gyro.lower.priv, + &gyro, sizeof(struct sensor_gyro)); } } else if ((value = goldfish_sensor_match(buf, "magnetic:")) != NULL) @@ -477,39 +531,78 @@ static void goldfish_sensor_parse_event(FAR struct goldfish_sensor_s *sensor) { mag.temperature = NAN; mag.timestamp = now_ns + sensor->time_bias_ns; - sensor->lower_mag.push_event(sensor->lower_mag.priv, - &mag, - sizeof(struct sensor_mag)); + sensor->lower_mag.lower.push_event( + sensor->lower_mag.lower.priv, + &mag, sizeof(struct sensor_mag)); } } - else if ((value = goldfish_sensor_match( - buf, "gyroscope-uncalibrated:")) != NULL) + else if ((value = goldfish_sensor_match(buf, "orientation:")) != NULL) { - struct sensor_gyro gyro; + struct sensor_orientation orient; if (sscanf(value, "%f:%f:%f", - &gyro.x, &gyro.y, &gyro.z) == 3) + &orient.x, &orient.y, &orient.z) == 3) { - gyro.temperature = NAN; - gyro.timestamp = now_ns + sensor->time_bias_ns; - sensor->lower_gyro_uncalibrated.push_event( - sensor->lower_gyro_uncalibrated.priv, - &gyro, - sizeof(struct sensor_gyro)); + orient.timestamp = now_ns + sensor->time_bias_ns; + sensor->lower_orient.lower.push_event( + sensor->lower_orient.lower.priv, + &orient, sizeof(struct sensor_orientation)); } } - else if ((value = goldfish_sensor_match( - buf, "acceleration-uncalibrated:")) != NULL) + else if ((value = goldfish_sensor_match(buf, "temperature:")) != NULL) { - struct sensor_accel accel; - if (sscanf(value, "%f:%f:%f", - &accel.x, &accel.y, &accel.z) == 3) + struct sensor_temp temp; + if (sscanf(value, "%f", &temp.temperature) == 1) { - accel.temperature = NAN; - accel.timestamp = now_ns + sensor->time_bias_ns; - sensor->lower_accel_uncalibrated.push_event( - sensor->lower_accel_uncalibrated.priv, - &accel, - sizeof(struct sensor_accel)); + temp.timestamp = now_ns + sensor->time_bias_ns; + sensor->lower_temp.lower.push_event( + sensor->lower_temp.lower.priv, + &temp, sizeof(struct sensor_temp)); + } + } + else if ((value = goldfish_sensor_match(buf, "proximity:")) != NULL) + { + struct sensor_prox prox; + if (sscanf(value, "%f", &prox.proximity) == 1) + { + prox.timestamp = now_ns + sensor->time_bias_ns; + sensor->lower_prox.lower.push_event( + sensor->lower_prox.lower.priv, + &prox, sizeof(struct sensor_prox)); + } + } + else if ((value = goldfish_sensor_match(buf, "light:")) != NULL) + { + struct sensor_light light; + if (sscanf(value, "%f", &light.light) == 1) + { + light.timestamp = now_ns + sensor->time_bias_ns; + light.ir = NAN; + sensor->lower_light.lower.push_event( + sensor->lower_light.lower.priv, + &light, sizeof(struct sensor_light)); + } + } + else if ((value = goldfish_sensor_match(buf, "pressure:")) != NULL) + { + struct sensor_baro baro; + if (sscanf(value, "%f", &baro.pressure) == 1) + { + baro.timestamp = now_ns + sensor->time_bias_ns; + baro.temperature = NAN; + sensor->lower_baro.lower.push_event( + sensor->lower_baro.lower.priv, + &baro, sizeof(struct sensor_baro)); + } + } + else if ((value = goldfish_sensor_match(buf, "humidity:")) != NULL) + { + struct sensor_humi humi; + if (sscanf(value, "%f", &humi.humidity) == 1) + { + humi.timestamp = now_ns + sensor->time_bias_ns; + sensor->lower_humi.lower.push_event( + sensor->lower_humi.lower.priv, + &humi, sizeof(struct sensor_humi)); } } else if ((value = goldfish_sensor_match( @@ -521,67 +614,56 @@ static void goldfish_sensor_parse_event(FAR struct goldfish_sensor_s *sensor) { mag.temperature = NAN; mag.timestamp = now_ns + sensor->time_bias_ns; - sensor->lower_mag_uncalibrated.push_event( - sensor->lower_mag_uncalibrated.priv, - &mag, - sizeof(struct sensor_mag)); + sensor->lower_mag_uncalibrated.lower.push_event( + sensor->lower_mag_uncalibrated.lower.priv, + &mag, sizeof(struct sensor_mag)); } } - else if ((value = goldfish_sensor_match(buf, "temperature:")) != NULL) + else if ((value = goldfish_sensor_match( + buf, "gyroscope-uncalibrated:")) != NULL) { - struct sensor_temp temp; - if (sscanf(value, "%f", &temp.temperature) == 1) + struct sensor_gyro gyro; + if (sscanf(value, "%f:%f:%f", + &gyro.x, &gyro.y, &gyro.z) == 3) { - temp.timestamp = now_ns + sensor->time_bias_ns; - sensor->lower_temp.push_event(sensor->lower_temp.priv, - &temp, - sizeof(struct sensor_temp)); + gyro.temperature = NAN; + gyro.timestamp = now_ns + sensor->time_bias_ns; + sensor->lower_gyro_uncalibrated.lower.push_event( + sensor->lower_gyro_uncalibrated.lower.priv, + &gyro, sizeof(struct sensor_gyro)); } } - else if ((value = goldfish_sensor_match(buf, "proximity:")) != NULL) + else if ((value = goldfish_sensor_match(buf, "hinge-angle0")) != NULL) { - struct sensor_prox prox; - if (sscanf(value, "%f", &prox.proximity) == 1) + struct sensor_angle angle; + if (sscanf(value, "%f:", &angle.angle) == 1) { - prox.timestamp = now_ns + sensor->time_bias_ns; - sensor->lower_prox.push_event(sensor->lower_prox.priv, - &prox, - sizeof(struct sensor_prox)); + angle.timestamp = now_ns + sensor->time_bias_ns; + sensor->lower_hinge_angle0.lower.push_event( + sensor->lower_hinge_angle0.lower.priv, + &angle, sizeof(struct sensor_angle)); } } - else if ((value = goldfish_sensor_match(buf, "light:")) != NULL) + else if ((value = goldfish_sensor_match(buf, "hinge-angle1")) != NULL) { - struct sensor_light light; - if (sscanf(value, "%f", &light.light) == 1) + struct sensor_angle angle; + if (sscanf(value, "%f:", &angle.angle) == 1) { - light.timestamp = now_ns + sensor->time_bias_ns; - light.ir = NAN; - sensor->lower_light.push_event(sensor->lower_light.priv, - &light, - sizeof(struct sensor_light)); + angle.timestamp = now_ns + sensor->time_bias_ns; + sensor->lower_hinge_angle1.lower.push_event( + sensor->lower_hinge_angle1.lower.priv, + &angle, sizeof(struct sensor_angle)); } } - else if ((value = goldfish_sensor_match(buf, "pressure:")) != NULL) + else if ((value = goldfish_sensor_match(buf, "hinge-angle2")) != NULL) { - struct sensor_baro baro; - if (sscanf(value, "%f", &baro.pressure) == 1) + struct sensor_angle angle; + if (sscanf(value, "%f:", &angle.angle) == 1) { - baro.timestamp = now_ns + sensor->time_bias_ns; - baro.temperature = NAN; - sensor->lower_baro.push_event(sensor->lower_baro.priv, - &baro, - sizeof(struct sensor_baro)); - } - } - else if ((value = goldfish_sensor_match(buf, "humidity:")) != NULL) - { - struct sensor_humi humi; - if (sscanf(value, "%f", &humi.humidity) == 1) - { - humi.timestamp = now_ns + sensor->time_bias_ns; - sensor->lower_humi.push_event(sensor->lower_humi.priv, - &humi, - sizeof(struct sensor_humi)); + angle.timestamp = now_ns + sensor->time_bias_ns; + sensor->lower_hinge_angle2.lower.push_event( + sensor->lower_hinge_angle2.lower.priv, + &angle, sizeof(struct sensor_angle)); } } else if ((value = goldfish_sensor_match(buf, "heart-rate:")) != NULL) @@ -590,9 +672,34 @@ static void goldfish_sensor_parse_event(FAR struct goldfish_sensor_s *sensor) if (sscanf(value, "%f", &hrate.bpm) == 1) { hrate.timestamp = now_ns + sensor->time_bias_ns; - sensor->lower_hrate.push_event(sensor->lower_hrate.priv, - &hrate, - sizeof(struct sensor_hrate)); + sensor->lower_hrate.lower.push_event( + sensor->lower_hrate.lower.priv, + &hrate, sizeof(struct sensor_hrate)); + } + } + else if ((value = goldfish_sensor_match(buf, "wrist-tilt")) != NULL) + { + struct sensor_event wrist; + if (sscanf(value, "%" PRIu32, &wrist.event) == 1) + { + wrist.timestamp = now_ns + sensor->time_bias_ns; + sensor->lower_wrist_tilt.lower.push_event( + sensor->lower_wrist_tilt.lower.priv, + &wrist, sizeof(struct sensor_event)); + } + } + else if ((value = goldfish_sensor_match( + buf, "acceleration-uncalibrated:")) != NULL) + { + struct sensor_accel accel; + if (sscanf(value, "%f:%f:%f", + &accel.x, &accel.y, &accel.z) == 3) + { + accel.temperature = NAN; + accel.timestamp = now_ns + sensor->time_bias_ns; + sensor->lower_accel_uncalibrated.lower.push_event( + sensor->lower_accel_uncalibrated.lower.priv, + &accel, sizeof(struct sensor_accel)); } } else if ((value = goldfish_sensor_match(buf, "guest-sync:")) != NULL) @@ -615,118 +722,62 @@ static void goldfish_sensor_parse_event(FAR struct goldfish_sensor_s *sensor) } } -static int goldfish_get_priv(FAR struct sensor_lowerhalf_s *lower, - FAR struct goldfish_sensor_s **priv) +static FAR struct goldfish_sensor_s * +goldfish_get_priv(FAR struct sensor_lowerhalf_s *lower_) { - switch (lower->type) - { - case SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED: - priv = container_of(lower, struct goldfish_sensor_s, - lower_accel_uncalibrated); - return GOLDFISH_ACCELERATION_UNCALIBRATED; - case SENSOR_TYPE_ACCELEROMETER: - priv = container_of(lower, struct goldfish_sensor_s, lower_accel); - return GOLDFISH_ACCELERATION; - case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: - priv = container_of(lower, struct goldfish_sensor_s, - lower_mag_uncalibrated); - return GOLDFISH_MAGNETIC_FIELD_UNCALIBRATED; - case SENSOR_TYPE_MAGNETIC_FIELD: - priv = container_of(lower, struct goldfish_sensor_s, lower_mag); - return GOLDFISH_MAGNETIC_FIELD; - case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: - priv = container_of(lower, struct goldfish_sensor_s, - lower_gyro_uncalibrated); - return GOLDFISH_GYROSCOPE_UNCALIBRATED; - case SENSOR_TYPE_GYROSCOPE: - priv = container_of(lower, struct goldfish_sensor_s, lower_gyro); - return GOLDFISH_GYROSCOPE; - case SENSOR_TYPE_PROXIMITY: - *priv = container_of(lower, struct goldfish_sensor_s, lower_prox); - return GOLDFISH_PROXIMITY; - case SENSOR_TYPE_LIGHT: - *priv = container_of(lower, struct goldfish_sensor_s, lower_light); - return GOLDFISH_LIGHT; - case SENSOR_TYPE_BAROMETER: - *priv = container_of(lower, struct goldfish_sensor_s, lower_baro); - return GOLDFISH_PRESSURE; - case SENSOR_TYPE_RELATIVE_HUMIDITY: - *priv = container_of(lower, struct goldfish_sensor_s, lower_humi); - return GOLDFISH_RELATIVE_HUMIDITY; - case SENSOR_TYPE_AMBIENT_TEMPERATURE: - *priv = container_of(lower, struct goldfish_sensor_s, lower_temp); - return GOLDFISH_AMBIENT_TEMPERATURE; - case SENSOR_TYPE_HEART_RATE: - *priv = container_of(lower, struct goldfish_sensor_s, lower_hrate); - return GOLDFISH_HEART_RATE; - default: - return -EINVAL; - } + FAR struct goldfish_lowerhalf_s *lower = + container_of(lower_, struct goldfish_lowerhalf_s, lower); + intptr_t offset = lower->offset; + + return (FAR struct goldfish_sensor_s *)((FAR char *)lower - offset); } -static int goldfish_sensor_activate(FAR struct sensor_lowerhalf_s *lower, +static int goldfish_sensor_activate(FAR struct sensor_lowerhalf_s *lower_, FAR struct file *filep, bool enabled) { - FAR struct goldfish_sensor_s *priv; - int handle = goldfish_get_priv(lower, &priv); - - if (handle < 0) - { - return handle; - } - - return goldfish_sensor_do_activate(&priv->pipe, handle, enabled); -} - -static int goldfish_sensor_set_interval(FAR struct sensor_lowerhalf_s *lower, - FAR struct file *filep, - FAR uint32_t *period_us) -{ - struct FAR goldfish_sensor_s *priv; + FAR struct goldfish_lowerhalf_s *lower = + container_of(lower_, struct goldfish_lowerhalf_s, lower); + FAR struct goldfish_sensor_s *priv = goldfish_get_priv(lower_); char buffer[64]; - int handle; int len; - handle = goldfish_get_priv(lower, &priv); - if (handle < 0) - { - return handle; - } + len = snprintf(buffer, sizeof(buffer), + "set:%s:%d", + lower->info->name, + enabled); + return goldfish_sensor_send(&priv->pipe, buffer, len); +} + +static int +goldfish_sensor_set_interval(FAR struct sensor_lowerhalf_s *lower, + FAR struct file *filep, + FAR uint32_t *period_us) +{ + FAR struct goldfish_sensor_s *priv = goldfish_get_priv(lower); + char buffer[64]; + int len; + int ret; len = snprintf(buffer, sizeof(buffer), "set-delay: %d", (int)(*period_us / 1000)); - goldfish_sensor_send(&priv->pipe, buffer, len); + ret = goldfish_sensor_send(&priv->pipe, buffer, len); + if (ret < 0) + { + return ret; + } + priv->interval = *period_us; return OK; } -static int goldfish_sensor_get_info(FAR struct sensor_lowerhalf_s *lower, +static int goldfish_sensor_get_info(FAR struct sensor_lowerhalf_s *lower_, FAR struct file *filep, FAR struct sensor_device_info_s *info) { - FAR struct goldfish_sensor_s *priv; - int handle; - int i; - - handle = goldfish_get_priv(lower, &priv); - if (handle < 0) - { - return -handle; - } - - for (i = 0; i < sizeof(g_goldfish_sensor_info); i++) - { - if (!strncmp(goldfish_sensor_get_name(handle), - g_goldfish_sensor_info[i].name, - strlen(g_goldfish_sensor_info[i].name))) - { - memcpy(info, &g_goldfish_sensor_info[i], - sizeof(struct sensor_device_info_s)); - return OK; - } - } - - return -EINVAL; + FAR struct goldfish_lowerhalf_s *lower = + container_of(lower_, struct goldfish_lowerhalf_s, lower); + memcpy(info, lower->info, sizeof(struct sensor_device_info_s)); + return OK; } static int goldfish_sensor_thread(int argc, FAR char** argv) @@ -742,6 +793,21 @@ static int goldfish_sensor_thread(int argc, FAR char** argv) return OK; } +static int +goldfish_register_sensor(FAR struct goldfish_lowerhalf_s *lower, + FAR const struct sensor_device_info_s *info, + int type, int devno, uint32_t batch_number, + bool uncalibrated, intptr_t offset) +{ + lower->lower.uncalibrated = uncalibrated; + lower->lower.type = type; + lower->lower.ops = &g_goldfish_sensor_ops; + lower->lower.nbuffer = batch_number; + lower->info = info; + lower->offset = offset; + return sensor_register(&lower->lower, devno); +} + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -838,67 +904,106 @@ int goldfish_sensor_init(int devno, uint32_t batch_number) /* Register sensor */ - sensor->lower_accel.type = SENSOR_TYPE_ACCELEROMETER; - sensor->lower_accel.ops = &g_goldfish_sensor_ops; - sensor->lower_accel.nbuffer = batch_number; - - sensor->lower_mag.type = SENSOR_TYPE_MAGNETIC_FIELD; - sensor->lower_mag.ops = &g_goldfish_sensor_ops; - sensor->lower_mag.nbuffer = batch_number; - - sensor->lower_gyro.type = SENSOR_TYPE_GYROSCOPE; - sensor->lower_gyro.ops = &g_goldfish_sensor_ops; - sensor->lower_gyro.nbuffer = batch_number; - - sensor->lower_accel_uncalibrated.type = - SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED; - sensor->lower_accel_uncalibrated.ops = &g_goldfish_sensor_ops; - sensor->lower_accel_uncalibrated.nbuffer = batch_number; - - sensor->lower_mag_uncalibrated.type = - SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED; - sensor->lower_mag_uncalibrated.ops = &g_goldfish_sensor_ops; - sensor->lower_mag_uncalibrated.nbuffer = batch_number; - - sensor->lower_gyro_uncalibrated.type = - SENSOR_TYPE_GYROSCOPE_UNCALIBRATED; - sensor->lower_gyro_uncalibrated.ops = &g_goldfish_sensor_ops; - sensor->lower_gyro_uncalibrated.nbuffer = batch_number; - - sensor->lower_prox.type = SENSOR_TYPE_PROXIMITY; - sensor->lower_prox.ops = &g_goldfish_sensor_ops; - sensor->lower_prox.nbuffer = batch_number; - - sensor->lower_light.type = SENSOR_TYPE_LIGHT; - sensor->lower_light.ops = &g_goldfish_sensor_ops; - sensor->lower_light.nbuffer = batch_number; - - sensor->lower_baro.type = SENSOR_TYPE_BAROMETER; - sensor->lower_baro.ops = &g_goldfish_sensor_ops; - sensor->lower_baro.nbuffer = batch_number; - - sensor->lower_humi.type = SENSOR_TYPE_RELATIVE_HUMIDITY; - sensor->lower_humi.ops = &g_goldfish_sensor_ops; - sensor->lower_humi.nbuffer = batch_number; - - sensor->lower_temp.type = SENSOR_TYPE_AMBIENT_TEMPERATURE; - sensor->lower_temp.ops = &g_goldfish_sensor_ops; - sensor->lower_temp.nbuffer = batch_number; - - sensor->lower_hrate.type = SENSOR_TYPE_HEART_RATE; - sensor->lower_hrate.ops = &g_goldfish_sensor_ops; - sensor->lower_hrate.nbuffer = batch_number; - - return sensor_register(&sensor->lower_accel, devno) | - sensor_register(&sensor->lower_mag, devno) | - sensor_register(&sensor->lower_gyro, devno) | - sensor_register(&sensor->lower_accel_uncalibrated, devno) | - sensor_register(&sensor->lower_mag_uncalibrated, devno) | - sensor_register(&sensor->lower_gyro_uncalibrated, devno) | - sensor_register(&sensor->lower_prox, devno) | - sensor_register(&sensor->lower_light, devno) | - sensor_register(&sensor->lower_baro, devno) | - sensor_register(&sensor->lower_humi, devno) | - sensor_register(&sensor->lower_temp, devno) | - sensor_register(&sensor->lower_hrate, devno); + return goldfish_register_sensor( + &sensor->lower_accel, + &g_goldfish_sensor_accel, + SENSOR_TYPE_ACCELEROMETER, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_accel)) | + goldfish_register_sensor( + &sensor->lower_gyro, + &g_goldfish_sensor_gyro, + SENSOR_TYPE_GYROSCOPE, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_gyro)) | + goldfish_register_sensor( + &sensor->lower_mag, + &g_goldfish_sensor_mag, + SENSOR_TYPE_MAGNETIC_FIELD, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_mag)) | + goldfish_register_sensor( + &sensor->lower_orient, + &g_goldfish_sensor_orient, + SENSOR_TYPE_ORIENTATION, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_orient)) | + goldfish_register_sensor( + &sensor->lower_temp, + &g_goldfish_sensor_temp, + SENSOR_TYPE_AMBIENT_TEMPERATURE, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_temp)) | + goldfish_register_sensor( + &sensor->lower_prox, + &g_goldfish_sensor_prox, + SENSOR_TYPE_PROXIMITY, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_prox)) | + goldfish_register_sensor( + &sensor->lower_light, + &g_goldfish_sensor_light, + SENSOR_TYPE_LIGHT, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_light)) | + goldfish_register_sensor( + &sensor->lower_baro, + &g_goldfish_sensor_baro, + SENSOR_TYPE_BAROMETER, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_baro)) | + goldfish_register_sensor( + &sensor->lower_humi, + &g_goldfish_sensor_humi, + SENSOR_TYPE_RELATIVE_HUMIDITY, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_humi)) | + goldfish_register_sensor( + &sensor->lower_mag_uncalibrated, + &g_goldfish_sensor_mag_uncal, + SENSOR_TYPE_MAGNETIC_FIELD, + devno, batch_number, true, + offsetof(struct goldfish_sensor_s, lower_mag_uncalibrated)) | + goldfish_register_sensor( + &sensor->lower_gyro_uncalibrated, + &g_goldfish_sensor_gyro_uncal, + SENSOR_TYPE_GYROSCOPE, + devno, batch_number, true, + offsetof(struct goldfish_sensor_s, lower_gyro_uncalibrated)) | + goldfish_register_sensor( + &sensor->lower_hinge_angle0, + &g_goldfish_sensor_hinge_angle0, + SENSOR_TYPE_HINGE_ANGLE, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_hinge_angle0)) | + goldfish_register_sensor( + &sensor->lower_hinge_angle1, + &g_goldfish_sensor_hinge_angle1, + SENSOR_TYPE_HINGE_ANGLE, + devno + 1, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_hinge_angle1)) | + goldfish_register_sensor( + &sensor->lower_hinge_angle2, + &g_goldfish_sensor_hinge_angle2, + SENSOR_TYPE_HINGE_ANGLE, + devno + 2, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_hinge_angle2)) | + goldfish_register_sensor( + &sensor->lower_hrate, + &g_goldfish_sensor_hrate, + SENSOR_TYPE_HEART_RATE, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_hrate)) | + goldfish_register_sensor( + &sensor->lower_wrist_tilt, + &g_goldfish_sensor_wrist_tilt, + SENSOR_TYPE_WRIST_TILT_GESTURE, + devno, batch_number, false, + offsetof(struct goldfish_sensor_s, lower_wrist_tilt)) | + goldfish_register_sensor( + &sensor->lower_accel_uncalibrated, + &g_goldfish_sensor_accel_uncal, + SENSOR_TYPE_ACCELEROMETER, + devno, batch_number, true, + offsetof(struct goldfish_sensor_s, lower_accel_uncalibrated)); }