main.cpp

00001 
00002 #include "mbed.h"
00003 #include "mbed_i2c.h"
00004 #include "inv_mpu.h"
00005 #include "inv_mpu_dmp_motion_driver.h"
00006 #include <math.h>
00007 #include "kalman.h"
00008 
00009 #include "BLEDevice.h"
00010 #include "DFUService.h"
00011 #include "UARTService.h"
00012 
00013 
00014  
00015 
00016 
00017 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
00018 * it will have an impact on code-size and power consumption. */
00019 
00020 #if NEED_CONSOLE_OUTPUT
00021 #define DEBUG(STR) { if (uartServicePtr) uartServicePtr->writeString(STR); }
00022 #else
00023 #define DEBUG(...) /* nothing */
00024 #endif /* #if NEED_CONSOLE_OUTPUT */
00025 
00026 #define LOG(...)    { pc.printf(__VA_ARGS__); }
00027 
00028 #define LED_GREEN   p21
00029 #define LED_RED     p22
00030 #define LED_BLUE    p23
00031 #define BUTTON_PIN  p17
00032 #define BATTERY_PIN p1
00033 
00034 #define MPU6050_SDA p12
00035 #define MPU6050_SCL p13
00036 
00037 #define UART_TX     p9
00038 #define UART_RX     p11
00039 #define UART_CTS    p8
00040 #define UART_RTS    p10
00041 
00042 /* Starting sampling rate. */
00043 #define DEFAULT_MPU_HZ  (100)
00044 
00045 DigitalOut blue(LED_BLUE);
00046 DigitalOut green(LED_GREEN);
00047 DigitalOut red(LED_RED);
00048 
00049 InterruptIn button(BUTTON_PIN);
00050 AnalogIn    battery(BATTERY_PIN);
00051 Serial pc(UART_TX, UART_RX);
00052 
00053 InterruptIn motion_probe(p14);
00054 
00055 BLEDevice  ble;
00056 UARTService *uartServicePtr;
00057 
00058 volatile bool bleIsConnected = false;
00059 volatile uint8_t tick_event = 0;
00060 volatile uint8_t motion_event = 0;
00061 static signed char board_orientation[9] = {
00062     1, 0, 0,
00063     0, 1, 0,
00064     0, 0, 1
00065 };
00066 
00067 // Structs for containing filter data
00068 kalman_data pitch_data;
00069 kalman_data roll_data;
00070 
00071 short gyro[3], accel[3], sensors;
00072 long quat[4];
00073 
00074 int tick_flag=0;
00075 
00076 void check_i2c_bus(void);
00077 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
00078 
00079 void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params)
00080 {
00081     LOG("Connected!\n");
00082     bleIsConnected = true;
00083 }
00084 
00085 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
00086 {
00087     LOG("Disconnected!\n");
00088     LOG("Restarting the advertising process\n");
00089     ble.startAdvertising();
00090     bleIsConnected = false;
00091 }
00092 
00093 void tick(void)
00094 {
00095     green = !green;
00096     tick_flag++;
00097 
00098 }
00099 
00100 void datatoBLE(short *gyro, short *accel, long *quat)
00101 {
00102     
00103     /////////////////////////////////////////////////////////////////////////
00104 ////////////////////////////////////////////////////////////////////////
00105     float acc_x, acc_y, acc_z;
00106     float gyr_x, gyr_y, gyr_z;
00107     float acc_pitch, acc_roll;
00108     float pitch, roll;
00109  
00110  
00111     double pitchStar = atan((double)accel[0] / ((accel[0] * accel[0]) + (accel[2] * accel[2])));
00112     double rollStar = atan((double)accel[1] / ((accel[1] * accel[1]) + (accel[2] * accel[2])));
00113     
00114     acc_x = (float) accel[0];
00115     acc_y = (float) accel[1];
00116     acc_z = (float) accel[2];
00117  
00118     gyr_x = (float) gyro[0];
00119     gyr_y = (float) gyro[1];
00120     gyr_z = (float) gyro[2];
00121  
00122     acc_pitch = (float) pitchStar;
00123     acc_roll  = (float) rollStar;
00124  
00125     kalman_init(&pitch_data);
00126     kalman_init(&roll_data);
00127  //    kalman_init();
00128  //    kalman_init();
00129    
00130         // Read sensor
00131  //       read_accelerometer(&acc_x, &acc_y, &acc_z);
00132  //       read_gyro(&gyr_x, &gyr_y, &gyr_z);
00133  
00134         // Calculate pitch and roll based only on acceleration.
00135 //        acc_pitch = atan2(acc_x, -acc_z);
00136  //       acc_roll = -atan2(acc_y, -acc_z);
00137  
00138         // Perform filtering
00139         kalman_innovate(&pitch_data, acc_pitch, gyr_x);
00140         kalman_innovate(&roll_data, acc_roll, gyr_y);
00141  
00142         // The angle is stored in state 1
00143         pitch = pitch_data.x1;
00144         roll = roll_data.x1;
00145  
00146      //   delay(/* some time */);
00147    
00148 
00149 
00150 ///////////////////////////////////////////////////////////
00151 /////////////////////////////////////////////////////////// 
00152     
00153     
00154     
00155     
00156 
00157 
00158  //   double pitch = atan(0.5);
00159  //   double roll = atan(0.5);
00160     
00161     
00162     char string[50]="";
00163     sprintf(string, "ping - %d\r\n",tick_flag);
00164     DEBUG(string);
00165     sprintf(string, "GYRO: %d, %d, %d\r\n", gyro[0], gyro[1], gyro[2]);
00166     DEBUG(string);
00167     sprintf(string, "ACC: %d, %d, %d\r\n", accel[0], accel[1], accel[2]);
00168     DEBUG(string);
00169     sprintf(string, "Pitch: %ld\r\n", pitch);
00170     DEBUG(string);
00171     sprintf(string, "Roll: %ld\r\n", roll);
00172     DEBUG(string);
00173 }
00174 
00175 
00176 void detect(void)
00177 {
00178     LOG("Button pressed\n");
00179     DEBUG("Button pressed\n");
00180     datatoBLE(gyro, accel, quat);
00181     blue = !blue;
00182 }
00183 
00184 void motion_interrupt_handle(void)
00185 {
00186     motion_event = 1;
00187 }
00188 
00189 void tap_cb(unsigned char direction, unsigned char count)
00190 {
00191     LOG("Tap motion detected\n");
00192     DEBUG("Tap motion detected\n");
00193 }
00194 
00195 void android_orient_cb(unsigned char orientation)
00196 {
00197     LOG("Oriention changed\n");
00198     DEBUG("Oriention changed\n");
00199 }
00200 
00201 
00202  
00203 // Main function
00204 int main()
00205 {
00206     
00207     
00208     blue  = 1;
00209     green = 1;
00210     red   = 1;
00211 
00212     pc.baud(115200);
00213     LOG("---- Seeed Tiny BLE ----\n");
00214 
00215     Ticker ticker;
00216     ticker.attach(tick, 3);
00217 
00218     button.fall(detect);
00219 
00220     LOG("Initialising the nRF51822\n");
00221     ble.init();
00222     ble.onDisconnection(disconnectionCallback);
00223     ble.onConnection((Gap::ConnectionEventCallback_t)connectionCallback);
00224 
00225 
00226     /* setup advertising */
00227     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00228     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00229     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00230                                      (const uint8_t *)"CSSE4011", sizeof("CSSE4011"));
00231     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
00232                                      (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
00233     DFUService dfu(ble);
00234     UARTService uartService(ble);
00235     uartServicePtr = &uartService;
00236     //uartService.retargetStdout();
00237 
00238     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
00239     ble.startAdvertising();
00240 
00241     check_i2c_bus();
00242 
00243     mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
00244     motion_probe.fall(motion_interrupt_handle);
00245 
00246 
00247     if (mpu_init(0)) {
00248 //         LOG("failed to initialize mpu6050\r\n");
00249     }
00250 
00251     /* Get/set hardware configuration. Start gyro. */
00252     /* Wake up all sensors. */
00253     mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
00254     /* Push both gyro and accel data into the FIFO. */
00255     mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
00256     mpu_set_sample_rate(DEFAULT_MPU_HZ);
00257 
00258     /* Read back configuration in case it was set improperly. */
00259     unsigned char accel_fsr;
00260     unsigned short gyro_rate, gyro_fsr;
00261     mpu_get_sample_rate(&gyro_rate);
00262     mpu_get_gyro_fsr(&gyro_fsr);
00263     mpu_get_accel_fsr(&accel_fsr);
00264 
00265     dmp_load_motion_driver_firmware();
00266     dmp_set_orientation(
00267         inv_orientation_matrix_to_scalar(board_orientation));
00268     dmp_register_tap_cb(tap_cb);
00269     dmp_register_android_orient_cb(android_orient_cb);
00270 
00271     uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
00272                             DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
00273                             DMP_FEATURE_GYRO_CAL;
00274     dmp_enable_feature(dmp_features);
00275     dmp_set_fifo_rate(DEFAULT_MPU_HZ);
00276     mpu_set_dmp_state(1);
00277 
00278 //     dmp_set_interrupt_mode(DMP_INT_GESTURE);
00279     dmp_set_tap_thresh(TAP_XYZ, 50);
00280 
00281     while (true) {
00282         if (motion_event) {
00283             motion_event = 0;
00284 
00285             unsigned long sensor_timestamp;
00286 
00287             unsigned char more = 1;
00288 
00289             while (more) {
00290                 /* This function gets new data from the FIFO when the DMP is in
00291                  * use. The FIFO can contain any combination of gyro, accel,
00292                  * quaternion, and gesture data. The sensors parameter tells the
00293                  * caller which data fields were actually populated with new data.
00294                  * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
00295                  * the FIFO isn't being filled with accel data.
00296                  * The driver parses the gesture data to determine if a gesture
00297                  * event has occurred; on an event, the application will be notified
00298                  * via a callback (assuming that a callback function was properly
00299                  * registered). The more parameter is non-zero if there are
00300                  * leftover packets in the FIFO.
00301                  */
00302                 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
00303                               &more);
00304                 /* Gyro and accel data are written to the FIFO by the DMP in chip
00305                  * frame and hardware units. This behavior is convenient because it
00306                  * keeps the gyro and accel outputs of dmp_read_fifo and
00307                  * mpu_read_fifo consistent.
00308                  */
00309 
00310 
00311                 if (sensors & INV_XYZ_GYRO) {
00312                     // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
00313                 }
00314                 if (sensors & INV_XYZ_ACCEL) {
00315                     // LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
00316                 }
00317 
00318                 /* Unlike gyro and accel, quaternions are written to the FIFO in
00319                  * the body frame, q30. The orientation is set by the scalar passed
00320                  * to dmp_set_orientation during initialization.
00321                  */
00322                 if (sensors & INV_WXYZ_QUAT) {
00323                     //LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
00324                    // LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
00325                     //LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
00326                    
00327                 }
00328             }
00329         } else {
00330             ble.waitForEvent();
00331         }
00332     }
00333 }
00334 
00335 void check_i2c_bus(void)
00336 {
00337 
00338     DigitalInOut scl(MPU6050_SCL);
00339     DigitalInOut sda(MPU6050_SDA);
00340 
00341     scl.input();
00342     sda.input();
00343     int scl_level = scl;
00344     int sda_level = sda;
00345     if (scl_level == 0 || sda_level == 0) {
00346         printf("scl: %d, sda: %d, i2c bus is not released\r\n", scl_level, sda_level);
00347 
00348         scl.output();
00349         for (int i = 0; i < 8; i++) {
00350             scl = 0;
00351             wait_us(10);
00352             scl = 1;
00353             wait_us(10);
00354         }
00355     }
00356 
00357     scl.input();
00358 
00359     scl_level = scl;
00360     sda_level = sda;
00361     if (scl_level == 0 || sda_level == 0) {
00362         printf("scl: %d, sda: %d, i2c bus is still not released\r\n", scl_level, sda_level);
00363     }
00364 }
00365 
00366 /* These next two functions converts the orientation matrix (see
00367  * gyro_orientation) to a scalar representation for use by the DMP.
00368  * NOTE: These functions are borrowed from Invensense's MPL.
00369  */
00370 static inline unsigned short inv_row_2_scale(const signed char *row)
00371 {
00372     unsigned short b;
00373 
00374     if (row[0] > 0)
00375         b = 0;
00376     else if (row[0] < 0)
00377         b = 4;
00378     else if (row[1] > 0)
00379         b = 1;
00380     else if (row[1] < 0)
00381         b = 5;
00382     else if (row[2] > 0)
00383         b = 2;
00384     else if (row[2] < 0)
00385         b = 6;
00386     else
00387         b = 7;      // error
00388     return b;
00389 }
00390 
00391 unsigned short inv_orientation_matrix_to_scalar(
00392     const signed char *mtx)
00393 {
00394     unsigned short scalar;
00395 
00396     /*
00397        XYZ  010_001_000 Identity Matrix
00398        XZY  001_010_000
00399        YXZ  010_000_001
00400        YZX  000_010_001
00401        ZXY  001_000_010
00402        ZYX  000_001_010
00403      */
00404 
00405     scalar = inv_row_2_scale(mtx);
00406     scalar |= inv_row_2_scale(mtx + 3) << 3;
00407     scalar |= inv_row_2_scale(mtx + 6) << 6;
00408 
00409 
00410     return scalar;
00411 }
00412