diff options
Diffstat (limited to 'libsensors_iio/software/core/mllite/data_builder.c')
-rw-r--r-- | libsensors_iio/software/core/mllite/data_builder.c | 199 |
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. */ |