Yihui Xiong / Mbed 2 deprecated Seeed_nRF51822_Motion

initial

Dependencies:   mbed

main.cpp@0:638edba3adf6, 2016-01-11 (annotated)

Committer:
yihui
Date:
Mon Jan 11 02:32:24 2016 +0000
Revision:
0:638edba3adf6
initial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yihui 0:638edba3adf6 1
yihui 0:638edba3adf6 2 #include "mbed.h"
yihui 0:638edba3adf6 3 #include "mbed_spi.h"
yihui 0:638edba3adf6 4 #include "inv_mpu.h"
yihui 0:638edba3adf6 5 #include "inv_mpu_dmp_motion_driver.h"
yihui 0:638edba3adf6 6 #include "W25Q16BV.h"
yihui 0:638edba3adf6 7
yihui 0:638edba3adf6 8 #ifdef DEBUG
yihui 0:638edba3adf6 9 #define LOG(...) { printf(__VA_ARGS__); }
yihui 0:638edba3adf6 10 #else
yihui 0:638edba3adf6 11 #define LOG(...)
yihui 0:638edba3adf6 12 #endif
yihui 0:638edba3adf6 13
yihui 0:638edba3adf6 14 #define PIN_FLS_MOSI p19
yihui 0:638edba3adf6 15 #define PIN_FLS_MISO p18
yihui 0:638edba3adf6 16 #define PIN_FLS_SCLK p20
yihui 0:638edba3adf6 17 #define PIN_FLS_CS p8 // v1.1.0
yihui 0:638edba3adf6 18 //#define PIN_FLS_CS p21 // v1.0.0
yihui 0:638edba3adf6 19
yihui 0:638edba3adf6 20 #define MPU9250_MISO p16
yihui 0:638edba3adf6 21 #define MPU9250_MOSI p12
yihui 0:638edba3adf6 22 #define MPU9250_SCLK p13
yihui 0:638edba3adf6 23 #define MPU9250_CS p15
yihui 0:638edba3adf6 24 #define MPU9250_INT p10
yihui 0:638edba3adf6 25
yihui 0:638edba3adf6 26 /* Starting sampling rate. */
yihui 0:638edba3adf6 27 #define DEFAULT_MPU_HZ (100)
yihui 0:638edba3adf6 28
yihui 0:638edba3adf6 29 volatile uint8_t compass_event = 0;
yihui 0:638edba3adf6 30 volatile uint8_t motion_event = 0;
yihui 0:638edba3adf6 31
yihui 0:638edba3adf6 32 void compass_tick_handle(void)
yihui 0:638edba3adf6 33 {
yihui 0:638edba3adf6 34 compass_event = 1;
yihui 0:638edba3adf6 35 }
yihui 0:638edba3adf6 36
yihui 0:638edba3adf6 37
yihui 0:638edba3adf6 38 void motion_interrupt_handle(void)
yihui 0:638edba3adf6 39 {
yihui 0:638edba3adf6 40 motion_event = 1;
yihui 0:638edba3adf6 41 }
yihui 0:638edba3adf6 42
yihui 0:638edba3adf6 43 void tap_cb(unsigned char direction, unsigned char count)
yihui 0:638edba3adf6 44 {
yihui 0:638edba3adf6 45 LOG("Tap motion detected\n");
yihui 0:638edba3adf6 46 }
yihui 0:638edba3adf6 47
yihui 0:638edba3adf6 48 void android_orient_cb(unsigned char orientation)
yihui 0:638edba3adf6 49 {
yihui 0:638edba3adf6 50 LOG("Oriention changed\n");
yihui 0:638edba3adf6 51 }
yihui 0:638edba3adf6 52
yihui 0:638edba3adf6 53 void disable_uart(void)
yihui 0:638edba3adf6 54 {
yihui 0:638edba3adf6 55 // diable uart
yihui 0:638edba3adf6 56 ((NRF_UART_Type *)UART_0)->ENABLE = 0;
yihui 0:638edba3adf6 57
yihui 0:638edba3adf6 58 // change uart pins' settings
yihui 0:638edba3adf6 59 NRF_GPIO->PIN_CNF[USBTX] = 0x02;
yihui 0:638edba3adf6 60 NRF_GPIO->PIN_CNF[USBRX] = 0x02;
yihui 0:638edba3adf6 61 }
yihui 0:638edba3adf6 62
yihui 0:638edba3adf6 63 int main(void)
yihui 0:638edba3adf6 64 {
yihui 0:638edba3adf6 65 #ifndef DEBUG
yihui 0:638edba3adf6 66 disable_uart();
yihui 0:638edba3adf6 67 #else
yihui 0:638edba3adf6 68 Serial pc(USBTX, USBRX);
yihui 0:638edba3adf6 69 pc.baud(115200);
yihui 0:638edba3adf6 70 wait(1);
yihui 0:638edba3adf6 71 LOG("---- eMPL MPU library @ Seeed ----\n");
yihui 0:638edba3adf6 72 #endif
yihui 0:638edba3adf6 73
yihui 0:638edba3adf6 74 W25Q16BV flash(PIN_FLS_MOSI, PIN_FLS_MISO, PIN_FLS_SCLK, PIN_FLS_CS);
yihui 0:638edba3adf6 75 flash.enterDeepPowerDown();
yihui 0:638edba3adf6 76
yihui 0:638edba3adf6 77 mbed_spi_init(MPU9250_MOSI, MPU9250_MISO, MPU9250_SCLK, MPU9250_CS);
yihui 0:638edba3adf6 78
yihui 0:638edba3adf6 79
yihui 0:638edba3adf6 80 if (mpu_init(0)) {
yihui 0:638edba3adf6 81 LOG("failed to initialize mpu9250\r\n");
yihui 0:638edba3adf6 82 }
yihui 0:638edba3adf6 83
yihui 0:638edba3adf6 84 /* Get/set hardware configuration. Start gyro. */
yihui 0:638edba3adf6 85 /* Wake up all sensors. */
yihui 0:638edba3adf6 86 mpu_set_sensors(INV_XYZ_ACCEL); // | INV_XYZ_GYRO | INV_XYZ_COMPASS);
yihui 0:638edba3adf6 87 /* Push both gyro and accel data into the FIFO. */
yihui 0:638edba3adf6 88 // mpu_configure_fifo(INV_XYZ_ACCEL | INV_XYZ_GYRO);
yihui 0:638edba3adf6 89 // mpu_set_sample_rate(DEFAULT_MPU_HZ);
yihui 0:638edba3adf6 90
yihui 0:638edba3adf6 91 // dmp_load_motion_driver_firmware();
yihui 0:638edba3adf6 92 // dmp_register_tap_cb(tap_cb);
yihui 0:638edba3adf6 93 // dmp_register_android_orient_cb(android_orient_cb);
yihui 0:638edba3adf6 94
yihui 0:638edba3adf6 95 // uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
yihui 0:638edba3adf6 96 // DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
yihui 0:638edba3adf6 97 // DMP_FEATURE_GYRO_CAL;
yihui 0:638edba3adf6 98
yihui 0:638edba3adf6 99 // dmp_enable_feature(dmp_features);
yihui 0:638edba3adf6 100 // dmp_set_fifo_rate(DEFAULT_MPU_HZ);
yihui 0:638edba3adf6 101 // mpu_set_dmp_state(0);
yihui 0:638edba3adf6 102
yihui 0:638edba3adf6 103 // dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);
yihui 0:638edba3adf6 104 // dmp_set_tap_thresh(TAP_XYZ, 50);
yihui 0:638edba3adf6 105
yihui 0:638edba3adf6 106 // mpu_lp_accel_mode(1);
yihui 0:638edba3adf6 107 mpu_lp_motion_interrupt(128, 1, 1);
yihui 0:638edba3adf6 108
yihui 0:638edba3adf6 109 InterruptIn motion_probe(MPU9250_INT);
yihui 0:638edba3adf6 110 motion_probe.mode(PullNone);
yihui 0:638edba3adf6 111 motion_probe.fall(motion_interrupt_handle);
yihui 0:638edba3adf6 112
yihui 0:638edba3adf6 113 // Ticker ticker;
yihui 0:638edba3adf6 114 // ticker.attach(compass_tick_handle, 0.1);
yihui 0:638edba3adf6 115
yihui 0:638edba3adf6 116 mbed_spi_disable();
yihui 0:638edba3adf6 117
yihui 0:638edba3adf6 118 int try_to_sleep = 1;
yihui 0:638edba3adf6 119 while (true) {
yihui 0:638edba3adf6 120 #if 0
yihui 0:638edba3adf6 121 if (motion_event) {
yihui 0:638edba3adf6 122 try_to_sleep = 0;
yihui 0:638edba3adf6 123
yihui 0:638edba3adf6 124 unsigned long sensor_timestamp;
yihui 0:638edba3adf6 125 short gyro[3], accel[3], sensors;
yihui 0:638edba3adf6 126 long quat[4];
yihui 0:638edba3adf6 127 unsigned char more = 1;
yihui 0:638edba3adf6 128
yihui 0:638edba3adf6 129 mbed_spi_enable();
yihui 0:638edba3adf6 130 while (more) {
yihui 0:638edba3adf6 131 /* This function gets new data from the FIFO when the DMP is in
yihui 0:638edba3adf6 132 * use. The FIFO can contain any combination of gyro, accel,
yihui 0:638edba3adf6 133 * quaternion, and gesture data. The sensors parameter tells the
yihui 0:638edba3adf6 134 * caller which data fields were actually populated with new data.
yihui 0:638edba3adf6 135 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
yihui 0:638edba3adf6 136 * the FIFO isn't being filled with accel data.
yihui 0:638edba3adf6 137 * The driver parses the gesture data to determine if a gesture
yihui 0:638edba3adf6 138 * event has occurred; on an event, the application will be notified
yihui 0:638edba3adf6 139 * via a callback (assuming that a callback function was properly
yihui 0:638edba3adf6 140 * registered). The more parameter is non-zero if there are
yihui 0:638edba3adf6 141 * leftover packets in the FIFO.
yihui 0:638edba3adf6 142 */
yihui 0:638edba3adf6 143 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
yihui 0:638edba3adf6 144 &more);
yihui 0:638edba3adf6 145 /* Gyro and accel data are written to the FIFO by the DMP in chip
yihui 0:638edba3adf6 146 * frame and hardware units. This behavior is convenient because it
yihui 0:638edba3adf6 147 * keeps the gyro and accel outputs of dmp_read_fifo and
yihui 0:638edba3adf6 148 * mpu_read_fifo consistent.
yihui 0:638edba3adf6 149 */
yihui 0:638edba3adf6 150 if (sensors & INV_XYZ_GYRO) {
yihui 0:638edba3adf6 151 LOG("gyro: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
yihui 0:638edba3adf6 152 }
yihui 0:638edba3adf6 153 if (sensors & INV_XYZ_ACCEL) {
yihui 0:638edba3adf6 154 LOG("acc: %d, %d, %d\n", accel[0], accel[1], accel[2]);
yihui 0:638edba3adf6 155 }
yihui 0:638edba3adf6 156
yihui 0:638edba3adf6 157 /* Unlike gyro and accel, quaternions are written to the FIFO in
yihui 0:638edba3adf6 158 * the body frame, q30. The orientation is set by the scalar passed
yihui 0:638edba3adf6 159 * to dmp_set_orientation during initialization.
yihui 0:638edba3adf6 160 */
yihui 0:638edba3adf6 161 if (sensors & INV_WXYZ_QUAT) {
yihui 0:638edba3adf6 162 LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
yihui 0:638edba3adf6 163 }
yihui 0:638edba3adf6 164
yihui 0:638edba3adf6 165 }
yihui 0:638edba3adf6 166
yihui 0:638edba3adf6 167 mbed_spi_disable();
yihui 0:638edba3adf6 168
yihui 0:638edba3adf6 169 motion_event = 0;
yihui 0:638edba3adf6 170 }
yihui 0:638edba3adf6 171 #else
yihui 0:638edba3adf6 172 if (motion_event) {
yihui 0:638edba3adf6 173 try_to_sleep = 0;
yihui 0:638edba3adf6 174 motion_event = 0;
yihui 0:638edba3adf6 175
yihui 0:638edba3adf6 176 unsigned long sensor_timestamp;
yihui 0:638edba3adf6 177 short accel[3];
yihui 0:638edba3adf6 178
yihui 0:638edba3adf6 179 mbed_spi_enable();
yihui 0:638edba3adf6 180 mpu_get_accel_reg(accel, &sensor_timestamp);
yihui 0:638edba3adf6 181 mbed_spi_disable();
yihui 0:638edba3adf6 182
yihui 0:638edba3adf6 183 LOG("acc: %d, %d, %d @ %ld\n", accel[0], accel[1], accel[2], sensor_timestamp);
yihui 0:638edba3adf6 184
yihui 0:638edba3adf6 185 }
yihui 0:638edba3adf6 186 #endif
yihui 0:638edba3adf6 187
yihui 0:638edba3adf6 188 if (compass_event) {
yihui 0:638edba3adf6 189 try_to_sleep = 0;
yihui 0:638edba3adf6 190
yihui 0:638edba3adf6 191 unsigned long compass_timestamp = 0;
yihui 0:638edba3adf6 192 short compass[3];
yihui 0:638edba3adf6 193
yihui 0:638edba3adf6 194 mbed_spi_enable();
yihui 0:638edba3adf6 195 int retval = mpu_get_compass_reg(compass, &compass_timestamp);
yihui 0:638edba3adf6 196 mbed_spi_disable();
yihui 0:638edba3adf6 197 if (retval) {
yihui 0:638edba3adf6 198 LOG("read compass error: %d\n", retval);
yihui 0:638edba3adf6 199 } else {
yihui 0:638edba3adf6 200 LOG("compass: %d, %d, %d\n", compass[0], compass[1], compass[2]);
yihui 0:638edba3adf6 201 }
yihui 0:638edba3adf6 202
yihui 0:638edba3adf6 203 compass_event = 0;
yihui 0:638edba3adf6 204 }
yihui 0:638edba3adf6 205
yihui 0:638edba3adf6 206 if (try_to_sleep) {
yihui 0:638edba3adf6 207 sleep();
yihui 0:638edba3adf6 208 }
yihui 0:638edba3adf6 209
yihui 0:638edba3adf6 210 try_to_sleep = 1;
yihui 0:638edba3adf6 211 }
yihui 0:638edba3adf6 212 }
yihui 0:638edba3adf6 213