1 /* 2 $License: 3 Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 4 See included License.txt for License information. 5 $ 6 */ 7 #ifndef INV_DATA_BUILDER_H__ 8 #define INV_DATA_BUILDER_H__ 9 10 #include <stdio.h> 11 #include "mltypes.h" 12 13 #ifdef __cplusplus 14 extern "C" { 15 #endif 16 17 // Uncomment this flag to enable playback debug and record or playback scenarios 18 //#define INV_PLAYBACK_DBG 19 20 /** This is a new sample of accel data */ 21 #define INV_ACCEL_NEW 1 22 /** This is a new sample of gyro data */ 23 #define INV_GYRO_NEW 2 24 /** This is a new sample of compass data */ 25 #define INV_MAG_NEW 4 26 /** This is a new sample of temperature data */ 27 #define INV_TEMP_NEW 8 28 /** This is a new sample of quaternion data */ 29 #define INV_QUAT_NEW 16 30 /** This is a new sample of pressure data */ 31 #define INV_PRESSURE_NEW 32 32 33 /** Set if the data is contiguous. Typically not set if a sample was skipped */ 34 #define INV_CONTIGUOUS 16 35 /** Set if the calibrated data has been solved for */ 36 #define INV_CALIBRATED 32 37 /** INV_NEW_DATA set for a new set of data, cleared if not available. */ 38 #define INV_NEW_DATA 64 39 /** Set if raw data exists */ 40 #define INV_RAW_DATA 128 41 /** Set if the sensor is on */ 42 #define INV_SENSOR_ON 256 43 /** Set if quaternion has bias correction applied */ 44 #define INV_BIAS_APPLIED 512 45 /** Set if quaternion is 6-axis from DMP */ 46 #define INV_QUAT_6AXIS 1024 47 /** Set if quaternion is 9 axis from DMP */ 48 #define INV_QUAT_9AXIS 2048 49 /** Set if quaternion is 3-axis from DMP */ 50 #define INV_QUAT_3AXIS 4096 51 /** Set if DMP has applied bias */ 52 #define INV_DMP_BIAS_APPLIED 8192 53 /** Set if quaternion is 3 elements (android only) */ 54 #define INV_QUAT_3ELEMENT 16384 55 56 #define INV_PRIORITY_MOTION_NO_MOTION 100 57 #define INV_PRIORITY_GYRO_TC 150 58 #define INV_PRIORITY_QUATERNION_GYRO_ACCEL 200 59 #define INV_PRIORITY_QUATERNION_NO_GYRO 250 60 #define INV_PRIORITY_MAGNETIC_DISTURBANCE 300 61 #define INV_PRIORITY_HEADING_FROM_GYRO 350 62 #define INV_PRIORITY_COMPASS_BIAS_W_GYRO 375 63 #define INV_PRIORITY_COMPASS_VECTOR_CAL 400 64 #define INV_PRIORITY_COMPASS_ADV_BIAS 500 65 #define INV_PRIORITY_9_AXIS_FUSION 600 66 #define INV_PRIORITY_9_AXIS_FUSION_LIGHT 650 67 #define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS 700 68 #define INV_PRIORITY_QUATERNION_ACCURACY 750 69 #define INV_PRIORITY_RESULTS_HOLDER 800 70 #define INV_PRIORITY_INUSE_AUTO_CALIBRATION 850 71 #define INV_PRIORITY_HAL_OUTPUTS 900 72 #define INV_PRIORITY_GLYPH 950 73 #define INV_PRIORITY_SHAKE 975 74 #define INV_PRIORITY_SM 1000 75 76 struct inv_single_sensor_t { 77 /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when 78 * the rotation matrix could be thought of only having elements of 0,1,-1. 79 * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign. 80 * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row. 81 * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row. 82 * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row. 83 */ 84 int orientation; 85 /** The raw data in raw data units in the mounting frame */ 86 short raw[3]; 87 /** Raw data in body frame */ 88 long raw_scaled[3]; 89 /** Calibrated data */ 90 long calibrated[3]; 91 long sensitivity; 92 /** Sample rate in microseconds */ 93 long sample_rate_us; 94 long sample_rate_ms; 95 /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample 96 * skipped due to power savings turning off this sensor. 97 * INV_NEW_DATA set for a new set of data, cleared if not available. 98 * INV_CALIBRATED_SET if calibrated data has been solved for */ 99 int status; 100 /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */ 101 int accuracy; 102 inv_time_t timestamp; 103 inv_time_t timestamp_prev; 104 /** Bandwidth in Hz */ 105 int bandwidth; 106 }; 107 108 struct inv_quat_sensor_t { 109 long raw[4]; 110 /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample 111 * skipped due to power savings turning off this sensor. 112 * INV_NEW_DATA set for a new set of data, cleared if not available. 113 * INV_CALIBRATED_SET if calibrated data has been solved for */ 114 int status; 115 inv_time_t timestamp; 116 inv_time_t timestamp_prev; 117 long sample_rate_us; 118 long sample_rate_ms; 119 }; 120 121 struct inv_soft_iron_t { 122 long raw[3]; 123 long trans[3]; 124 long matrix_d[9]; // Q30 format fixed point. The dynamic range is (-2.0 to 2.0); 125 float matrix_f[9]; 126 127 int enable; 128 }; 129 130 struct inv_sensor_cal_t { 131 struct inv_single_sensor_t gyro; 132 struct inv_single_sensor_t accel; 133 struct inv_single_sensor_t compass; 134 struct inv_single_sensor_t temp; 135 struct inv_quat_sensor_t quat; 136 struct inv_single_sensor_t pressure; 137 struct inv_soft_iron_t soft_iron; 138 /** Combinations of INV_GYRO_NEW, INV_ACCEL_NEW, INV_MAG_NEW to indicate 139 * which data is a new sample as these data points may have different sample rates. 140 */ 141 int status; 142 }; 143 144 // Useful for debug record and playback 145 typedef enum { 146 RD_NO_DEBUG, 147 RD_RECORD, 148 RD_PLAYBACK 149 } rd_dbg_mode; 150 151 typedef enum { 152 PLAYBACK_DBG_TYPE_GYRO, 153 PLAYBACK_DBG_TYPE_ACCEL, 154 PLAYBACK_DBG_TYPE_COMPASS, 155 PLAYBACK_DBG_TYPE_TEMPERATURE, 156 PLAYBACK_DBG_TYPE_EXECUTE, 157 PLAYBACK_DBG_TYPE_A_ORIENT, 158 PLAYBACK_DBG_TYPE_G_ORIENT, 159 PLAYBACK_DBG_TYPE_C_ORIENT, 160 PLAYBACK_DBG_TYPE_A_SAMPLE_RATE, 161 PLAYBACK_DBG_TYPE_C_SAMPLE_RATE, 162 PLAYBACK_DBG_TYPE_G_SAMPLE_RATE, 163 PLAYBACK_DBG_TYPE_GYRO_OFF, 164 PLAYBACK_DBG_TYPE_ACCEL_OFF, 165 PLAYBACK_DBG_TYPE_COMPASS_OFF, 166 PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE, 167 PLAYBACK_DBG_TYPE_QUAT, 168 PLAYBACK_DBG_TYPE_QUAT_OFF 169 } inv_rd_dbg_states; 170 171 /** Change this key if the definition of the struct inv_db_save_t changes. 172 Previous keys: 53394, 53395, 53396 */ 173 #define INV_DB_SAVE_KEY (53397) 174 175 #define INV_DB_SAVE_MPL_KEY (50001) 176 #define INV_DB_SAVE_ACCEL_MPL_KEY (50002) 177 178 struct inv_db_save_t { 179 /** compass Bias in chip frame, hardware units scaled by 2^16. */ 180 long compass_bias[3]; 181 /** gyro factory bias in chip frame, hardware units scaled by 2^16, 182 +/- 2000 dps full scale. */ 183 long factory_gyro_bias[3]; 184 /** accel factory bias in chip frame, hardware units scaled by 2^16, 185 +/- 2 gee full scale. */ 186 long factory_accel_bias[3]; 187 /** temperature when factory_gyro_bias was stored. */ 188 long gyro_temp; 189 /** flag to indicate temperature compensation that biases where stored. */ 190 int gyro_bias_tc_set; 191 /** temperature when accel bias was stored. */ 192 long accel_temp; 193 long gyro_temp_slope[3]; 194 /** sensor accuracies */ 195 int gyro_accuracy; 196 int accel_accuracy; 197 int compass_accuracy; 198 }; 199 200 struct inv_db_save_mpl_t { 201 /** gyro bias in chip frame, hardware units scaled by 2^16, +/- 2000 dps 202 full scale */ 203 long gyro_bias[3]; 204 }; 205 206 struct inv_db_save_accel_mpl_t { 207 /** accel bias in chip frame, hardware units scaled by 2^16, +/- 2 gee 208 full scale */ 209 long accel_bias[3]; 210 }; 211 212 /** Maximum number of data callbacks that are supported. Safe to increase if needed.*/ 213 #define INV_MAX_DATA_CB 20 214 215 #ifdef INV_PLAYBACK_DBG 216 void inv_turn_on_data_logging(FILE *file); 217 void inv_turn_off_data_logging(); 218 #endif 219 220 void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity); 221 void inv_set_accel_orientation_and_scale(int orientation, 222 long sensitivity); 223 void inv_set_compass_orientation_and_scale(int orientation, 224 long sensitivity); 225 void inv_set_gyro_sample_rate(long sample_rate_us); 226 void inv_set_compass_sample_rate(long sample_rate_us); 227 void inv_set_quat_sample_rate(long sample_rate_us); 228 void inv_set_accel_sample_rate(long sample_rate_us); 229 void inv_set_gyro_bandwidth(int bandwidth_hz); 230 void inv_set_accel_bandwidth(int bandwidth_hz); 231 void inv_set_compass_bandwidth(int bandwidth_hz); 232 233 void inv_get_gyro_sample_rate_ms(long *sample_rate_ms); 234 void inv_get_accel_sample_rate_ms(long *sample_rate_ms); 235 void inv_get_compass_sample_rate_ms(long *sample_rate_ms); 236 237 inv_error_t inv_register_data_cb(inv_error_t (*func) 238 (struct inv_sensor_cal_t * data), int priority, 239 int sensor_type); 240 inv_error_t inv_unregister_data_cb(inv_error_t (*func) 241 (struct inv_sensor_cal_t * data)); 242 243 inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp); 244 inv_error_t inv_build_compass(const long *compass, int status, 245 inv_time_t timestamp); 246 inv_error_t inv_build_accel(const long *accel, int status, 247 inv_time_t timestamp); 248 inv_error_t inv_build_temp(const long temp, inv_time_t timestamp); 249 inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp); 250 inv_error_t inv_build_pressure(const long pressure, int status, inv_time_t timestamp); 251 inv_error_t inv_execute_on_data(void); 252 253 void inv_get_compass_bias(long *bias); 254 255 void inv_set_compass_bias(const long *bias, int accuracy); 256 void inv_set_compass_disturbance(int dist); 257 void inv_set_gyro_bias(const long *bias); 258 void inv_set_mpl_gyro_bias(const long *bias, int accuracy); 259 void inv_set_accel_bias(const long *bias); 260 void inv_set_mpl_accel_bias(const long *bias, int accuracy); 261 void inv_set_accel_accuracy(int accuracy); 262 void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask); 263 264 void inv_get_compass_soft_iron_matrix_d(long *matrix); 265 void inv_set_compass_soft_iron_matrix_d(long *matrix); 266 267 void inv_get_compass_soft_iron_matrix_f(float *matrix); 268 void inv_set_compass_soft_iron_matrix_f(float *matrix); 269 270 void inv_get_compass_soft_iron_output_data(long *data); 271 void inv_get_compass_soft_iron_input_data(long *data); 272 void inv_set_compass_soft_iron_input_data(const long *data); 273 274 void inv_reset_compass_soft_iron_matrix(void); 275 void inv_enable_compass_soft_iron_matrix(void); 276 void inv_disable_compass_soft_iron_matrix(void); 277 278 void inv_get_mpl_gyro_bias(long *bias, long *temp); 279 void inv_get_gyro_bias(long *bias); 280 void inv_get_gyro_bias_dmp_units(long *bias); 281 int inv_get_factory_accel_bias_mask(); 282 void inv_get_mpl_accel_bias(long *bias, long *temp); 283 void inv_get_accel_bias(long *bias); 284 void inv_get_accel_bias_dmp_units(long *bias); 285 286 void inv_gyro_was_turned_off(void); 287 void inv_accel_was_turned_off(void); 288 void inv_compass_was_turned_off(void); 289 void inv_quaternion_sensor_was_turned_off(void); 290 inv_error_t inv_init_data_builder(void); 291 long inv_get_gyro_sensitivity(void); 292 long inv_get_accel_sensitivity(void); 293 long inv_get_compass_sensitivity(void); 294 295 void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp); 296 void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t * timestamp); 297 void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp); 298 void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp); 299 void inv_get_compass_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp); 300 301 void inv_get_gyro(long *gyro); 302 303 int inv_get_gyro_accuracy(void); 304 int inv_get_accel_accuracy(void); 305 int inv_get_mag_accuracy(void); 306 void inv_get_raw_compass(short *raw); 307 308 int inv_get_compass_on(void); 309 int inv_get_gyro_on(void); 310 int inv_get_accel_on(void); 311 312 inv_time_t inv_get_last_timestamp(void); 313 int inv_get_compass_disturbance(void); 314 315 // new DMP cal functions 316 inv_error_t inv_get_gyro_orient(int *orient); 317 inv_error_t inv_get_accel_orient(int *orient); 318 319 #ifdef WIN32 320 void inv_overwrite_dmp_9quat(void); 321 #endif 322 323 // internal 324 int inv_get_gyro_bias_tc_set(void); 325 int inv_get_9_axis_timestamp(long sample_rate_us, inv_time_t *ts); 326 int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts); 327 int inv_get_6_axis_compass_accel_timestamp(long sample_rate_us, inv_time_t *ts); 328 329 #ifdef __cplusplus 330 } 331 #endif 332 333 #endif /* INV_DATA_BUILDER_H__ */ 334