From 12819f8d260b31a745ba5d12b04e73203394dacc Mon Sep 17 00:00:00 2001 From: TheJackiMonster Date: Wed, 12 Apr 2023 00:56:06 +0200 Subject: [PATCH] RE padding fields to be multipliers and divisors Signed-off-by: TheJackiMonster --- interface_lib/include/device3.h | 14 +++++--- interface_lib/src/device3.c | 61 +++++++++++++++++++++------------ 2 files changed, 49 insertions(+), 26 deletions(-) diff --git a/interface_lib/include/device3.h b/interface_lib/include/device3.h index 2b5b888..6189ad4 100644 --- a/interface_lib/include/device3.h +++ b/interface_lib/include/device3.h @@ -39,22 +39,25 @@ extern "C" { struct __attribute__((__packed__)) device3_packet_t { uint8_t signature [2]; - uint8_t _padding0 [2]; + uint8_t temperature [2]; uint64_t timestamp; - uint8_t _padding1 [6]; + uint8_t angular_multiplier [2]; + uint8_t angular_divisor [4]; uint8_t angular_velocity_x [3]; uint8_t angular_velocity_y [3]; uint8_t angular_velocity_z [3]; - uint8_t _padding2 [6]; + uint8_t acceleration_multiplier [2]; + uint8_t acceleration_divisor [4]; uint8_t acceleration_x [3]; uint8_t acceleration_y [3]; uint8_t acceleration_z [3]; - uint8_t _padding3 [6]; + uint8_t magnetic_multiplier [2]; + uint8_t magnetic_divisor [4]; uint8_t magnetic_x [2]; uint8_t magnetic_y [2]; uint8_t magnetic_z [2]; uint32_t checksum; - uint8_t _padding4 [6]; + uint8_t _padding [6]; }; enum device3_event_t { @@ -124,6 +127,7 @@ struct device3_t { bool claimed; uint64_t last_timestamp; + float temperature; void* offset; device3_ahrs_type* ahrs; diff --git a/interface_lib/src/device3.c b/interface_lib/src/device3.c index 5085cd8..8b12b6a 100644 --- a/interface_lib/src/device3.c +++ b/interface_lib/src/device3.c @@ -327,53 +327,68 @@ static void device3_callback(device3_type* device, device->callback(timestamp, event, device->ahrs, device->correction); } +static int32_t pack32bit_signed(const uint8_t* data) { + uint32_t unsigned_value = (data[0]) | (data[1] << 8) | (data[2] << 16) | (data[3] << 24); + return ((int32_t) unsigned_value); +} + static int32_t pack24bit_signed(const uint8_t* data) { uint32_t unsigned_value = (data[0]) | (data[1] << 8) | (data[2] << 16); if ((data[2] & 0x80) != 0) unsigned_value |= (0xFF << 24); - return (int32_t) unsigned_value; + return ((int32_t) unsigned_value); } static int16_t pack16bit_signed(const uint8_t* data) { - uint16_t unsigned_value = (data[0] << 8) | (data[1]); + uint16_t unsigned_value = (data[0]) | (data[1] << 8); return (int16_t) unsigned_value; } -// based on 24bit signed int w/ FSR = +/-2000 dps, datasheet option -#define GYRO_SCALAR (1.0f / 8388608.0f * 2000.0f) +static int32_t pack32bit_signed_swap(const uint8_t* data) { + uint32_t unsigned_value = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | (data[3]); + return ((int32_t) unsigned_value); +} -// based on 24bit signed int w/ FSR = +/-16 g, datasheet option -#define ACCEL_SCALAR (1.0f / 8388608.0f * 16.0f) - -// based on 16bit signed int w/ FSR = +/-16 gauss, datasheet option -#define MAGNO_SCALAR (1.0f / 8388608.0f * 16.0f) +static int16_t pack16bit_signed_swap(const uint8_t* data) { + uint16_t unsigned_value = (data[0] << 8) | (data[1]); + return (int16_t) unsigned_value; +} static void readIMU_from_packet(const device3_packet_type* packet, FusionVector* gyroscope, FusionVector* accelerometer, FusionVector* magnetometer) { + int32_t vel_m = pack16bit_signed(packet->angular_multiplier); + int32_t vel_d = pack32bit_signed(packet->angular_divisor); + int32_t vel_x = pack24bit_signed(packet->angular_velocity_x); int32_t vel_y = pack24bit_signed(packet->angular_velocity_y); int32_t vel_z = pack24bit_signed(packet->angular_velocity_z); - gyroscope->axis.x = (float) vel_x * GYRO_SCALAR; - gyroscope->axis.y = (float) vel_y * GYRO_SCALAR; - gyroscope->axis.z = (float) vel_z * GYRO_SCALAR; + gyroscope->axis.x = (float) vel_x * (float) vel_m / (float) vel_d; + gyroscope->axis.y = (float) vel_y * (float) vel_m / (float) vel_d; + gyroscope->axis.z = (float) vel_z * (float) vel_m / (float) vel_d; + + int32_t accel_m = pack16bit_signed(packet->acceleration_multiplier); + int32_t accel_d = pack32bit_signed(packet->acceleration_divisor); int32_t accel_x = pack24bit_signed(packet->acceleration_x); int32_t accel_y = pack24bit_signed(packet->acceleration_y); int32_t accel_z = pack24bit_signed(packet->acceleration_z); - accelerometer->axis.x = (float) accel_x * ACCEL_SCALAR; - accelerometer->axis.y = (float) accel_y * ACCEL_SCALAR; - accelerometer->axis.z = (float) accel_z * ACCEL_SCALAR; + accelerometer->axis.x = (float) accel_x * (float) accel_m / (float) accel_d; + accelerometer->axis.y = (float) accel_y * (float) accel_m / (float) accel_d; + accelerometer->axis.z = (float) accel_z * (float) accel_m / (float) accel_d; - int16_t magnet_x = pack16bit_signed(packet->magnetic_x); - int16_t magnet_y = pack16bit_signed(packet->magnetic_y); - int16_t magnet_z = pack16bit_signed(packet->magnetic_z); + int32_t magnet_m = pack16bit_signed_swap(packet->magnetic_multiplier); + int32_t magnet_d = pack32bit_signed_swap(packet->magnetic_divisor); - magnetometer->axis.x = (float) magnet_x * MAGNO_SCALAR; - magnetometer->axis.y = (float) magnet_y * MAGNO_SCALAR; - magnetometer->axis.z = (float) magnet_z * MAGNO_SCALAR; + int16_t magnet_x = pack16bit_signed_swap(packet->magnetic_x); + int16_t magnet_y = pack16bit_signed_swap(packet->magnetic_y); + int16_t magnet_z = pack16bit_signed_swap(packet->magnetic_z); + + magnetometer->axis.x = (float) magnet_x * (float) magnet_m / (float) magnet_d; + magnetometer->axis.y = (float) magnet_y * (float) magnet_m / (float) magnet_d; + magnetometer->axis.z = (float) magnet_z * (float) magnet_m / (float) magnet_d; } #define min(x, y) ((x) < (y)? (x) : (y)) @@ -695,6 +710,10 @@ int device3_read(device3_type* device, int timeout) { device->last_timestamp = timestamp; + int16_t temperature = pack16bit_signed(packet.temperature); + + device->temperature = (float) temperature; + FusionVector gyroscope; FusionVector accelerometer; FusionVector magnetometer;