Tawanda M / Mbed 2 deprecated CSSE4011_BLE_IMU_Project_rev2

CSSE

Dependencies:   BLE_API eMPL_MPU6050 mbed nRF51822

main.cpp@4:0bb9b3bf75ba, 2015-06-04 (annotated)

Committer:
tmushie
Date:
Thu Jun 04 06:06:28 2015 +0000
Revision:
4:0bb9b3bf75ba
Parent:
3:88db44c43914 
kk

Who changed what in which revision?

UserRevisionLine numberNew contents of line
flywind 0:fbd73a00ef9c 1
flywind 0:fbd73a00ef9c 2 #include "mbed.h"
flywind 0:fbd73a00ef9c 3 #include "mbed_i2c.h"
flywind 0:fbd73a00ef9c 4 #include "inv_mpu.h"
flywind 0:fbd73a00ef9c 5 #include "inv_mpu_dmp_motion_driver.h"
tmushie 3:88db44c43914 6 #include <math.h>
tmushie 3:88db44c43914 7 #include "kalman.h"
flywind 0:fbd73a00ef9c 8
flywind 0:fbd73a00ef9c 9 #include "BLEDevice.h"
flywind 0:fbd73a00ef9c 10 #include "DFUService.h"
flywind 0:fbd73a00ef9c 11 #include "UARTService.h"
flywind 0:fbd73a00ef9c 12
tmushie 3:88db44c43914 13
tmushie 3:88db44c43914 14
tmushie 3:88db44c43914 15
tmushie 3:88db44c43914 16
flywind 0:fbd73a00ef9c 17 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
flywind 0:fbd73a00ef9c 18 * it will have an impact on code-size and power consumption. */
flywind 0:fbd73a00ef9c 19
flywind 0:fbd73a00ef9c 20 #if NEED_CONSOLE_OUTPUT
flywind 0:fbd73a00ef9c 21 #define DEBUG(STR) { if (uartServicePtr) uartServicePtr->writeString(STR); }
flywind 0:fbd73a00ef9c 22 #else
flywind 0:fbd73a00ef9c 23 #define DEBUG(...) /* nothing */
flywind 0:fbd73a00ef9c 24 #endif /* #if NEED_CONSOLE_OUTPUT */
flywind 0:fbd73a00ef9c 25
flywind 0:fbd73a00ef9c 26 #define LOG(...) { pc.printf(__VA_ARGS__); }
flywind 0:fbd73a00ef9c 27
flywind 0:fbd73a00ef9c 28 #define LED_GREEN p21
flywind 0:fbd73a00ef9c 29 #define LED_RED p22
flywind 0:fbd73a00ef9c 30 #define LED_BLUE p23
flywind 0:fbd73a00ef9c 31 #define BUTTON_PIN p17
flywind 0:fbd73a00ef9c 32 #define BATTERY_PIN p1
flywind 0:fbd73a00ef9c 33
flywind 0:fbd73a00ef9c 34 #define MPU6050_SDA p12
flywind 0:fbd73a00ef9c 35 #define MPU6050_SCL p13
flywind 0:fbd73a00ef9c 36
flywind 0:fbd73a00ef9c 37 #define UART_TX p9
flywind 0:fbd73a00ef9c 38 #define UART_RX p11
flywind 0:fbd73a00ef9c 39 #define UART_CTS p8
flywind 0:fbd73a00ef9c 40 #define UART_RTS p10
flywind 0:fbd73a00ef9c 41
flywind 0:fbd73a00ef9c 42 /* Starting sampling rate. */
flywind 0:fbd73a00ef9c 43 #define DEFAULT_MPU_HZ (100)
flywind 0:fbd73a00ef9c 44
flywind 0:fbd73a00ef9c 45 DigitalOut blue(LED_BLUE);
flywind 0:fbd73a00ef9c 46 DigitalOut green(LED_GREEN);
flywind 0:fbd73a00ef9c 47 DigitalOut red(LED_RED);
flywind 0:fbd73a00ef9c 48
flywind 0:fbd73a00ef9c 49 InterruptIn button(BUTTON_PIN);
flywind 0:fbd73a00ef9c 50 AnalogIn battery(BATTERY_PIN);
flywind 0:fbd73a00ef9c 51 Serial pc(UART_TX, UART_RX);
flywind 0:fbd73a00ef9c 52
flywind 0:fbd73a00ef9c 53 InterruptIn motion_probe(p14);
flywind 0:fbd73a00ef9c 54
flywind 0:fbd73a00ef9c 55 BLEDevice ble;
flywind 0:fbd73a00ef9c 56 UARTService *uartServicePtr;
flywind 0:fbd73a00ef9c 57
flywind 0:fbd73a00ef9c 58 volatile bool bleIsConnected = false;
flywind 0:fbd73a00ef9c 59 volatile uint8_t tick_event = 0;
flywind 0:fbd73a00ef9c 60 volatile uint8_t motion_event = 0;
flywind 0:fbd73a00ef9c 61 static signed char board_orientation[9] = {
flywind 0:fbd73a00ef9c 62 1, 0, 0,
flywind 0:fbd73a00ef9c 63 0, 1, 0,
flywind 0:fbd73a00ef9c 64 0, 0, 1
flywind 0:fbd73a00ef9c 65 };
tmushie 3:88db44c43914 66
tmushie 3:88db44c43914 67 // Structs for containing filter data
tmushie 3:88db44c43914 68 kalman_data pitch_data;
tmushie 3:88db44c43914 69 kalman_data roll_data;
tmushie 3:88db44c43914 70
flywind 0:fbd73a00ef9c 71 short gyro[3], accel[3], sensors;
flywind 0:fbd73a00ef9c 72 long quat[4];
flywind 0:fbd73a00ef9c 73
flywind 0:fbd73a00ef9c 74 int tick_flag=0;
flywind 0:fbd73a00ef9c 75
flywind 0:fbd73a00ef9c 76 void check_i2c_bus(void);
flywind 0:fbd73a00ef9c 77 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
flywind 0:fbd73a00ef9c 78
flywind 0:fbd73a00ef9c 79 void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params)
flywind 0:fbd73a00ef9c 80 {
flywind 0:fbd73a00ef9c 81 LOG("Connected!\n");
flywind 0:fbd73a00ef9c 82 bleIsConnected = true;
flywind 0:fbd73a00ef9c 83 }
flywind 0:fbd73a00ef9c 84
flywind 0:fbd73a00ef9c 85 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
flywind 0:fbd73a00ef9c 86 {
flywind 0:fbd73a00ef9c 87 LOG("Disconnected!\n");
flywind 0:fbd73a00ef9c 88 LOG("Restarting the advertising process\n");
flywind 0:fbd73a00ef9c 89 ble.startAdvertising();
flywind 0:fbd73a00ef9c 90 bleIsConnected = false;
flywind 0:fbd73a00ef9c 91 }
flywind 0:fbd73a00ef9c 92
flywind 0:fbd73a00ef9c 93 void tick(void)
flywind 0:fbd73a00ef9c 94 {
flywind 0:fbd73a00ef9c 95 green = !green;
flywind 0:fbd73a00ef9c 96 tick_flag++;
flywind 0:fbd73a00ef9c 97
flywind 0:fbd73a00ef9c 98 }
flywind 0:fbd73a00ef9c 99
flywind 0:fbd73a00ef9c 100 void datatoBLE(short *gyro, short *accel, long *quat)
flywind 0:fbd73a00ef9c 101 {
tmushie 3:88db44c43914 102
tmushie 3:88db44c43914 103 /////////////////////////////////////////////////////////////////////////
tmushie 3:88db44c43914 104 ////////////////////////////////////////////////////////////////////////
tmushie 3:88db44c43914 105 float acc_x, acc_y, acc_z;
tmushie 3:88db44c43914 106 float gyr_x, gyr_y, gyr_z;
tmushie 3:88db44c43914 107 float acc_pitch, acc_roll;
tmushie 3:88db44c43914 108 float pitch, roll;
tmushie 3:88db44c43914 109
tmushie 3:88db44c43914 110
tmushie 3:88db44c43914 111 double pitchStar = atan((double)accel[0] / ((accel[0] * accel[0]) + (accel[2] * accel[2])));
tmushie 3:88db44c43914 112 double rollStar = atan((double)accel[1] / ((accel[1] * accel[1]) + (accel[2] * accel[2])));
tmushie 3:88db44c43914 113
tmushie 3:88db44c43914 114 acc_x = (float) accel[0];
tmushie 3:88db44c43914 115 acc_y = (float) accel[1];
tmushie 3:88db44c43914 116 acc_z = (float) accel[2];
tmushie 3:88db44c43914 117
tmushie 3:88db44c43914 118 gyr_x = (float) gyro[0];
tmushie 3:88db44c43914 119 gyr_y = (float) gyro[1];
tmushie 3:88db44c43914 120 gyr_z = (float) gyro[2];
tmushie 3:88db44c43914 121
tmushie 3:88db44c43914 122 acc_pitch = (float) pitchStar;
tmushie 3:88db44c43914 123 acc_roll = (float) rollStar;
tmushie 3:88db44c43914 124
tmushie 3:88db44c43914 125 kalman_init(&pitch_data);
tmushie 3:88db44c43914 126 kalman_init(&roll_data);
tmushie 3:88db44c43914 127 // kalman_init();
tmushie 3:88db44c43914 128 // kalman_init();
tmushie 3:88db44c43914 129
tmushie 3:88db44c43914 130 // Read sensor
tmushie 3:88db44c43914 131 // read_accelerometer(&acc_x, &acc_y, &acc_z);
tmushie 3:88db44c43914 132 // read_gyro(&gyr_x, &gyr_y, &gyr_z);
tmushie 3:88db44c43914 133
tmushie 3:88db44c43914 134 // Calculate pitch and roll based only on acceleration.
tmushie 3:88db44c43914 135 // acc_pitch = atan2(acc_x, -acc_z);
tmushie 3:88db44c43914 136 // acc_roll = -atan2(acc_y, -acc_z);
tmushie 3:88db44c43914 137
tmushie 3:88db44c43914 138 // Perform filtering
tmushie 3:88db44c43914 139 kalman_innovate(&pitch_data, acc_pitch, gyr_x);
tmushie 3:88db44c43914 140 kalman_innovate(&roll_data, acc_roll, gyr_y);
tmushie 3:88db44c43914 141
tmushie 3:88db44c43914 142 // The angle is stored in state 1
tmushie 3:88db44c43914 143 pitch = pitch_data.x1;
tmushie 3:88db44c43914 144 roll = roll_data.x1;
tmushie 3:88db44c43914 145
tmushie 3:88db44c43914 146 // delay(/* some time */);
tmushie 3:88db44c43914 147
tmushie 3:88db44c43914 148
tmushie 3:88db44c43914 149
tmushie 3:88db44c43914 150 ///////////////////////////////////////////////////////////
tmushie 3:88db44c43914 151 ///////////////////////////////////////////////////////////
tmushie 3:88db44c43914 152
tmushie 3:88db44c43914 153
tmushie 3:88db44c43914 154
tmushie 3:88db44c43914 155
tmushie 3:88db44c43914 156
tmushie 3:88db44c43914 157
tmushie 3:88db44c43914 158 // double pitch = atan(0.5);
tmushie 3:88db44c43914 159 // double roll = atan(0.5);
tmushie 3:88db44c43914 160
tmushie 3:88db44c43914 161
flywind 0:fbd73a00ef9c 162 char string[50]="";
flywind 0:fbd73a00ef9c 163 sprintf(string, "ping - %d\r\n",tick_flag);
flywind 0:fbd73a00ef9c 164 DEBUG(string);
flywind 0:fbd73a00ef9c 165 sprintf(string, "GYRO: %d, %d, %d\r\n", gyro[0], gyro[1], gyro[2]);
flywind 0:fbd73a00ef9c 166 DEBUG(string);
flywind 0:fbd73a00ef9c 167 sprintf(string, "ACC: %d, %d, %d\r\n", accel[0], accel[1], accel[2]);
flywind 0:fbd73a00ef9c 168 DEBUG(string);
tmushie 3:88db44c43914 169 sprintf(string, "Pitch: %ld\r\n", pitch);
tmushie 3:88db44c43914 170 DEBUG(string);
tmushie 3:88db44c43914 171 sprintf(string, "Roll: %ld\r\n", roll);
flywind 0:fbd73a00ef9c 172 DEBUG(string);
flywind 0:fbd73a00ef9c 173 }
flywind 0:fbd73a00ef9c 174
flywind 0:fbd73a00ef9c 175
flywind 0:fbd73a00ef9c 176 void detect(void)
flywind 0:fbd73a00ef9c 177 {
flywind 0:fbd73a00ef9c 178 LOG("Button pressed\n");
flywind 0:fbd73a00ef9c 179 DEBUG("Button pressed\n");
flywind 0:fbd73a00ef9c 180 datatoBLE(gyro, accel, quat);
flywind 0:fbd73a00ef9c 181 blue = !blue;
flywind 0:fbd73a00ef9c 182 }
flywind 0:fbd73a00ef9c 183
flywind 0:fbd73a00ef9c 184 void motion_interrupt_handle(void)
flywind 0:fbd73a00ef9c 185 {
flywind 0:fbd73a00ef9c 186 motion_event = 1;
flywind 0:fbd73a00ef9c 187 }
flywind 0:fbd73a00ef9c 188
flywind 0:fbd73a00ef9c 189 void tap_cb(unsigned char direction, unsigned char count)
flywind 0:fbd73a00ef9c 190 {
flywind 0:fbd73a00ef9c 191 LOG("Tap motion detected\n");
flywind 0:fbd73a00ef9c 192 DEBUG("Tap motion detected\n");
flywind 0:fbd73a00ef9c 193 }
flywind 0:fbd73a00ef9c 194
flywind 0:fbd73a00ef9c 195 void android_orient_cb(unsigned char orientation)
flywind 0:fbd73a00ef9c 196 {
flywind 0:fbd73a00ef9c 197 LOG("Oriention changed\n");
flywind 0:fbd73a00ef9c 198 DEBUG("Oriention changed\n");
flywind 0:fbd73a00ef9c 199 }
flywind 0:fbd73a00ef9c 200
flywind 0:fbd73a00ef9c 201
tmushie 3:88db44c43914 202
tmushie 3:88db44c43914 203 // Main function
tmushie 3:88db44c43914 204 int main()
flywind 0:fbd73a00ef9c 205 {
tmushie 3:88db44c43914 206
tmushie 3:88db44c43914 207
flywind 0:fbd73a00ef9c 208 blue = 1;
flywind 0:fbd73a00ef9c 209 green = 1;
flywind 0:fbd73a00ef9c 210 red = 1;
flywind 0:fbd73a00ef9c 211
flywind 0:fbd73a00ef9c 212 pc.baud(115200);
flywind 0:fbd73a00ef9c 213 LOG("---- Seeed Tiny BLE ----\n");
flywind 0:fbd73a00ef9c 214
flywind 0:fbd73a00ef9c 215 Ticker ticker;
flywind 0:fbd73a00ef9c 216 ticker.attach(tick, 3);
flywind 0:fbd73a00ef9c 217
flywind 0:fbd73a00ef9c 218 button.fall(detect);
flywind 0:fbd73a00ef9c 219
flywind 0:fbd73a00ef9c 220 LOG("Initialising the nRF51822\n");
flywind 0:fbd73a00ef9c 221 ble.init();
flywind 0:fbd73a00ef9c 222 ble.onDisconnection(disconnectionCallback);
flywind 2:78552e94d631 223 ble.onConnection((Gap::ConnectionEventCallback_t)connectionCallback);
flywind 0:fbd73a00ef9c 224
flywind 0:fbd73a00ef9c 225
flywind 0:fbd73a00ef9c 226 /* setup advertising */
flywind 0:fbd73a00ef9c 227 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
flywind 0:fbd73a00ef9c 228 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
flywind 0:fbd73a00ef9c 229 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
flywind 0:fbd73a00ef9c 230 (const uint8_t *)"CSSE4011", sizeof("CSSE4011"));
flywind 0:fbd73a00ef9c 231 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
flywind 0:fbd73a00ef9c 232 (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
flywind 0:fbd73a00ef9c 233 DFUService dfu(ble);
flywind 0:fbd73a00ef9c 234 UARTService uartService(ble);
flywind 0:fbd73a00ef9c 235 uartServicePtr = &uartService;
flywind 0:fbd73a00ef9c 236 //uartService.retargetStdout();
flywind 0:fbd73a00ef9c 237
flywind 0:fbd73a00ef9c 238 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
flywind 0:fbd73a00ef9c 239 ble.startAdvertising();
flywind 0:fbd73a00ef9c 240
flywind 0:fbd73a00ef9c 241 check_i2c_bus();
flywind 0:fbd73a00ef9c 242
flywind 0:fbd73a00ef9c 243 mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
flywind 0:fbd73a00ef9c 244 motion_probe.fall(motion_interrupt_handle);
flywind 0:fbd73a00ef9c 245
flywind 0:fbd73a00ef9c 246
flywind 0:fbd73a00ef9c 247 if (mpu_init(0)) {
flywind 0:fbd73a00ef9c 248 // LOG("failed to initialize mpu6050\r\n");
flywind 0:fbd73a00ef9c 249 }
flywind 0:fbd73a00ef9c 250
flywind 0:fbd73a00ef9c 251 /* Get/set hardware configuration. Start gyro. */
flywind 0:fbd73a00ef9c 252 /* Wake up all sensors. */
flywind 0:fbd73a00ef9c 253 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
flywind 0:fbd73a00ef9c 254 /* Push both gyro and accel data into the FIFO. */
flywind 0:fbd73a00ef9c 255 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
flywind 0:fbd73a00ef9c 256 mpu_set_sample_rate(DEFAULT_MPU_HZ);
flywind 0:fbd73a00ef9c 257
flywind 0:fbd73a00ef9c 258 /* Read back configuration in case it was set improperly. */
flywind 0:fbd73a00ef9c 259 unsigned char accel_fsr;
flywind 0:fbd73a00ef9c 260 unsigned short gyro_rate, gyro_fsr;
flywind 0:fbd73a00ef9c 261 mpu_get_sample_rate(&gyro_rate);
flywind 0:fbd73a00ef9c 262 mpu_get_gyro_fsr(&gyro_fsr);
flywind 0:fbd73a00ef9c 263 mpu_get_accel_fsr(&accel_fsr);
flywind 0:fbd73a00ef9c 264
flywind 0:fbd73a00ef9c 265 dmp_load_motion_driver_firmware();
flywind 0:fbd73a00ef9c 266 dmp_set_orientation(
flywind 0:fbd73a00ef9c 267 inv_orientation_matrix_to_scalar(board_orientation));
flywind 0:fbd73a00ef9c 268 dmp_register_tap_cb(tap_cb);
flywind 0:fbd73a00ef9c 269 dmp_register_android_orient_cb(android_orient_cb);
flywind 0:fbd73a00ef9c 270
flywind 0:fbd73a00ef9c 271 uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
flywind 0:fbd73a00ef9c 272 DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
flywind 0:fbd73a00ef9c 273 DMP_FEATURE_GYRO_CAL;
flywind 0:fbd73a00ef9c 274 dmp_enable_feature(dmp_features);
flywind 0:fbd73a00ef9c 275 dmp_set_fifo_rate(DEFAULT_MPU_HZ);
flywind 0:fbd73a00ef9c 276 mpu_set_dmp_state(1);
flywind 0:fbd73a00ef9c 277
flywind 0:fbd73a00ef9c 278 // dmp_set_interrupt_mode(DMP_INT_GESTURE);
flywind 0:fbd73a00ef9c 279 dmp_set_tap_thresh(TAP_XYZ, 50);
flywind 0:fbd73a00ef9c 280
flywind 0:fbd73a00ef9c 281 while (true) {
flywind 0:fbd73a00ef9c 282 if (motion_event) {
flywind 0:fbd73a00ef9c 283 motion_event = 0;
flywind 0:fbd73a00ef9c 284
flywind 0:fbd73a00ef9c 285 unsigned long sensor_timestamp;
flywind 0:fbd73a00ef9c 286
flywind 0:fbd73a00ef9c 287 unsigned char more = 1;
flywind 0:fbd73a00ef9c 288
flywind 0:fbd73a00ef9c 289 while (more) {
flywind 0:fbd73a00ef9c 290 /* This function gets new data from the FIFO when the DMP is in
flywind 0:fbd73a00ef9c 291 * use. The FIFO can contain any combination of gyro, accel,
flywind 0:fbd73a00ef9c 292 * quaternion, and gesture data. The sensors parameter tells the
flywind 0:fbd73a00ef9c 293 * caller which data fields were actually populated with new data.
flywind 0:fbd73a00ef9c 294 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
flywind 0:fbd73a00ef9c 295 * the FIFO isn't being filled with accel data.
flywind 0:fbd73a00ef9c 296 * The driver parses the gesture data to determine if a gesture
flywind 0:fbd73a00ef9c 297 * event has occurred; on an event, the application will be notified
flywind 0:fbd73a00ef9c 298 * via a callback (assuming that a callback function was properly
flywind 0:fbd73a00ef9c 299 * registered). The more parameter is non-zero if there are
flywind 0:fbd73a00ef9c 300 * leftover packets in the FIFO.
flywind 0:fbd73a00ef9c 301 */
flywind 0:fbd73a00ef9c 302 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
flywind 0:fbd73a00ef9c 303 &more);
flywind 0:fbd73a00ef9c 304 /* Gyro and accel data are written to the FIFO by the DMP in chip
flywind 0:fbd73a00ef9c 305 * frame and hardware units. This behavior is convenient because it
flywind 0:fbd73a00ef9c 306 * keeps the gyro and accel outputs of dmp_read_fifo and
flywind 0:fbd73a00ef9c 307 * mpu_read_fifo consistent.
flywind 0:fbd73a00ef9c 308 */
flywind 0:fbd73a00ef9c 309
flywind 0:fbd73a00ef9c 310
flywind 0:fbd73a00ef9c 311 if (sensors & INV_XYZ_GYRO) {
flywind 0:fbd73a00ef9c 312 // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
flywind 0:fbd73a00ef9c 313 }
flywind 0:fbd73a00ef9c 314 if (sensors & INV_XYZ_ACCEL) {
flywind 0:fbd73a00ef9c 315 // LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
flywind 0:fbd73a00ef9c 316 }
flywind 0:fbd73a00ef9c 317
flywind 0:fbd73a00ef9c 318 /* Unlike gyro and accel, quaternions are written to the FIFO in
flywind 0:fbd73a00ef9c 319 * the body frame, q30. The orientation is set by the scalar passed
flywind 0:fbd73a00ef9c 320 * to dmp_set_orientation during initialization.
flywind 0:fbd73a00ef9c 321 */
flywind 0:fbd73a00ef9c 322 if (sensors & INV_WXYZ_QUAT) {
flywind 0:fbd73a00ef9c 323 //LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
flywind 0:fbd73a00ef9c 324 // LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
flywind 0:fbd73a00ef9c 325 //LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
flywind 0:fbd73a00ef9c 326
flywind 0:fbd73a00ef9c 327 }
flywind 0:fbd73a00ef9c 328 }
flywind 0:fbd73a00ef9c 329 } else {
flywind 0:fbd73a00ef9c 330 ble.waitForEvent();
flywind 0:fbd73a00ef9c 331 }
flywind 0:fbd73a00ef9c 332 }
flywind 0:fbd73a00ef9c 333 }
flywind 0:fbd73a00ef9c 334
flywind 0:fbd73a00ef9c 335 void check_i2c_bus(void)
flywind 0:fbd73a00ef9c 336 {
flywind 0:fbd73a00ef9c 337
flywind 0:fbd73a00ef9c 338 DigitalInOut scl(MPU6050_SCL);
flywind 0:fbd73a00ef9c 339 DigitalInOut sda(MPU6050_SDA);
flywind 0:fbd73a00ef9c 340
flywind 0:fbd73a00ef9c 341 scl.input();
flywind 0:fbd73a00ef9c 342 sda.input();
flywind 0:fbd73a00ef9c 343 int scl_level = scl;
flywind 0:fbd73a00ef9c 344 int sda_level = sda;
flywind 0:fbd73a00ef9c 345 if (scl_level == 0 || sda_level == 0) {
flywind 0:fbd73a00ef9c 346 printf("scl: %d, sda: %d, i2c bus is not released\r\n", scl_level, sda_level);
flywind 0:fbd73a00ef9c 347
flywind 0:fbd73a00ef9c 348 scl.output();
flywind 0:fbd73a00ef9c 349 for (int i = 0; i < 8; i++) {
flywind 0:fbd73a00ef9c 350 scl = 0;
flywind 0:fbd73a00ef9c 351 wait_us(10);
flywind 0:fbd73a00ef9c 352 scl = 1;
flywind 0:fbd73a00ef9c 353 wait_us(10);
flywind 0:fbd73a00ef9c 354 }
flywind 0:fbd73a00ef9c 355 }
flywind 0:fbd73a00ef9c 356
flywind 0:fbd73a00ef9c 357 scl.input();
flywind 0:fbd73a00ef9c 358
flywind 0:fbd73a00ef9c 359 scl_level = scl;
flywind 0:fbd73a00ef9c 360 sda_level = sda;
flywind 0:fbd73a00ef9c 361 if (scl_level == 0 || sda_level == 0) {
flywind 0:fbd73a00ef9c 362 printf("scl: %d, sda: %d, i2c bus is still not released\r\n", scl_level, sda_level);
flywind 0:fbd73a00ef9c 363 }
flywind 0:fbd73a00ef9c 364 }
flywind 0:fbd73a00ef9c 365
flywind 0:fbd73a00ef9c 366 /* These next two functions converts the orientation matrix (see
flywind 0:fbd73a00ef9c 367 * gyro_orientation) to a scalar representation for use by the DMP.
flywind 0:fbd73a00ef9c 368 * NOTE: These functions are borrowed from Invensense's MPL.
flywind 0:fbd73a00ef9c 369 */
flywind 0:fbd73a00ef9c 370 static inline unsigned short inv_row_2_scale(const signed char *row)
flywind 0:fbd73a00ef9c 371 {
flywind 0:fbd73a00ef9c 372 unsigned short b;
flywind 0:fbd73a00ef9c 373
flywind 0:fbd73a00ef9c 374 if (row[0] > 0)
flywind 0:fbd73a00ef9c 375 b = 0;
flywind 0:fbd73a00ef9c 376 else if (row[0] < 0)
flywind 0:fbd73a00ef9c 377 b = 4;
flywind 0:fbd73a00ef9c 378 else if (row[1] > 0)
flywind 0:fbd73a00ef9c 379 b = 1;
flywind 0:fbd73a00ef9c 380 else if (row[1] < 0)
flywind 0:fbd73a00ef9c 381 b = 5;
flywind 0:fbd73a00ef9c 382 else if (row[2] > 0)
flywind 0:fbd73a00ef9c 383 b = 2;
flywind 0:fbd73a00ef9c 384 else if (row[2] < 0)
flywind 0:fbd73a00ef9c 385 b = 6;
flywind 0:fbd73a00ef9c 386 else
flywind 0:fbd73a00ef9c 387 b = 7; // error
flywind 0:fbd73a00ef9c 388 return b;
flywind 0:fbd73a00ef9c 389 }
flywind 0:fbd73a00ef9c 390
flywind 0:fbd73a00ef9c 391 unsigned short inv_orientation_matrix_to_scalar(
flywind 0:fbd73a00ef9c 392 const signed char *mtx)
flywind 0:fbd73a00ef9c 393 {
flywind 0:fbd73a00ef9c 394 unsigned short scalar;
flywind 0:fbd73a00ef9c 395
flywind 0:fbd73a00ef9c 396 /*
flywind 0:fbd73a00ef9c 397 XYZ 010_001_000 Identity Matrix
flywind 0:fbd73a00ef9c 398 XZY 001_010_000
flywind 0:fbd73a00ef9c 399 YXZ 010_000_001
flywind 0:fbd73a00ef9c 400 YZX 000_010_001
flywind 0:fbd73a00ef9c 401 ZXY 001_000_010
flywind 0:fbd73a00ef9c 402 ZYX 000_001_010
flywind 0:fbd73a00ef9c 403 */
flywind 0:fbd73a00ef9c 404
flywind 0:fbd73a00ef9c 405 scalar = inv_row_2_scale(mtx);
flywind 0:fbd73a00ef9c 406 scalar |= inv_row_2_scale(mtx + 3) << 3;
flywind 0:fbd73a00ef9c 407 scalar |= inv_row_2_scale(mtx + 6) << 6;
flywind 0:fbd73a00ef9c 408
flywind 0:fbd73a00ef9c 409
flywind 0:fbd73a00ef9c 410 return scalar;
flywind 0:fbd73a00ef9c 411 }
flywind 0:fbd73a00ef9c 412