adafruit-mirror/zephyr
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions 9

sensor: default_rtio_sensor: fix: Treat individual axis data as q31_t

Browse source 
Instead of assuming that an individual axis must contain all
data-values. Additionally, for determining that there's XYZ data, all
three channels must be present in the header.

Signed-off-by: Luis Ubieda <luisf@croxel.com>
This commit is contained in:
Luis Ubieda 2024-12-31 12:48:06 -05:00 committed by Benjamin Cabé
parent 36dfea121d
commit 5a1dfc82c3
1 changed files with 32 additions and 37 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

61
drivers/sensor/default_rtio_sensor.c
View file

@ -318,26 +318,45 @@ static int get_frame_count(const uint8_t *buffer, struct sensor_chan_spec channe
switch (channel.chan_type) {
case SENSOR_CHAN_ACCEL_XYZ:
channel.chan_type = SENSOR_CHAN_ACCEL_X;
break;
case SENSOR_CHAN_GYRO_XYZ:
channel.chan_type = SENSOR_CHAN_GYRO_X;
break;
case SENSOR_CHAN_MAGN_XYZ:
channel.chan_type = SENSOR_CHAN_MAGN_X;
break;
case SENSOR_CHAN_POS_DXYZ:
channel.chan_type = SENSOR_CHAN_POS_DX;
for (size_t i = 0 ; i < header->num_channels; ++i) {
/* For 3-axis channels, we need to verify we have each individual axis */
struct sensor_chan_spec channel_x = {
.chan_type = channel.chan_type - 3,
.chan_idx = channel.chan_idx,
};
struct sensor_chan_spec channel_y = {
.chan_type = channel.chan_type - 2,
.chan_idx = channel.chan_idx,
};
struct sensor_chan_spec channel_z = {
.chan_type = channel.chan_type - 1,
.chan_idx = channel.chan_idx,
};
/** The three axes don't need to be at the beginning of the header, but
* they should be consecutive.
*/
if (((header->num_channels - i) >= 3) &&
sensor_chan_spec_eq(header->channels[i], channel_x) &&
sensor_chan_spec_eq(header->channels[i + 1], channel_y) &&
sensor_chan_spec_eq(header->channels[i + 2], channel_z)) {
*frame_count = 1;
return 0;
}
}
break;
default:
break;
}
for (size_t i = 0; i < header->num_channels; ++i) {
if (sensor_chan_spec_eq(header->channels[i], channel)) {
*frame_count = 1;
return 0;
}
}
break;
}
return -ENOTSUP;
}
@ -353,21 +372,9 @@ int sensor_natively_supported_channel_size_info(struct sensor_chan_spec channel,
}
switch (channel.chan_type) {
case SENSOR_CHAN_ACCEL_X:
case SENSOR_CHAN_ACCEL_Y:
case SENSOR_CHAN_ACCEL_Z:
case SENSOR_CHAN_ACCEL_XYZ:
case SENSOR_CHAN_GYRO_X:
case SENSOR_CHAN_GYRO_Y:
case SENSOR_CHAN_GYRO_Z:
case SENSOR_CHAN_GYRO_XYZ:
case SENSOR_CHAN_MAGN_X:
case SENSOR_CHAN_MAGN_Y:
case SENSOR_CHAN_MAGN_Z:
case SENSOR_CHAN_MAGN_XYZ:
case SENSOR_CHAN_POS_DX:
case SENSOR_CHAN_POS_DY:
case SENSOR_CHAN_POS_DZ:
case SENSOR_CHAN_POS_DXYZ:
*base_size = sizeof(struct sensor_three_axis_data);
*frame_size = sizeof(struct sensor_three_axis_sample_data);
@ -480,33 +487,21 @@ static int decode(const uint8_t *buffer, struct sensor_chan_spec chan_spec,
/* Check for 3d channel mappings */
switch (chan_spec.chan_type) {
case SENSOR_CHAN_ACCEL_X:
case SENSOR_CHAN_ACCEL_Y:
case SENSOR_CHAN_ACCEL_Z:
case SENSOR_CHAN_ACCEL_XYZ:
count = decode_three_axis(header, q, data_out, SENSOR_CHAN_ACCEL_X,
SENSOR_CHAN_ACCEL_Y, SENSOR_CHAN_ACCEL_Z,
chan_spec.chan_idx);
break;
case SENSOR_CHAN_GYRO_X:
case SENSOR_CHAN_GYRO_Y:
case SENSOR_CHAN_GYRO_Z:
case SENSOR_CHAN_GYRO_XYZ:
count = decode_three_axis(header, q, data_out, SENSOR_CHAN_GYRO_X,
SENSOR_CHAN_GYRO_Y, SENSOR_CHAN_GYRO_Z,
chan_spec.chan_idx);
break;
case SENSOR_CHAN_MAGN_X:
case SENSOR_CHAN_MAGN_Y:
case SENSOR_CHAN_MAGN_Z:
case SENSOR_CHAN_MAGN_XYZ:
count = decode_three_axis(header, q, data_out, SENSOR_CHAN_MAGN_X,
SENSOR_CHAN_MAGN_Y, SENSOR_CHAN_MAGN_Z,
chan_spec.chan_idx);
break;
case SENSOR_CHAN_POS_DX:
case SENSOR_CHAN_POS_DY:
case SENSOR_CHAN_POS_DZ:
case SENSOR_CHAN_POS_DXYZ:
count = decode_three_axis(header, q, data_out, SENSOR_CHAN_POS_DX,
SENSOR_CHAN_POS_DY, SENSOR_CHAN_POS_DZ,