#![allow(unused, clippy::comparison_to_empty, clippy::manual_range_patterns)]
use crate::profile::{ProfileType, typedef};
use crate::proto::*;
#[derive(Debug, Clone)]
pub struct ThreeDSensorCalibration {
pub timestamp: typedef::DateTime,
pub sensor_type: typedef::SensorType,
pub calibration_factor: u32,
pub calibration_divisor: u32,
pub level_shift: u32,
pub offset_cal: [i32; 3],
pub orientation_matrix: [i32; 9],
pub unknown_fields: Vec<Field>,
pub developer_fields: Vec<DeveloperField>,
}
impl ThreeDSensorCalibration {
pub const TIMESTAMP: u8 = 253;
pub const SENSOR_TYPE: u8 = 0;
pub const CALIBRATION_FACTOR: u8 = 1;
pub const CALIBRATION_DIVISOR: u8 = 2;
pub const LEVEL_SHIFT: u8 = 3;
pub const OFFSET_CAL: u8 = 4;
pub const ORIENTATION_MATRIX: u8 = 5;
pub const fn new() -> Self {
Self {
timestamp: typedef::DateTime(u32::MAX),
sensor_type: typedef::SensorType(u8::MAX),
calibration_factor: u32::MAX,
calibration_divisor: u32::MAX,
level_shift: u32::MAX,
offset_cal: [i32::MAX; 3],
orientation_matrix: [i32::MAX; 9],
unknown_fields: Vec::new(),
developer_fields: Vec::new(),
}
}
pub fn orientation_matrix_scaled(&self) -> [f64; 9] {
let mut v = [f64::from_bits(u64::MAX); 9];
for (i, &x) in self.orientation_matrix.iter().enumerate() {
if x == i32::MAX {
continue;
}
v[i] = x as f64 / 65535.0 - 0.0;
}
v
}
pub fn set_orientation_matrix_scaled(&mut self, v: [f64; 9]) -> &mut ThreeDSensorCalibration {
self.orientation_matrix = [i32::MAX; 9];
for (i, &x) in v.iter().enumerate() {
let unscaled = (x + 0.0) * 65535.0;
if unscaled.is_nan() || unscaled.is_infinite() || unscaled > i32::MAX as f64 {
continue;
}
self.orientation_matrix[i] = (unscaled as i32);
}
self
}
}
impl Default for ThreeDSensorCalibration {
fn default() -> Self {
Self::new()
}
}
impl From<&Message> for ThreeDSensorCalibration {
fn from(mesg: &Message) -> Self {
let mut vals: [&Value; 254] = [const { &Value::Invalid }; 254];
const KNOWN_NUMS: [u64; 4] = [63, 0, 0, 2305843009213693952];
let mut n = 0u64;
for field in &mesg.fields {
n += (KNOWN_NUMS[field.num as usize >> 6] >> (field.num & 63)) & 1 ^ 1
}
let mut unknown_fields: Vec<Field> = Vec::with_capacity(n as usize);
for field in &mesg.fields {
if (KNOWN_NUMS[field.num as usize >> 6] >> (field.num & 63)) & 1 == 0 {
unknown_fields.push(field.clone());
continue;
}
vals[field.num as usize] = &field.value;
}
Self {
timestamp: typedef::DateTime(vals[253].as_u32()),
sensor_type: typedef::SensorType(vals[0].as_u8()),
calibration_factor: vals[1].as_u32(),
calibration_divisor: vals[2].as_u32(),
level_shift: vals[3].as_u32(),
offset_cal: match &vals[4] {
Value::VecInt32(v) => {
let mut arr: [i32; 3] = [i32::MAX; 3];
for (i, x) in v.iter().enumerate() {
arr[i] = *x;
}
arr
}
_ => [i32::MAX; 3],
},
orientation_matrix: match &vals[5] {
Value::VecInt32(v) => {
let mut arr: [i32; 9] = [i32::MAX; 9];
for (i, x) in v.iter().enumerate() {
arr[i] = *x;
}
arr
}
_ => [i32::MAX; 9],
},
unknown_fields,
developer_fields: mesg.developer_fields.clone(),
}
}
}
impl From<ThreeDSensorCalibration> for Message {
fn from(m: ThreeDSensorCalibration) -> Self {
let mut arr = [const {
Field {
num: 0,
profile_type: ProfileType(0),
value: Value::Invalid,
is_expanded: false,
}
}; 7];
let mut len = 0usize;
if m.timestamp != typedef::DateTime(u32::MAX) {
arr[len] = Field {
num: 253,
profile_type: ProfileType::DATE_TIME,
value: Value::Uint32(m.timestamp.0),
is_expanded: false,
};
len += 1;
}
if m.sensor_type != typedef::SensorType(u8::MAX) {
arr[len] = Field {
num: 0,
profile_type: ProfileType::SENSOR_TYPE,
value: Value::Uint8(m.sensor_type.0),
is_expanded: false,
};
len += 1;
}
if m.calibration_factor != u32::MAX {
arr[len] = Field {
num: 1,
profile_type: ProfileType::UINT32,
value: Value::Uint32(m.calibration_factor),
is_expanded: false,
};
len += 1;
}
if m.calibration_divisor != u32::MAX {
arr[len] = Field {
num: 2,
profile_type: ProfileType::UINT32,
value: Value::Uint32(m.calibration_divisor),
is_expanded: false,
};
len += 1;
}
if m.level_shift != u32::MAX {
arr[len] = Field {
num: 3,
profile_type: ProfileType::UINT32,
value: Value::Uint32(m.level_shift),
is_expanded: false,
};
len += 1;
}
if m.offset_cal != [i32::MAX; 3] {
arr[len] = Field {
num: 4,
profile_type: ProfileType::SINT32,
value: Value::VecInt32(Vec::from(&m.offset_cal)),
is_expanded: false,
};
len += 1;
}
if m.orientation_matrix != [i32::MAX; 9] {
arr[len] = Field {
num: 5,
profile_type: ProfileType::SINT32,
value: Value::VecInt32(Vec::from(&m.orientation_matrix)),
is_expanded: false,
};
len += 1;
}
Message {
header: 0,
num: typedef::MesgNum::THREE_D_SENSOR_CALIBRATION,
fields: {
let mut fields: Vec<Field> = Vec::with_capacity(len + m.unknown_fields.len());
fields.extend_from_slice(&arr[..len]);
fields.extend_from_slice(&m.unknown_fields);
fields
},
developer_fields: m.developer_fields,
}
}
}