summaryrefslogtreecommitdiff
path: root/libsensors_iio/software/core/mllite/data_builder.c
diff options
context:
space:
mode:
Diffstat (limited to 'libsensors_iio/software/core/mllite/data_builder.c')
-rw-r--r--libsensors_iio/software/core/mllite/data_builder.c186
1 files changed, 64 insertions, 122 deletions
diff --git a/libsensors_iio/software/core/mllite/data_builder.c b/libsensors_iio/software/core/mllite/data_builder.c
index f70be7c..b139771 100644
--- a/libsensors_iio/software/core/mllite/data_builder.c
+++ b/libsensors_iio/software/core/mllite/data_builder.c
@@ -23,6 +23,7 @@
#include "mlmath.h"
#include "storage_manager.h"
#include "message_layer.h"
+#include "results_holder.h"
#include "log.h"
#undef MPL_LOG_TAG
@@ -48,6 +49,10 @@ struct inv_db_save_t {
/** Temperature when accel bias was stored. */
long accel_temp;
long gyro_temp_slope[3];
+ /** Sensor Accuracy */
+ int gyro_accuracy;
+ int accel_accuracy;
+ int compass_accuracy;
};
struct inv_data_builder_t {
@@ -69,7 +74,7 @@ static struct inv_data_builder_t inv_data_builder;
static struct inv_sensor_cal_t sensors;
/** Change this key if the data being stored by this file changes */
-#define INV_DB_SAVE_KEY 53394
+#define INV_DB_SAVE_KEY 53395
#ifdef INV_PLAYBACK_DBG
@@ -98,6 +103,14 @@ void inv_turn_off_data_logging()
static inv_error_t inv_db_load_func(const unsigned char *data)
{
memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save));
+ // copy in the saved accuracy in the actual sensors accuracy
+ sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy;
+ sensors.accel.accuracy = inv_data_builder.save.accel_accuracy;
+ sensors.compass.accuracy = inv_data_builder.save.compass_accuracy;
+ // TODO
+ if (sensors.compass.accuracy == 3) {
+ inv_set_compass_bias_found(1);
+ }
return INV_SUCCESS;
}
@@ -162,119 +175,7 @@ void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor,
sensor->sensitivity = sensitivity;
sensor->orientation = orientation;
}
-void inv_get_quaternion_transformation(int orientation, long *q)
-{
- long s = 759250125; // sqrt(.5)*2^30
-
- switch (orientation)
- {
- case 0x70:
- q[0] = 759250125;
- q[1] = 759250125;
- q[2] = 0;
- q[3] = 0;
- break;
- case 0x10a:
- q[0] = 759250125;
- q[1] = 0;
- q[2] = 759250125;
- q[3] = 0;
- break;
- case 0x85:
- q[0] = 759250125;
- q[1] = 0;
- q[2] = 0;
- q[3] = 759250125;
- break;
- case 0x181:
- q[0] = 0;
- q[1] = 759250125;
- q[2] = 759250125;
- q[3] = 0;
- break;
- case 0x2a:
- q[0] = 0;
- q[1] = 759250125;
- q[2] = 0;
- q[3] = 759250125;
- break;
- case 0x54:
- q[0] = 0;
- q[1] = 0;
- q[2] = 759250125;
- q[3] = 759250125;
- break;
- case 0x150:
- q[0] = 759250125;
- q[1] = -759250125;
- q[2] = 0;
- q[3] = 0;
- break;
- case 0xe:
- q[0] = 759250125;
- q[1] = 0;
- q[2] = -759250125;
- q[3] = 0;
- break;
- case 0xa1:
- q[0] = 759250125;
- q[1] = 0;
- q[2] = 0;
- q[3] = -759250125;
- break;
- case 0x1a5:
- q[0] = 0;
- q[1] = 759250125;
- q[2] = -759250125;
- q[3] = 0;
- break;
- case 0x12e:
- q[0] = 0;
- q[1] = 759250125;
- q[2] = 0;
- q[3] = -759250125;
- break;
- case 0x174:
- q[0] = 0;
- q[1] = 0;
- q[2] = 759250125;
- q[3] = -759250125;
- break;
-
-
- case 0x88:
- q[0] = 1073741824;
- q[1] = 0;
- q[2] = 0;
- q[3] = 0;
- break;
-
-
- case 0x1a8:
- q[0] = 0;
- q[1] = 1073741824;
- q[2] = 0;
- q[3] = 0;
- break;
-
- case 0x18c:
- q[0] = 0;
- q[1] = 0;
- q[2] = 1073741824;
- q[3] = 0;
- break;
-
- case 0xac:
- q[0] = 0;
- q[1] = 0;
- q[2] = 0;
- q[3] = 1073741824;
- break;
-
- default:
- break;
- }
-}
+
/** Sets the Orientation and Sensitivity of the gyro data.
* @param[in] orientation A scalar defining the transformation from chip mounting
* to the body frame. The function inv_orientation_matrix_to_scalar()
@@ -354,6 +255,22 @@ void inv_set_compass_sample_rate(long sample_rate_us)
sensors.compass.bandwidth = (int)(1000000L / sample_rate_us);
}
}
+
+void inv_get_gyro_sample_rate_ms(long *sample_rate_ms)
+{
+ *sample_rate_ms = sensors.gyro.sample_rate_ms;
+}
+
+void inv_get_accel_sample_rate_ms(long *sample_rate_ms)
+{
+ *sample_rate_ms = sensors.accel.sample_rate_ms;
+}
+
+void inv_get_compass_sample_rate_ms(long *sample_rate_ms)
+{
+ *sample_rate_ms = sensors.compass.sample_rate_ms;
+}
+
/** Set Quat Sample rate in micro seconds.
* @param[in] sample_rate_us Set Quat Sample rate in us
*/
@@ -498,7 +415,7 @@ void inv_matrix_vector_mult(const long *A, const long *x, long *y)
}
/** Takes raw data stored in the sensor, removes bias, and converts it to
-* calibrated data in the body frame.
+* calibrated data in the body frame. Also store raw data for body frame.
* @param[in,out] sensor structure to modify
* @param[in] bias bias in the mounting frame, in hardware units scaled by
* 2^16. Length 3.
@@ -508,15 +425,17 @@ void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias)
long raw32[3];
// Convert raw to calibrated
- raw32[0] = (long)sensor->raw[0] << 16;
- raw32[1] = (long)sensor->raw[1] << 16;
- raw32[2] = (long)sensor->raw[2] << 16;
+ raw32[0] = (long)sensor->raw[0] << 15;
+ raw32[1] = (long)sensor->raw[1] << 15;
+ raw32[2] = (long)sensor->raw[2] << 15;
- raw32[0] -= bias[0];
- raw32[1] -= bias[1];
- raw32[2] -= bias[2];
+ inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_data);
- inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity, raw32, sensor->calibrated);
+ raw32[0] -= bias[0] >> 1;
+ raw32[1] -= bias[1] >> 1;
+ raw32[2] -= bias[2] >> 1;
+
+ inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated);
sensor->status |= INV_CALIBRATED;
}
@@ -539,6 +458,7 @@ void inv_set_compass_bias(const long *bias, int accuracy)
inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
}
sensors.compass.accuracy = accuracy;
+ inv_data_builder.save.compass_accuracy = accuracy;
inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0);
}
@@ -566,6 +486,7 @@ void inv_set_accel_bias(const long *bias, int accuracy)
}
}
sensors.accel.accuracy = accuracy;
+ inv_data_builder.save.accel_accuracy = accuracy;
inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
}
@@ -590,6 +511,7 @@ void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask)
inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias);
}
sensors.accel.accuracy = accuracy;
+ inv_data_builder.save.accel_accuracy = accuracy;
}
@@ -607,6 +529,8 @@ void inv_set_gyro_bias(const long *bias, int accuracy)
}
}
sensors.gyro.accuracy = accuracy;
+ inv_data_builder.save.gyro_accuracy = accuracy;
+
/* TODO: What should we do if there's no temperature data? */
if (sensors.temp.calibrated[0])
inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0];
@@ -687,6 +611,7 @@ inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp)
sensors.accel.calibrated[2] = accel[2];
sensors.accel.status |= INV_CALIBRATED;
sensors.accel.accuracy = status & 3;
+ inv_data_builder.save.accel_accuracy = status & 3;
}
sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON;
sensors.accel.timestamp_prev = sensors.accel.timestamp;
@@ -757,6 +682,7 @@ inv_error_t inv_build_compass(const long *compass, int status,
sensors.compass.calibrated[2] = compass[2];
sensors.compass.status |= INV_CALIBRATED;
sensors.compass.accuracy = status & 3;
+ inv_data_builder.save.compass_accuracy = status & 3;
}
sensors.compass.timestamp_prev = sensors.compass.timestamp;
sensors.compass.timestamp = timestamp;
@@ -1077,6 +1003,22 @@ void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp)
}
}
+/** Gets a whole set of gyro raw data including data, accuracy and timestamp.
+ * @param[out] data Gyro Data where 1 dps = 2^16
+ * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate.
+ * @param[out] timestamp The timestamp of the data sample.
+*/
+void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp)
+{
+ memcpy(data, sensors.gyro.raw_data, sizeof(sensors.gyro.raw_data));
+ if (timestamp != NULL) {
+ *timestamp = sensors.gyro.timestamp;
+ }
+ if (accuracy != NULL) {
+ *accuracy = sensors.gyro.accuracy;
+ }
+}
+
/** Get's latest gyro data.
* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16.
*/
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-24 01:55:20 +0000