Yihui Xiong / Seeed_nRF51822_MPU9250

Use MPU9250 with nRF51822

Dependencies:   eMPL_MPU

Fork of Seeed_Tiny_BLE_Flash by Darren Huang

main.cpp@5:9b240c1d5251, 2015-12-10 (annotated)

Committer:
yihui
Date:
Thu Dec 10 08:00:18 2015 +0000
Revision:
5:9b240c1d5251
Parent:
4:19a0764d6b81 
get 9dof data from mpu9250

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yihui 5:9b240c1d5251 1
yihui 0:26da608265f8 2
yihui 0:26da608265f8 3 #include "mbed.h"
yihui 5:9b240c1d5251 4 #include "mbed_spi.h"
yihui 0:26da608265f8 5 #include "inv_mpu.h"
yihui 0:26da608265f8 6 #include "inv_mpu_dmp_motion_driver.h"
yihui 0:26da608265f8 7
yihui 0:26da608265f8 8 #define LOG(...) { pc.printf(__VA_ARGS__); }
yihui 0:26da608265f8 9
yihui 5:9b240c1d5251 10 #define MPU9250_MISO p16
yihui 5:9b240c1d5251 11 #define MPU9250_MOSI p12
yihui 5:9b240c1d5251 12 #define MPU9250_SCLK p13
yihui 5:9b240c1d5251 13 #define MPU9250_CS p15
yihui 5:9b240c1d5251 14 #define MPU9250_INT p14
yihui 0:26da608265f8 15
yihui 0:26da608265f8 16 /* Starting sampling rate. */
yihui 5:9b240c1d5251 17 #define DEFAULT_MPU_HZ (100)
yihui 0:26da608265f8 18
yihui 0:26da608265f8 19
yihui 5:9b240c1d5251 20 Serial pc(p9, p11);
yihui 5:9b240c1d5251 21 Ticker ticker;
yihui 5:9b240c1d5251 22 InterruptIn motion_probe(MPU9250_INT);
yihui 2:b61ddbb8528e 23
yihui 5:9b240c1d5251 24 volatile uint8_t compass_event = 0;
yihui 0:26da608265f8 25 volatile uint8_t motion_event = 0;
yihui 0:26da608265f8 26 static signed char board_orientation[9] = {
yihui 0:26da608265f8 27 1, 0, 0,
yihui 0:26da608265f8 28 0, 1, 0,
yihui 0:26da608265f8 29 0, 0, 1
yihui 0:26da608265f8 30 };
yihui 0:26da608265f8 31
yihui 0:26da608265f8 32 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
yihui 0:26da608265f8 33
yihui 2:b61ddbb8528e 34
yihui 5:9b240c1d5251 35 void compass_tick_handle(void)
yihui 0:26da608265f8 36 {
yihui 5:9b240c1d5251 37 compass_event = 1;
yihui 0:26da608265f8 38 }
yihui 0:26da608265f8 39
yihui 0:26da608265f8 40
yihui 0:26da608265f8 41 void motion_interrupt_handle(void)
yihui 0:26da608265f8 42 {
yihui 0:26da608265f8 43 motion_event = 1;
yihui 0:26da608265f8 44 }
yihui 0:26da608265f8 45
yihui 0:26da608265f8 46 void tap_cb(unsigned char direction, unsigned char count)
yihui 0:26da608265f8 47 {
yihui 0:26da608265f8 48 LOG("Tap motion detected\n");
yihui 0:26da608265f8 49 }
yihui 0:26da608265f8 50
yihui 0:26da608265f8 51 void android_orient_cb(unsigned char orientation)
yihui 0:26da608265f8 52 {
yihui 0:26da608265f8 53 LOG("Oriention changed\n");
yihui 0:26da608265f8 54 }
yihui 0:26da608265f8 55
yihui 0:26da608265f8 56
SOTB_DA 4:19a0764d6b81 57
yihui 0:26da608265f8 58 int main(void)
yihui 0:26da608265f8 59 {
yihui 5:9b240c1d5251 60 pc.baud(115200);
yihui 5:9b240c1d5251 61 wait(1);
yihui 5:9b240c1d5251 62 LOG("---- eMPL MPU library @ Seeed ----\n");
yihui 0:26da608265f8 63
yihui 5:9b240c1d5251 64 mbed_spi_init(MPU9250_MOSI, MPU9250_MISO, MPU9250_SCLK, MPU9250_CS);
yihui 5:9b240c1d5251 65
yihui 0:26da608265f8 66
yihui 0:26da608265f8 67 if (mpu_init(0)) {
yihui 5:9b240c1d5251 68 LOG("failed to initialize mpu9250\r\n");
yihui 0:26da608265f8 69 }
yihui 5:9b240c1d5251 70
yihui 5:9b240c1d5251 71 /* Get/set hardware configuration. Start gyro. */
yihui 5:9b240c1d5251 72 /* Wake up all sensors. */
yihui 5:9b240c1d5251 73 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); //
yihui 5:9b240c1d5251 74 /* Push both gyro and accel data into the FIFO. */
yihui 0:26da608265f8 75 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
yihui 0:26da608265f8 76 mpu_set_sample_rate(DEFAULT_MPU_HZ);
yihui 5:9b240c1d5251 77
yihui 5:9b240c1d5251 78 /* Read back configuration in case it was set improperly. */
yihui 5:9b240c1d5251 79 unsigned char accel_fsr;
yihui 5:9b240c1d5251 80 unsigned short gyro_rate, gyro_fsr;
yihui 5:9b240c1d5251 81 mpu_get_sample_rate(&gyro_rate);
yihui 5:9b240c1d5251 82 mpu_get_gyro_fsr(&gyro_fsr);
yihui 5:9b240c1d5251 83 mpu_get_accel_fsr(&accel_fsr);
yihui 5:9b240c1d5251 84
yihui 0:26da608265f8 85 dmp_load_motion_driver_firmware();
yihui 0:26da608265f8 86 dmp_set_orientation(
yihui 0:26da608265f8 87 inv_orientation_matrix_to_scalar(board_orientation));
yihui 0:26da608265f8 88 dmp_register_tap_cb(tap_cb);
yihui 0:26da608265f8 89 dmp_register_android_orient_cb(android_orient_cb);
yihui 5:9b240c1d5251 90
yihui 0:26da608265f8 91 uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
yihui 5:9b240c1d5251 92 DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
yihui 5:9b240c1d5251 93 DMP_FEATURE_GYRO_CAL;
yihui 5:9b240c1d5251 94
yihui 0:26da608265f8 95 dmp_enable_feature(dmp_features);
yihui 0:26da608265f8 96 dmp_set_fifo_rate(DEFAULT_MPU_HZ);
yihui 0:26da608265f8 97 mpu_set_dmp_state(1);
yihui 5:9b240c1d5251 98
yihui 5:9b240c1d5251 99 dmp_set_interrupt_mode(DMP_INT_CONTINUOUS);
yihui 0:26da608265f8 100 dmp_set_tap_thresh(TAP_XYZ, 50);
yihui 5:9b240c1d5251 101
yihui 2:b61ddbb8528e 102 motion_probe.fall(motion_interrupt_handle);
yihui 0:26da608265f8 103
yihui 5:9b240c1d5251 104 ticker.attach(compass_tick_handle, 0.1);
yihui 2:b61ddbb8528e 105
yihui 5:9b240c1d5251 106 int try_to_sleep = 1;
yihui 0:26da608265f8 107 while (true) {
yihui 0:26da608265f8 108 if (motion_event) {
yihui 5:9b240c1d5251 109 try_to_sleep = 0;
yihui 5:9b240c1d5251 110
yihui 3:24e365bd1b97 111 unsigned long sensor_timestamp;
yihui 3:24e365bd1b97 112 short gyro[3], accel[3], sensors;
yihui 3:24e365bd1b97 113 long quat[4];
yihui 3:24e365bd1b97 114 unsigned char more = 1;
yihui 5:9b240c1d5251 115
yihui 3:24e365bd1b97 116 while (more) {
yihui 3:24e365bd1b97 117 /* This function gets new data from the FIFO when the DMP is in
yihui 3:24e365bd1b97 118 * use. The FIFO can contain any combination of gyro, accel,
yihui 3:24e365bd1b97 119 * quaternion, and gesture data. The sensors parameter tells the
yihui 3:24e365bd1b97 120 * caller which data fields were actually populated with new data.
yihui 3:24e365bd1b97 121 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
yihui 3:24e365bd1b97 122 * the FIFO isn't being filled with accel data.
yihui 3:24e365bd1b97 123 * The driver parses the gesture data to determine if a gesture
yihui 3:24e365bd1b97 124 * event has occurred; on an event, the application will be notified
yihui 3:24e365bd1b97 125 * via a callback (assuming that a callback function was properly
yihui 3:24e365bd1b97 126 * registered). The more parameter is non-zero if there are
yihui 3:24e365bd1b97 127 * leftover packets in the FIFO.
yihui 3:24e365bd1b97 128 */
yihui 3:24e365bd1b97 129 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
yihui 3:24e365bd1b97 130 &more);
yihui 3:24e365bd1b97 131 /* Gyro and accel data are written to the FIFO by the DMP in chip
yihui 3:24e365bd1b97 132 * frame and hardware units. This behavior is convenient because it
yihui 3:24e365bd1b97 133 * keeps the gyro and accel outputs of dmp_read_fifo and
yihui 3:24e365bd1b97 134 * mpu_read_fifo consistent.
yihui 3:24e365bd1b97 135 */
yihui 3:24e365bd1b97 136 if (sensors & INV_XYZ_GYRO) {
yihui 5:9b240c1d5251 137 LOG("gyro: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
yihui 3:24e365bd1b97 138 }
yihui 3:24e365bd1b97 139 if (sensors & INV_XYZ_ACCEL) {
yihui 5:9b240c1d5251 140 LOG("acc: %d, %d, %d\n", accel[0], accel[1], accel[2]);
yihui 3:24e365bd1b97 141 }
yihui 5:9b240c1d5251 142
yihui 3:24e365bd1b97 143 /* Unlike gyro and accel, quaternions are written to the FIFO in
yihui 3:24e365bd1b97 144 * the body frame, q30. The orientation is set by the scalar passed
yihui 3:24e365bd1b97 145 * to dmp_set_orientation during initialization.
yihui 3:24e365bd1b97 146 */
yihui 3:24e365bd1b97 147 if (sensors & INV_WXYZ_QUAT) {
yihui 5:9b240c1d5251 148 LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
yihui 3:24e365bd1b97 149 }
yihui 5:9b240c1d5251 150
yihui 3:24e365bd1b97 151 }
yihui 5:9b240c1d5251 152
yihui 5:9b240c1d5251 153 motion_event = 0;
yihui 5:9b240c1d5251 154 }
yihui 5:9b240c1d5251 155
yihui 5:9b240c1d5251 156 if (compass_event) {
yihui 5:9b240c1d5251 157 try_to_sleep = 0;
yihui 5:9b240c1d5251 158
yihui 5:9b240c1d5251 159 unsigned long compass_timestamp = 0;
yihui 5:9b240c1d5251 160 short compass[3];
yihui 5:9b240c1d5251 161
yihui 5:9b240c1d5251 162
yihui 5:9b240c1d5251 163 int retval = mpu_get_compass_reg(compass, &compass_timestamp);
yihui 5:9b240c1d5251 164 if (retval) {
yihui 5:9b240c1d5251 165 LOG("read compass error: %d\n", retval);
yihui 5:9b240c1d5251 166 } else {
yihui 5:9b240c1d5251 167 LOG("compass: %d, %d, %d\n", compass[0], compass[1], compass[2]);
SOTB_DA 4:19a0764d6b81 168 }
yihui 5:9b240c1d5251 169
yihui 5:9b240c1d5251 170 compass_event = 0;
yihui 5:9b240c1d5251 171 }
yihui 5:9b240c1d5251 172
yihui 5:9b240c1d5251 173 if (try_to_sleep) {
yihui 5:9b240c1d5251 174 sleep();
yihui 5:9b240c1d5251 175 try_to_sleep = 1;
yihui 0:26da608265f8 176 }
yihui 0:26da608265f8 177 }
yihui 0:26da608265f8 178 }
yihui 0:26da608265f8 179
yihui 0:26da608265f8 180 /* These next two functions converts the orientation matrix (see
yihui 0:26da608265f8 181 * gyro_orientation) to a scalar representation for use by the DMP.
yihui 0:26da608265f8 182 * NOTE: These functions are borrowed from Invensense's MPL.
yihui 0:26da608265f8 183 */
yihui 0:26da608265f8 184 static inline unsigned short inv_row_2_scale(const signed char *row)
yihui 0:26da608265f8 185 {
yihui 0:26da608265f8 186 unsigned short b;
yihui 0:26da608265f8 187
yihui 0:26da608265f8 188 if (row[0] > 0)
yihui 0:26da608265f8 189 b = 0;
yihui 0:26da608265f8 190 else if (row[0] < 0)
yihui 0:26da608265f8 191 b = 4;
yihui 0:26da608265f8 192 else if (row[1] > 0)
yihui 0:26da608265f8 193 b = 1;
yihui 0:26da608265f8 194 else if (row[1] < 0)
yihui 0:26da608265f8 195 b = 5;
yihui 0:26da608265f8 196 else if (row[2] > 0)
yihui 0:26da608265f8 197 b = 2;
yihui 0:26da608265f8 198 else if (row[2] < 0)
yihui 0:26da608265f8 199 b = 6;
yihui 0:26da608265f8 200 else
yihui 0:26da608265f8 201 b = 7; // error
yihui 0:26da608265f8 202 return b;
yihui 0:26da608265f8 203 }
yihui 0:26da608265f8 204
yihui 0:26da608265f8 205 unsigned short inv_orientation_matrix_to_scalar(
yihui 0:26da608265f8 206 const signed char *mtx)
yihui 0:26da608265f8 207 {
yihui 0:26da608265f8 208 unsigned short scalar;
yihui 0:26da608265f8 209
yihui 0:26da608265f8 210 /*
yihui 0:26da608265f8 211 XYZ 010_001_000 Identity Matrix
yihui 0:26da608265f8 212 XZY 001_010_000
yihui 0:26da608265f8 213 YXZ 010_000_001
yihui 0:26da608265f8 214 YZX 000_010_001
yihui 0:26da608265f8 215 ZXY 001_000_010
yihui 0:26da608265f8 216 ZYX 000_001_010
yihui 0:26da608265f8 217 */
yihui 0:26da608265f8 218
yihui 0:26da608265f8 219 scalar = inv_row_2_scale(mtx);
yihui 0:26da608265f8 220 scalar |= inv_row_2_scale(mtx + 3) << 3;
yihui 0:26da608265f8 221 scalar |= inv_row_2_scale(mtx + 6) << 6;
yihui 0:26da608265f8 222
yihui 0:26da608265f8 223
yihui 0:26da608265f8 224 return scalar;
yihui 0:26da608265f8 225 }
yihui 0:26da608265f8 226