summaryrefslogtreecommitdiffstats
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.c199
1 files changed, 122 insertions, 77 deletions
diff --git a/libsensors_iio/software/core/mllite/data_builder.c b/libsensors_iio/software/core/mllite/data_builder.c
index 48993bc..f70be7c 100644
--- a/libsensors_iio/software/core/mllite/data_builder.c
+++ b/libsensors_iio/software/core/mllite/data_builder.c
@@ -18,14 +18,11 @@
#undef MPL_LOG_NDEBUG
#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */
-#include <string.h>
-
#include "ml_math_func.h"
#include "data_builder.h"
#include "mlmath.h"
#include "storage_manager.h"
#include "message_layer.h"
-#include "results_holder.h"
#include "log.h"
#undef MPL_LOG_TAG
@@ -51,10 +48,6 @@ 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 {
@@ -76,7 +69,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 53395
+#define INV_DB_SAVE_KEY 53394
#ifdef INV_PLAYBACK_DBG
@@ -105,14 +98,6 @@ 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;
}
@@ -177,7 +162,119 @@ 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()
@@ -257,22 +354,6 @@ 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
*/
@@ -417,7 +498,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. Also store raw data for body frame.
+* calibrated data in the 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.
@@ -427,17 +508,15 @@ 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] << 15;
- raw32[1] = (long)sensor->raw[1] << 15;
- raw32[2] = (long)sensor->raw[2] << 15;
-
- inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled);
+ raw32[0] = (long)sensor->raw[0] << 16;
+ raw32[1] = (long)sensor->raw[1] << 16;
+ raw32[2] = (long)sensor->raw[2] << 16;
- raw32[0] -= bias[0] >> 1;
- raw32[1] -= bias[1] >> 1;
- raw32[2] -= bias[2] >> 1;
+ 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->calibrated);
+ inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity, raw32, sensor->calibrated);
sensor->status |= INV_CALIBRATED;
}
@@ -460,7 +539,6 @@ 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);
}
@@ -488,17 +566,6 @@ 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);
-}
-
-/** Sets the accel accuracy.
-* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate.
-*/
-void inv_set_accel_accuracy(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);
}
@@ -523,8 +590,6 @@ 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;
- inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0);
}
@@ -542,8 +607,6 @@ 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];
@@ -624,7 +687,6 @@ 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;
@@ -695,7 +757,6 @@ 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;
@@ -1016,22 +1077,6 @@ 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_scaled, sizeof(sensors.gyro.raw_scaled));
- 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.
*/