linguini1 opened a new pull request, #15789:
URL: https://github.com/apache/nuttx/pull/15789
## Summary
Adds support for the LSM6DSO32 IMU by STM using the uORB framework. This
only contains limited support for the I2C interface, interrupt and polling
driven measurement and standard modes of operation (high performance ODRs).
Features like interrupts besides DRDY, filtering, gesture recognition and
acting as a bus master are not implemented.
Accompanying documentation included.
## Impact
This PR adds another available uORB driver to the NuttX sensor drivers.
It impacts the build system by adding more Kconfig options and impacts the
kernel with more driver source files.
## Testing
All builds were performed on a Linux host. All testing was performed on an
RP2040 MCU on a custom flight computer board.
Below is the script that was used for testing.
```c
#include <errno.h>
#include <fcntl.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <nuttx/sensors/lsm6dso32.h>
#include <uORB/uORB.h>
#define SAMPLE_FREQ 50
#define ACCEL_NAME "sensor_accel0"
#define GYRO_NAME "sensor_gyro0"
int main(int argc, char **argv)
{
int err;
unsigned frequency;
int accel;
struct sensor_accel accel_data;
int gyro;
struct sensor_gyro gyro_data;
struct sensor_device_info_s info;
bool update = false;
printf("TESTING ACCELEROMETER IMPLEMENTATION\n");
/* Get accelerometer metadata */
const struct orb_metadata *accel_meta = orb_get_meta(ACCEL_NAME);
if (accel_meta == NULL)
{
fprintf(stderr, "Failed to get metadata for %s\n", ACCEL_NAME);
return EXIT_FAILURE;
}
/* Subscribe to accelerometer */
accel = orb_subscribe(accel_meta);
if (accel < 0)
{
fprintf(stderr, "Could not subsribe to %s: %d\n", ACCEL_NAME, errno);
return EXIT_FAILURE;
}
/* Set sampling rate to 50Hz */
err = orb_set_frequency(accel, SAMPLE_FREQ);
if (err)
{
fprintf(stderr, "Wasn't able to set frequency to %uHz: %d\n",
SAMPLE_FREQ, err);
return EXIT_FAILURE;
}
/* Show sampling rate */
err = orb_get_frequency(accel, &frequency);
if (err)
{
fprintf(stderr, "Could not verify frequency: %d\n", err);
return EXIT_FAILURE;
}
printf("Sampling frequency is %uHz\n", frequency);
/* Print sensor information */
err = orb_ioctl(accel, SNIOC_GET_INFO, (unsigned long)&info);
if (err < 0)
{
fprintf(stderr, "Could not get sensor information: %d", errno);
return EXIT_FAILURE;
}
printf("Sensor: %s\n", info.name);
printf("Manufacturer: %s\n", info.vendor);
printf("Power consumption: %.2fmA\n", info.power);
printf("Resolution: %.6f m/s^2\n", info.resolution);
printf("Max range: %.2f m/s^2\n", info.max_range);
printf("Min delay: %ld us\n", info.min_delay);
/* Perform self-test */
err = orb_ioctl(accel, SNIOC_SELFTEST, 0);
if (err < 0)
{
fprintf(stderr, "Self test failed: %d\n", errno);
return EXIT_FAILURE;
}
printf("Self test passed!\n");
/* Loop forever while printing out tab separated data as X, Y, Z */
for (int i = 0; i < 10;)
{
err = orb_check(accel, &update);
if (err)
{
fprintf(stderr, "Failed to check for new accel_data: %d\n", err);
continue;
}
/* Grab latest accel_data */
if (update)
{
err = orb_copy(accel_meta, accel, &accel_data);
if (err)
{
fprintf(stderr, "Failed to get new accel_data: %d\n", err);
continue;
}
printf("%f\t%f\t%f\n", accel_data.x, accel_data.y, accel_data.z);
i++;
}
}
/* Subtract offset */
float offsets[3] = {0.0f, -0.4f, 9.98f};
err = orb_ioctl(accel, SNIOC_SET_CALIBVALUE, (unsigned long)offsets);
if (err < 0)
{
fprintf(stderr, "Failed to set calibration values: %d", err);
return EXIT_FAILURE;
}
printf("Offsets set: %.2f X, %.2f Y, %.2f Z\n", offsets[0], offsets[1],
offsets[2]);
/* Loop forever while printing out tab separated accel_data as X, Y, Z */
for (int i = 0; i < 10;)
{
err = orb_check(accel, &update);
if (err)
{
fprintf(stderr, "Failed to check for new accel_data: %d\n", err);
continue;
}
/* Grab latest accel_data */
if (update)
{
err = orb_copy(accel_meta, accel, &accel_data);
if (err)
{
fprintf(stderr, "Failed to get new accel_data: %d\n", err);
continue;
}
printf("%f\t%f\t%f\n", accel_data.x, accel_data.y, accel_data.z);
i++;
}
}
printf("TESTING GYROSCOPE IMPLEMENTATION\n");
/* Get gyro metadata */
const struct orb_metadata *gyro_meta = orb_get_meta(GYRO_NAME);
if (gyro_meta == NULL)
{
fprintf(stderr, "Failed to get metadata for %s\n", GYRO_NAME);
return EXIT_FAILURE;
}
/* Subscribe to gyroerometer */
gyro = orb_subscribe(gyro_meta);
if (gyro < 0)
{
fprintf(stderr, "Could not subsribe to %s: %d\n", GYRO_NAME, errno);
return EXIT_FAILURE;
}
/* Set sampling rate to 50Hz */
err = orb_set_frequency(gyro, SAMPLE_FREQ);
if (err)
{
fprintf(stderr, "Wasn't able to set frequency to %uHz: %d\n",
SAMPLE_FREQ, err);
return EXIT_FAILURE;
}
/* Show sampling rate */
err = orb_get_frequency(gyro, &frequency);
if (err)
{
fprintf(stderr, "Could not verify frequency: %d\n", err);
return EXIT_FAILURE;
}
printf("Sampling frequency is %uHz\n", frequency);
/* Print sensor information */
err = orb_ioctl(gyro, SNIOC_GET_INFO, (unsigned long)&info);
if (err < 0)
{
fprintf(stderr, "Could not get sensor information: %d", errno);
return EXIT_FAILURE;
}
printf("Sensor: %s\n", info.name);
printf("Manufacturer: %s\n", info.vendor);
printf("Power consumption: %.2fmA\n", info.power);
printf("Resolution: %.6f rad/s\n", info.resolution);
printf("Max range: %.2f rad/s\n", info.max_range);
printf("Min delay: %ld us\n", info.min_delay);
/* Perform self-test */
err = orb_ioctl(gyro, SNIOC_SELFTEST, 0);
if (err < 0)
{
fprintf(stderr, "Self test failed: %d\n", errno);
return EXIT_FAILURE;
}
printf("Self test passed!\n");
/* Loop forever while printing out tab separated gyro_data as X, Y, Z */
for (int i = 0; i < 10;)
{
err = orb_check(gyro, &update);
if (err)
{
fprintf(stderr, "Failed to check for new gyro_data: %d\n", err);
continue;
}
/* Grab latest gyro_data */
if (update)
{
err = orb_copy(gyro_meta, gyro, &gyro_data);
if (err)
{
fprintf(stderr, "Failed to get new gyro_data: %d\n", err);
continue;
}
printf("%f\t%f\t%f\n", gyro_data.x, gyro_data.y, gyro_data.z);
i++;
}
}
/* Subtract offset */
float offsets2[3] = {0.0f, -1.0f, 0.0f};
err = orb_ioctl(gyro, SNIOC_SET_CALIBVALUE, (unsigned long)offsets2);
if (err < 0)
{
fprintf(stderr, "Failed to set calibration values: %d", err);
return EXIT_FAILURE;
}
printf("Offsets set: %.2f X, %.2f Y, %.2f Z\n", offsets2[0], offsets2[1],
offsets2[2]);
/* Loop forever while printing out tab separated gyro_data as X, Y, Z */
for (int i = 0; i < 10;)
{
err = orb_check(gyro, &update);
if (err)
{
fprintf(stderr, "Failed to check for new gyro_data: %d\n", err);
continue;
}
/* Grab latest gyro_data */
if (update)
{
err = orb_copy(gyro_meta, gyro, &gyro_data);
if (err)
{
fprintf(stderr, "Failed to get new gyro_data: %d\n", err);
continue;
}
printf("%f\t%f\t%f\n", gyro_data.x, gyro_data.y, gyro_data.z);
i++;
}
}
uint8_t id = 0;
err = orb_ioctl(gyro, SNIOC_WHO_AM_I, (unsigned long)&id);
if (err < 0)
{
fprintf(stderr, "Could not get WHOAMI value: %d\n", errno);
return EXIT_FAILURE;
}
printf("WHOAMI: %02X\n", id);
printf("Changing FSR of accelerometer to +/-32g\n");
err = orb_ioctl(accel, SNIOC_SETFULLSCALE, LSM6DSO32_FSR_XL_32G);
if (err < 0)
{
fprintf(stderr, "Could not get WHOAMI value: %d\n", errno);
return EXIT_FAILURE;
}
printf("New measurement values:\n");
for (int i = 0; i < 10;)
{
err = orb_check(accel, &update);
if (err)
{
fprintf(stderr, "Failed to check for new accel_data: %d\n", err);
continue;
}
/* Grab latest accel_data */
if (update)
{
err = orb_copy(accel_meta, accel, &accel_data);
if (err)
{
fprintf(stderr, "Failed to get new accel_data: %d\n", err);
continue;
}
printf("%f\t%f\t%f\n", accel_data.x, accel_data.y, accel_data.z);
i++;
}
}
orb_unsubscribe(accel);
orb_unsubscribe(gyro);
return EXIT_SUCCESS;
}
```
This script tests the following:
- Measurements are within the correct range for stationary board (i.e.
9.81~m/s^2 in one axis, 0 in others and 0 angular velocity in any direction)
- Self-tests for the sensors pass
- Self-tests for the sensors do not affect subsequent measurements due to
hysteresis
- `WHOAMI` register is read and the value is correct
- `SETFULLSCALE` changes the range of the sensor correctly and measurements
are correct following this change
- Measurement values are correctly modified by using `SET_CALIBVALUE`
- Measurement frequency can be set
- Changing FSR and ODR correctly changes the data in `get_info`
- `get_info` returns the correct information depending on accel/gyro
Here is the output of the script in interrupt-driven mode:
```console
TESTING ACCELEROMETER IMPLEMENTATION
Sampling frequency is 50Hz
Sensor: LSM6DSO32
Manufacturer: STMicro
Power consumption: 0.55mA
Resolution: 0.002392 m/s^2
Max range: 78.41 m/s^2
Min delay: 80000 us
Self test passed!
0.126820 -0.806385 19.946671
0.126820 -0.806385 19.946671
0.078963 -0.555138 13.189305
0.021535 -0.023928 0.165106
0.028714 -0.011964 0.052642
0.033499 -0.007178 0.052642
0.033499 -0.014357 0.057428
0.035892 -0.016749 0.057428
0.033499 -0.023928 0.055035
0.031106 -0.019142 0.057428
Offsets set: 0.00 X, -0.40 Y, 9.98 Z
0.031106 -0.016749 0.059820
0.031106 -0.009571 0.059820
0.038285 -0.009571 0.052642
0.031106 -0.014357 0.057428
0.033499 -0.016749 0.057428
0.026321 -0.021535 0.055035
0.026321 -0.028714 0.052642
0.028714 -0.023928 0.055035
0.035892 -0.011964 0.059820
0.035892 -0.016749 0.057428
TESTING GYROSCOPE IMPLEMENTATION
Sampling frequency is 50Hz
Sensor: LSM6DSO32
Manufacturer: STMicro
Power consumption: 0.55mA
Resolution: 0.000076 rad/s
Max range: 2.50 rad/s
Min delay: 80000 us
Self test passed!
0.001374 1.004123 0.001527
0.001374 1.004123 0.001527
0.001450 1.003894 0.001374
0.001450 1.003818 0.001450
0.001298 1.003894 0.001450
0.000839 1.003742 0.001374
0.000534 1.003894 0.001679
0.000763 1.003742 0.001374
0.000534 1.004047 0.001450
0.000916 1.003971 0.001298
Offsets set: 0.00 X, -1.00 Y, 0.00 Z
0.001069 1.003818 0.001679
0.001450 1.003818 0.001298
0.001298 1.003818 0.001679
0.001374 1.003665 0.001679
0.001221 1.003894 0.001679
0.001298 1.003665 0.001527
0.001145 1.003589 0.001374
0.000839 1.003589 0.001450
0.000687 1.003742 0.001374
0.000916 1.003894 0.001527
WHOAMI: 6C
Changing FSR of accelerometer to +/-32g
New measurement values:
0.035892 -0.016749 0.062213
0.028714 -0.014357 0.064606
0.028714 -0.019142 0.059820
0.031106 -0.016749 0.069392
0.031106 -0.014357 0.066999
0.035892 -0.011964 0.059820
0.035892 -0.011964 0.062213
0.035892 -0.011964 0.059820
0.033499 -0.014357 0.062213
0.028714 -0.016749 0.066999
```
Here is the output of the script in polling mode:
```console
TESTING ACCELEROMETER IMPLEMENTATION
Sampling frequency is 50Hz
Sensor: LSM6DSO32
Manufacturer: STMicro
Power consumption: 0.55mA
Resolution: 0.001196 m/s^2
Max range: 39.20 m/s^2
Min delay: 80000 us
Self test passed!
0.000000 0.000000 0.000000
0.078963 -0.436692 9.978121
0.076570 -0.429514 9.979318
0.065803 -0.425925 9.979318
0.076570 -0.445067 9.970942
0.064606 -0.440282 9.970942
0.053838 -0.443871 9.955389
0.069392 -0.423532 9.978121
0.074177 -0.423532 9.973335
0.070588 -0.434299 9.986496
Offsets set: 0.00 X, -0.40 Y, 9.98 Z
0.059820 -0.130409 0.177070
0.078963 -0.120838 0.180659
0.075374 -0.117249 0.175873
0.055035 -0.132802 0.165106
0.072981 -0.120838 0.183052
0.057428 -0.117249 0.163909
0.080160 -0.108874 0.173481
0.069392 -0.124427 0.177070
0.069392 -0.116052 0.185445
0.056231 -0.125624 0.180659
TESTING GYROSCOPE IMPLEMENTATION
Sampling frequency is 50Hz
Sensor: LSM6DSO32
Manufacturer: STMicro
Power consumption: 0.55mA
Resolution: 0.000076 rad/s
Max range: 2.50 rad/s
Min delay: 80000 us
Self test passed!
0.000000 0.000000 0.000000
0.001221 0.003436 0.001527
0.001069 0.003894 0.001450
0.000992 0.003665 0.001450
0.001221 0.003665 0.001450
0.000839 0.003741 0.001527
0.001298 0.003665 0.001603
0.001450 0.003359 0.000839
0.000916 0.003665 0.001374
0.001603 0.003436 0.001069
Offsets set: 0.00 X, -1.00 Y, 0.00 Z
0.000534 1.003589 0.001298
0.001450 1.003665 0.001450
0.001603 1.003360 0.001527
0.001679 1.003207 0.001374
0.001374 1.003665 0.001603
0.001756 1.003436 0.001298
0.001298 1.003589 0.001603
0.001527 1.003589 0.001527
0.001603 1.003207 0.001221
0.001069 1.003665 0.001450
WHOAMI: 6C
Changing FSR of accelerometer to +/-32g
New measurement values:
0.071785 -0.124427 0.212962
0.033499 -0.033499 0.066999
0.035892 -0.028714 0.066999
0.035892 -0.033499 0.069392
0.031106 -0.038285 0.066999
0.040678 -0.023928 0.064606
0.035892 -0.033499 0.071785
0.033499 -0.031106 0.066999
0.031106 -0.033499 0.066999
0.031106 -0.038285 0.069392
```
Here is the output of `uorb_listener` in polling mode (there is also a
magnetometer on this board, only the accel and gyro readings are from this
driver).
```console
object_name:sensor_mag, object_instance:0
object_name:sensor_gyro, object_instance:0
object_name:sensor_accel, object_instance:0
sensor_mag(now:80440000):timestamp:80440000,x:-72.900002,y:0.150000,z:-242.700012,temperature:23.101563,s0
sensor_gyro(now:80450000):timestamp:80440000,x:-0.000076,y:0.000000,z:-0.000076,temperature:23.000000
sensor_accel(now:80450000):timestamp:80450000,x:0.041874,y:-0.029910,z:-0.010767,temperature:23.000000
sensor_mag(now:80470000):timestamp:80470000,x:-74.099998,y:0.450000,z:-241.800003,temperature:23.070313,s0
sensor_gyro(now:80470000):timestamp:80470000,x:-0.050701,y:-0.017944,z:0.028863,temperature:24.000000
sensor_accel(now:80480000):timestamp:80480000,x:0.011964,y:-0.002392,z:-0.297908,temperature:24.000000
sensor_mag(now:80500000):timestamp:80500000,x:-74.250000,y:-0.750000,z:-242.700012,temperature:23.066406,0
sensor_gyro(now:80500000):timestamp:80500000,x:-0.031994,y:-0.001985,z:-0.008704,temperature:24.000000
sensor_accel(now:80510000):timestamp:80510000,x:0.014357,y:-0.017946,z:0.025124,temperature:24.000000
sensor_mag(now:80530000):timestamp:80530000,x:-73.800003,y:0.450000,z:-243.449997,temperature:23.078125,s0
Object name:sensor_mag0, recieved:4
Object name:sensor_gyro0, recieved:3
Object name:sensor_accel0, recieved:3
Total number of received Message:10/10
```
Here is the output of `uorb_listener` in interrupt-driven mode (there is
also a magnetometer and barometer on this board, only the accel and gyro
readings are from this driver).
```console
nsh> uorb_listener -r 50 -n 10
Mointor objects num:4
object_name:sensor_mag, object_instance:0
object_name:sensor_gyro, object_instance:0
object_name:sensor_baro, object_instance:0
object_name:sensor_accel, object_instance:0
sensor_mag(now:54820000):timestamp:54810000,x:-77.100006,y:0.150000,z:-243.600006,temperature:22.992188,s0
sensor_mag(now:54830000):timestamp:54830000,x:-75.599998,y:-0.150000,z:-243.300003,temperature:23.031250,0
sensor_gyro(now:54830000):timestamp:54820000,x:-0.039400,y:-0.034513,z:0.038331,temperature:23.000000
sensor_accel(now:54830000):timestamp:54820000,x:-0.007178,y:-0.093320,z:2.392835,temperature:23.000000
sensor_gyro(now:54840000):timestamp:54840000,x:-0.027107,y:-0.011377,z:-0.004886,temperature:24.000000
sensor_accel(now:54840000):timestamp:54840000,x:0.009571,y:-0.458228,z:9.941032,temperature:24.000000
sensor_baro(now:54850000):timestamp:54850000,pressure:505.600006,temperature:18.510000
sensor_mag(now:54850000):timestamp:54850000,x:-75.300003,y:0.600000,z:-243.300003,temperature:23.039063,s0
sensor_gyro(now:54860000):timestamp:54860000,x:0.126449,y:-0.159665,z:-0.030008,temperature:24.000000
sensor_accel(now:54860000):timestamp:54860000,x:0.039481,y:-0.513263,z:9.961371,temperature:24.000000
Object name:sensor_mag0, recieved:3
Object name:sensor_gyro0, recieved:3
Object name:sensor_baro0, recieved:1
Object name:sensor_accel0, recieved:3
Total number of received Message:10/10
```
Here are the system logs in debug mode when the driver is registered in
polling mode:
```console
lsm6dso32_register: LSM6DSO32 gyro using polling thread.
lsm6dso32_register: LSM6DSO32 accel using polling thread.
lsm6dso32_register: LSM6DSO32 driver registered!
```
Here are the system logs in debug mode when the driver is registered in
interrupt-driven mode:
```console
lis2mdl_register: Registered LIS2MDL driver in interrupt driven mode.
lis2mdl_register: Registered LIS2MDL driver.
sensor_custom_register: Registering /dev/uorb/sensor_gyro0
sensor_custom_register: Registering /dev/uorb/sensor_accel0
lsm6dso32_register: LSM6DSO32 gyro interrupt handler attached to INT1.
lsm6dso32_register: LSM6DSO32 accel interrupt handler attached to INT2.
lsm6dso32_register: LSM6DSO32 driver registered!
```
--
This is an automated message from the Apache Git Service.
To respond to the message, please log on to GitHub and use the
URL above to go to the specific comment.
To unsubscribe, e-mail: commits-unsubscr...@nuttx.apache.org
For queries about this service, please contact Infrastructure at:
us...@infra.apache.org
