Pull request for i.a. sensor buffer template
Dependencies: BLE_API MPU6050 mbed nRF51822
main.cpp@1:b529e733ab71, 2018-09-14 (annotated)
- Committer:
- pietermaljaars
- Date:
- Fri Sep 14 19:47:25 2018 +0000
- Revision:
- 1:b529e733ab71
- Parent:
- 0:7c12d0922052
- Child:
- 2:bf1a19d489eb
Add BLE MAC address to broadcast name
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pietermaljaars | 0:7c12d0922052 | 1 | |
pietermaljaars | 0:7c12d0922052 | 2 | #include "mbed.h" |
pietermaljaars | 0:7c12d0922052 | 3 | #include "mbed_i2c.h" |
pietermaljaars | 0:7c12d0922052 | 4 | #include "inv_mpu.h" |
pietermaljaars | 0:7c12d0922052 | 5 | #include "inv_mpu_dmp_motion_driver.h" |
pietermaljaars | 0:7c12d0922052 | 6 | #include "nrf51.h" |
pietermaljaars | 0:7c12d0922052 | 7 | #include "nrf51_bitfields.h" |
pietermaljaars | 0:7c12d0922052 | 8 | |
pietermaljaars | 0:7c12d0922052 | 9 | #include "BLE.h" |
pietermaljaars | 0:7c12d0922052 | 10 | #include "DFUService.h" |
pietermaljaars | 0:7c12d0922052 | 11 | #include "UARTService.h" |
pietermaljaars | 0:7c12d0922052 | 12 | |
pietermaljaars | 0:7c12d0922052 | 13 | |
pietermaljaars | 0:7c12d0922052 | 14 | #define LOG(...) { pc.printf(__VA_ARGS__); } |
pietermaljaars | 0:7c12d0922052 | 15 | |
pietermaljaars | 0:7c12d0922052 | 16 | #define LED_GREEN p21 |
pietermaljaars | 0:7c12d0922052 | 17 | #define LED_RED p22 |
pietermaljaars | 0:7c12d0922052 | 18 | #define LED_BLUE p23 |
pietermaljaars | 0:7c12d0922052 | 19 | #define BUTTON_PIN p17 |
pietermaljaars | 0:7c12d0922052 | 20 | #define BATTERY_PIN p1 |
pietermaljaars | 0:7c12d0922052 | 21 | |
pietermaljaars | 0:7c12d0922052 | 22 | #define MPU6050_SDA p12 |
pietermaljaars | 0:7c12d0922052 | 23 | #define MPU6050_SCL p13 |
pietermaljaars | 0:7c12d0922052 | 24 | |
pietermaljaars | 0:7c12d0922052 | 25 | #define UART_TX p9 |
pietermaljaars | 0:7c12d0922052 | 26 | #define UART_RX p11 |
pietermaljaars | 0:7c12d0922052 | 27 | #define UART_CTS p8 |
pietermaljaars | 0:7c12d0922052 | 28 | #define UART_RTS p10 |
pietermaljaars | 0:7c12d0922052 | 29 | |
pietermaljaars | 0:7c12d0922052 | 30 | /* Starting sampling rate. */ |
pietermaljaars | 0:7c12d0922052 | 31 | #define DEFAULT_MPU_HZ (100) |
pietermaljaars | 0:7c12d0922052 | 32 | |
pietermaljaars | 0:7c12d0922052 | 33 | DigitalOut blue(LED_BLUE); |
pietermaljaars | 0:7c12d0922052 | 34 | DigitalOut green(LED_GREEN); |
pietermaljaars | 0:7c12d0922052 | 35 | DigitalOut red(LED_RED); |
pietermaljaars | 0:7c12d0922052 | 36 | |
pietermaljaars | 0:7c12d0922052 | 37 | InterruptIn button(BUTTON_PIN); |
pietermaljaars | 0:7c12d0922052 | 38 | AnalogIn battery(BATTERY_PIN); |
pietermaljaars | 0:7c12d0922052 | 39 | Serial pc(UART_TX, UART_RX); |
pietermaljaars | 0:7c12d0922052 | 40 | |
pietermaljaars | 0:7c12d0922052 | 41 | InterruptIn motion_probe(p14); |
pietermaljaars | 0:7c12d0922052 | 42 | |
pietermaljaars | 0:7c12d0922052 | 43 | int read_none_count = 0; |
pietermaljaars | 0:7c12d0922052 | 44 | |
pietermaljaars | 0:7c12d0922052 | 45 | BLEDevice ble; |
pietermaljaars | 0:7c12d0922052 | 46 | UARTService *uartServicePtr; |
pietermaljaars | 0:7c12d0922052 | 47 | |
pietermaljaars | 0:7c12d0922052 | 48 | volatile bool bleIsConnected = false; |
pietermaljaars | 0:7c12d0922052 | 49 | volatile uint8_t tick_event = 0; |
pietermaljaars | 0:7c12d0922052 | 50 | volatile uint8_t motion_event = 0; |
pietermaljaars | 0:7c12d0922052 | 51 | static signed char board_orientation[9] = { |
pietermaljaars | 0:7c12d0922052 | 52 | 1, 0, 0, |
pietermaljaars | 0:7c12d0922052 | 53 | 0, 1, 0, |
pietermaljaars | 0:7c12d0922052 | 54 | 0, 0, 1 |
pietermaljaars | 0:7c12d0922052 | 55 | }; |
pietermaljaars | 0:7c12d0922052 | 56 | |
pietermaljaars | 0:7c12d0922052 | 57 | |
pietermaljaars | 0:7c12d0922052 | 58 | void check_i2c_bus(void); |
pietermaljaars | 0:7c12d0922052 | 59 | unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx); |
pietermaljaars | 0:7c12d0922052 | 60 | |
pietermaljaars | 0:7c12d0922052 | 61 | |
pietermaljaars | 0:7c12d0922052 | 62 | void connectionCallback(const Gap::ConnectionCallbackParams_t *params) |
pietermaljaars | 0:7c12d0922052 | 63 | { |
pietermaljaars | 0:7c12d0922052 | 64 | LOG("Connected!\n"); |
pietermaljaars | 0:7c12d0922052 | 65 | bleIsConnected = true; |
pietermaljaars | 0:7c12d0922052 | 66 | } |
pietermaljaars | 0:7c12d0922052 | 67 | |
pietermaljaars | 0:7c12d0922052 | 68 | void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams) |
pietermaljaars | 0:7c12d0922052 | 69 | { |
pietermaljaars | 0:7c12d0922052 | 70 | LOG("Disconnected!\n"); |
pietermaljaars | 0:7c12d0922052 | 71 | LOG("Restarting the advertising process\n"); |
pietermaljaars | 0:7c12d0922052 | 72 | ble.startAdvertising(); |
pietermaljaars | 0:7c12d0922052 | 73 | bleIsConnected = false; |
pietermaljaars | 0:7c12d0922052 | 74 | } |
pietermaljaars | 0:7c12d0922052 | 75 | |
pietermaljaars | 0:7c12d0922052 | 76 | void tick(void) |
pietermaljaars | 0:7c12d0922052 | 77 | { |
pietermaljaars | 0:7c12d0922052 | 78 | static uint32_t count = 0; |
pietermaljaars | 0:7c12d0922052 | 79 | |
pietermaljaars | 0:7c12d0922052 | 80 | LOG("%d\r\n", count++); |
pietermaljaars | 0:7c12d0922052 | 81 | green = !green; |
pietermaljaars | 0:7c12d0922052 | 82 | } |
pietermaljaars | 0:7c12d0922052 | 83 | |
pietermaljaars | 0:7c12d0922052 | 84 | void detect(void) |
pietermaljaars | 0:7c12d0922052 | 85 | { |
pietermaljaars | 0:7c12d0922052 | 86 | LOG("Button pressed\n"); |
pietermaljaars | 0:7c12d0922052 | 87 | blue = !blue; |
pietermaljaars | 0:7c12d0922052 | 88 | } |
pietermaljaars | 0:7c12d0922052 | 89 | |
pietermaljaars | 0:7c12d0922052 | 90 | void motion_interrupt_handle(void) |
pietermaljaars | 0:7c12d0922052 | 91 | { |
pietermaljaars | 0:7c12d0922052 | 92 | motion_event = 1; |
pietermaljaars | 0:7c12d0922052 | 93 | } |
pietermaljaars | 0:7c12d0922052 | 94 | |
pietermaljaars | 0:7c12d0922052 | 95 | void tap_cb(unsigned char direction, unsigned char count) |
pietermaljaars | 0:7c12d0922052 | 96 | { |
pietermaljaars | 0:7c12d0922052 | 97 | LOG("Tap motion detected\n"); |
pietermaljaars | 0:7c12d0922052 | 98 | } |
pietermaljaars | 0:7c12d0922052 | 99 | |
pietermaljaars | 0:7c12d0922052 | 100 | void android_orient_cb(unsigned char orientation) |
pietermaljaars | 0:7c12d0922052 | 101 | { |
pietermaljaars | 0:7c12d0922052 | 102 | LOG("Oriention changed\n"); |
pietermaljaars | 0:7c12d0922052 | 103 | } |
pietermaljaars | 0:7c12d0922052 | 104 | |
pietermaljaars | 0:7c12d0922052 | 105 | |
pietermaljaars | 0:7c12d0922052 | 106 | int main(void) |
pietermaljaars | 0:7c12d0922052 | 107 | { |
pietermaljaars | 0:7c12d0922052 | 108 | blue = 1; |
pietermaljaars | 0:7c12d0922052 | 109 | green = 1; |
pietermaljaars | 0:7c12d0922052 | 110 | red = 1; |
pietermaljaars | 0:7c12d0922052 | 111 | |
pietermaljaars | 0:7c12d0922052 | 112 | pc.baud(115200); |
pietermaljaars | 0:7c12d0922052 | 113 | |
pietermaljaars | 0:7c12d0922052 | 114 | wait(1); |
pietermaljaars | 0:7c12d0922052 | 115 | |
pietermaljaars | 0:7c12d0922052 | 116 | LOG("---- Seeed Tiny BLE ----\n"); |
pietermaljaars | 0:7c12d0922052 | 117 | |
pietermaljaars | 0:7c12d0922052 | 118 | mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL); |
pietermaljaars | 0:7c12d0922052 | 119 | mbed_i2c_init(MPU6050_SDA, MPU6050_SCL); |
pietermaljaars | 0:7c12d0922052 | 120 | |
pietermaljaars | 0:7c12d0922052 | 121 | |
pietermaljaars | 0:7c12d0922052 | 122 | if (mpu_init(0)) { |
pietermaljaars | 0:7c12d0922052 | 123 | LOG("failed to initialize mpu6050\r\n"); |
pietermaljaars | 0:7c12d0922052 | 124 | } |
pietermaljaars | 0:7c12d0922052 | 125 | |
pietermaljaars | 0:7c12d0922052 | 126 | /* Get/set hardware configuration. Start gyro. */ |
pietermaljaars | 0:7c12d0922052 | 127 | /* Wake up all sensors. */ |
pietermaljaars | 0:7c12d0922052 | 128 | mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); |
pietermaljaars | 0:7c12d0922052 | 129 | /* Push both gyro and accel data into the FIFO. */ |
pietermaljaars | 0:7c12d0922052 | 130 | mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); |
pietermaljaars | 0:7c12d0922052 | 131 | mpu_set_sample_rate(DEFAULT_MPU_HZ); |
pietermaljaars | 0:7c12d0922052 | 132 | |
pietermaljaars | 0:7c12d0922052 | 133 | /* Read back configuration in case it was set improperly. */ |
pietermaljaars | 0:7c12d0922052 | 134 | unsigned char accel_fsr; |
pietermaljaars | 0:7c12d0922052 | 135 | unsigned short gyro_rate, gyro_fsr; |
pietermaljaars | 0:7c12d0922052 | 136 | mpu_get_sample_rate(&gyro_rate); |
pietermaljaars | 0:7c12d0922052 | 137 | mpu_get_gyro_fsr(&gyro_fsr); |
pietermaljaars | 0:7c12d0922052 | 138 | mpu_get_accel_fsr(&accel_fsr); |
pietermaljaars | 0:7c12d0922052 | 139 | |
pietermaljaars | 0:7c12d0922052 | 140 | dmp_load_motion_driver_firmware(); |
pietermaljaars | 0:7c12d0922052 | 141 | dmp_set_orientation( |
pietermaljaars | 0:7c12d0922052 | 142 | inv_orientation_matrix_to_scalar(board_orientation)); |
pietermaljaars | 0:7c12d0922052 | 143 | dmp_register_tap_cb(tap_cb); |
pietermaljaars | 0:7c12d0922052 | 144 | dmp_register_android_orient_cb(android_orient_cb); |
pietermaljaars | 0:7c12d0922052 | 145 | |
pietermaljaars | 0:7c12d0922052 | 146 | uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | |
pietermaljaars | 0:7c12d0922052 | 147 | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | |
pietermaljaars | 0:7c12d0922052 | 148 | DMP_FEATURE_GYRO_CAL; |
pietermaljaars | 0:7c12d0922052 | 149 | dmp_enable_feature(dmp_features); |
pietermaljaars | 0:7c12d0922052 | 150 | dmp_set_fifo_rate(DEFAULT_MPU_HZ); |
pietermaljaars | 0:7c12d0922052 | 151 | mpu_set_dmp_state(1); |
pietermaljaars | 0:7c12d0922052 | 152 | |
pietermaljaars | 0:7c12d0922052 | 153 | dmp_set_interrupt_mode(DMP_INT_GESTURE); |
pietermaljaars | 0:7c12d0922052 | 154 | dmp_set_tap_thresh(TAP_XYZ, 50); |
pietermaljaars | 0:7c12d0922052 | 155 | |
pietermaljaars | 0:7c12d0922052 | 156 | |
pietermaljaars | 0:7c12d0922052 | 157 | motion_probe.fall(motion_interrupt_handle); |
pietermaljaars | 0:7c12d0922052 | 158 | |
pietermaljaars | 0:7c12d0922052 | 159 | |
pietermaljaars | 0:7c12d0922052 | 160 | |
pietermaljaars | 0:7c12d0922052 | 161 | Ticker ticker; |
pietermaljaars | 0:7c12d0922052 | 162 | ticker.attach(tick, 3); |
pietermaljaars | 0:7c12d0922052 | 163 | |
pietermaljaars | 0:7c12d0922052 | 164 | button.fall(detect); |
pietermaljaars | 0:7c12d0922052 | 165 | |
pietermaljaars | 0:7c12d0922052 | 166 | LOG("Initialising the nRF51822\n"); |
pietermaljaars | 0:7c12d0922052 | 167 | ble.init(); |
pietermaljaars | 0:7c12d0922052 | 168 | ble.gap().onDisconnection(disconnectionCallback); |
pietermaljaars | 0:7c12d0922052 | 169 | ble.gap().onConnection(connectionCallback); |
pietermaljaars | 1:b529e733ab71 | 170 | |
pietermaljaars | 1:b529e733ab71 | 171 | uint8_t name[] = "iot aabbccddeeff"; |
pietermaljaars | 1:b529e733ab71 | 172 | |
pietermaljaars | 1:b529e733ab71 | 173 | Gap::AddressType_t addr_type; |
pietermaljaars | 1:b529e733ab71 | 174 | Gap::Address_t address; |
pietermaljaars | 1:b529e733ab71 | 175 | ble_error_t error = ble.gap().getAddress(&addr_type, address); |
pietermaljaars | 1:b529e733ab71 | 176 | if (error == BLE_ERROR_NONE) { |
pietermaljaars | 1:b529e733ab71 | 177 | for (int i = 5; i >= 0; i--){ |
pietermaljaars | 1:b529e733ab71 | 178 | //sprintf(name[4 + ((5-i)*2)], "%02x", (uint8_t)address[i]); |
pietermaljaars | 1:b529e733ab71 | 179 | //sprintf(buffer, "%02x", address[i]); |
pietermaljaars | 1:b529e733ab71 | 180 | char buffer[3]; |
pietermaljaars | 1:b529e733ab71 | 181 | sprintf(buffer, "%02x", address[i]); |
pietermaljaars | 1:b529e733ab71 | 182 | name[4 + ((5-i)*2)] = buffer[0]; |
pietermaljaars | 1:b529e733ab71 | 183 | name[4 + ((5-i)*2) + 1] = buffer[1]; |
pietermaljaars | 1:b529e733ab71 | 184 | } |
pietermaljaars | 1:b529e733ab71 | 185 | } |
pietermaljaars | 1:b529e733ab71 | 186 | LOG("name = %s\n", name); |
pietermaljaars | 1:b529e733ab71 | 187 | |
pietermaljaars | 0:7c12d0922052 | 188 | /* setup advertising */ |
pietermaljaars | 0:7c12d0922052 | 189 | ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); |
pietermaljaars | 0:7c12d0922052 | 190 | ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
pietermaljaars | 0:7c12d0922052 | 191 | ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, |
pietermaljaars | 1:b529e733ab71 | 192 | name, sizeof(name)); |
pietermaljaars | 0:7c12d0922052 | 193 | ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, |
pietermaljaars | 0:7c12d0922052 | 194 | (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed)); |
pietermaljaars | 0:7c12d0922052 | 195 | DFUService dfu(ble); |
pietermaljaars | 0:7c12d0922052 | 196 | UARTService uartService(ble); |
pietermaljaars | 0:7c12d0922052 | 197 | uartServicePtr = &uartService; |
pietermaljaars | 0:7c12d0922052 | 198 | //uartService.retargetStdout(); |
pietermaljaars | 0:7c12d0922052 | 199 | |
pietermaljaars | 0:7c12d0922052 | 200 | ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */ |
pietermaljaars | 0:7c12d0922052 | 201 | ble.gap().startAdvertising(); |
pietermaljaars | 0:7c12d0922052 | 202 | |
pietermaljaars | 0:7c12d0922052 | 203 | while (true) { |
pietermaljaars | 0:7c12d0922052 | 204 | if (motion_event) { |
pietermaljaars | 0:7c12d0922052 | 205 | |
pietermaljaars | 0:7c12d0922052 | 206 | unsigned long sensor_timestamp; |
pietermaljaars | 0:7c12d0922052 | 207 | short gyro[3], accel[3], sensors; |
pietermaljaars | 0:7c12d0922052 | 208 | long quat[4]; |
pietermaljaars | 0:7c12d0922052 | 209 | unsigned char more = 1; |
pietermaljaars | 0:7c12d0922052 | 210 | |
pietermaljaars | 0:7c12d0922052 | 211 | while (more) { |
pietermaljaars | 0:7c12d0922052 | 212 | /* This function gets new data from the FIFO when the DMP is in |
pietermaljaars | 0:7c12d0922052 | 213 | * use. The FIFO can contain any combination of gyro, accel, |
pietermaljaars | 0:7c12d0922052 | 214 | * quaternion, and gesture data. The sensors parameter tells the |
pietermaljaars | 0:7c12d0922052 | 215 | * caller which data fields were actually populated with new data. |
pietermaljaars | 0:7c12d0922052 | 216 | * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then |
pietermaljaars | 0:7c12d0922052 | 217 | * the FIFO isn't being filled with accel data. |
pietermaljaars | 0:7c12d0922052 | 218 | * The driver parses the gesture data to determine if a gesture |
pietermaljaars | 0:7c12d0922052 | 219 | * event has occurred; on an event, the application will be notified |
pietermaljaars | 0:7c12d0922052 | 220 | * via a callback (assuming that a callback function was properly |
pietermaljaars | 0:7c12d0922052 | 221 | * registered). The more parameter is non-zero if there are |
pietermaljaars | 0:7c12d0922052 | 222 | * leftover packets in the FIFO. |
pietermaljaars | 0:7c12d0922052 | 223 | */ |
pietermaljaars | 0:7c12d0922052 | 224 | dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, |
pietermaljaars | 0:7c12d0922052 | 225 | &more); |
pietermaljaars | 0:7c12d0922052 | 226 | |
pietermaljaars | 0:7c12d0922052 | 227 | |
pietermaljaars | 0:7c12d0922052 | 228 | /* Gyro and accel data are written to the FIFO by the DMP in chip |
pietermaljaars | 0:7c12d0922052 | 229 | * frame and hardware units. This behavior is convenient because it |
pietermaljaars | 0:7c12d0922052 | 230 | * keeps the gyro and accel outputs of dmp_read_fifo and |
pietermaljaars | 0:7c12d0922052 | 231 | * mpu_read_fifo consistent. |
pietermaljaars | 0:7c12d0922052 | 232 | */ |
pietermaljaars | 0:7c12d0922052 | 233 | if (sensors & INV_XYZ_GYRO) { |
pietermaljaars | 0:7c12d0922052 | 234 | // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]); |
pietermaljaars | 0:7c12d0922052 | 235 | } |
pietermaljaars | 0:7c12d0922052 | 236 | if (sensors & INV_XYZ_ACCEL) { |
pietermaljaars | 0:7c12d0922052 | 237 | //LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]); |
pietermaljaars | 0:7c12d0922052 | 238 | } |
pietermaljaars | 0:7c12d0922052 | 239 | |
pietermaljaars | 0:7c12d0922052 | 240 | /* Unlike gyro and accel, quaternions are written to the FIFO in |
pietermaljaars | 0:7c12d0922052 | 241 | * the body frame, q30. The orientation is set by the scalar passed |
pietermaljaars | 0:7c12d0922052 | 242 | * to dmp_set_orientation during initialization. |
pietermaljaars | 0:7c12d0922052 | 243 | */ |
pietermaljaars | 0:7c12d0922052 | 244 | if (sensors & INV_WXYZ_QUAT) { |
pietermaljaars | 0:7c12d0922052 | 245 | // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]); |
pietermaljaars | 0:7c12d0922052 | 246 | } |
pietermaljaars | 0:7c12d0922052 | 247 | |
pietermaljaars | 0:7c12d0922052 | 248 | if (sensors) { |
pietermaljaars | 0:7c12d0922052 | 249 | read_none_count = 0; |
pietermaljaars | 0:7c12d0922052 | 250 | } else { |
pietermaljaars | 0:7c12d0922052 | 251 | read_none_count++; |
pietermaljaars | 0:7c12d0922052 | 252 | if (read_none_count > 3) { |
pietermaljaars | 0:7c12d0922052 | 253 | read_none_count = 0; |
pietermaljaars | 0:7c12d0922052 | 254 | |
pietermaljaars | 0:7c12d0922052 | 255 | LOG("I2C may be stuck @ %d\r\n", sensor_timestamp); |
pietermaljaars | 0:7c12d0922052 | 256 | mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL); |
pietermaljaars | 0:7c12d0922052 | 257 | } |
pietermaljaars | 0:7c12d0922052 | 258 | } |
pietermaljaars | 0:7c12d0922052 | 259 | } |
pietermaljaars | 0:7c12d0922052 | 260 | |
pietermaljaars | 0:7c12d0922052 | 261 | motion_event = 0; |
pietermaljaars | 0:7c12d0922052 | 262 | } else { |
pietermaljaars | 0:7c12d0922052 | 263 | ble.waitForEvent(); |
pietermaljaars | 0:7c12d0922052 | 264 | } |
pietermaljaars | 0:7c12d0922052 | 265 | } |
pietermaljaars | 0:7c12d0922052 | 266 | } |
pietermaljaars | 0:7c12d0922052 | 267 | |
pietermaljaars | 0:7c12d0922052 | 268 | /* These next two functions converts the orientation matrix (see |
pietermaljaars | 0:7c12d0922052 | 269 | * gyro_orientation) to a scalar representation for use by the DMP. |
pietermaljaars | 0:7c12d0922052 | 270 | * NOTE: These functions are borrowed from Invensense's MPL. |
pietermaljaars | 0:7c12d0922052 | 271 | */ |
pietermaljaars | 0:7c12d0922052 | 272 | static inline unsigned short inv_row_2_scale(const signed char *row) |
pietermaljaars | 0:7c12d0922052 | 273 | { |
pietermaljaars | 0:7c12d0922052 | 274 | unsigned short b; |
pietermaljaars | 0:7c12d0922052 | 275 | |
pietermaljaars | 0:7c12d0922052 | 276 | if (row[0] > 0) |
pietermaljaars | 0:7c12d0922052 | 277 | b = 0; |
pietermaljaars | 0:7c12d0922052 | 278 | else if (row[0] < 0) |
pietermaljaars | 0:7c12d0922052 | 279 | b = 4; |
pietermaljaars | 0:7c12d0922052 | 280 | else if (row[1] > 0) |
pietermaljaars | 0:7c12d0922052 | 281 | b = 1; |
pietermaljaars | 0:7c12d0922052 | 282 | else if (row[1] < 0) |
pietermaljaars | 0:7c12d0922052 | 283 | b = 5; |
pietermaljaars | 0:7c12d0922052 | 284 | else if (row[2] > 0) |
pietermaljaars | 0:7c12d0922052 | 285 | b = 2; |
pietermaljaars | 0:7c12d0922052 | 286 | else if (row[2] < 0) |
pietermaljaars | 0:7c12d0922052 | 287 | b = 6; |
pietermaljaars | 0:7c12d0922052 | 288 | else |
pietermaljaars | 0:7c12d0922052 | 289 | b = 7; // error |
pietermaljaars | 0:7c12d0922052 | 290 | return b; |
pietermaljaars | 0:7c12d0922052 | 291 | } |
pietermaljaars | 0:7c12d0922052 | 292 | |
pietermaljaars | 0:7c12d0922052 | 293 | unsigned short inv_orientation_matrix_to_scalar( |
pietermaljaars | 0:7c12d0922052 | 294 | const signed char *mtx) |
pietermaljaars | 0:7c12d0922052 | 295 | { |
pietermaljaars | 0:7c12d0922052 | 296 | unsigned short scalar; |
pietermaljaars | 0:7c12d0922052 | 297 | |
pietermaljaars | 0:7c12d0922052 | 298 | /* |
pietermaljaars | 0:7c12d0922052 | 299 | XYZ 010_001_000 Identity Matrix |
pietermaljaars | 0:7c12d0922052 | 300 | XZY 001_010_000 |
pietermaljaars | 0:7c12d0922052 | 301 | YXZ 010_000_001 |
pietermaljaars | 0:7c12d0922052 | 302 | YZX 000_010_001 |
pietermaljaars | 0:7c12d0922052 | 303 | ZXY 001_000_010 |
pietermaljaars | 0:7c12d0922052 | 304 | ZYX 000_001_010 |
pietermaljaars | 0:7c12d0922052 | 305 | */ |
pietermaljaars | 0:7c12d0922052 | 306 | |
pietermaljaars | 0:7c12d0922052 | 307 | scalar = inv_row_2_scale(mtx); |
pietermaljaars | 0:7c12d0922052 | 308 | scalar |= inv_row_2_scale(mtx + 3) << 3; |
pietermaljaars | 0:7c12d0922052 | 309 | scalar |= inv_row_2_scale(mtx + 6) << 6; |
pietermaljaars | 0:7c12d0922052 | 310 | |
pietermaljaars | 0:7c12d0922052 | 311 | |
pietermaljaars | 0:7c12d0922052 | 312 | return scalar; |
pietermaljaars | 0:7c12d0922052 | 313 | } |
pietermaljaars | 0:7c12d0922052 | 314 |