Use MPU9250 with nRF51822
Fork of Seeed_Tiny_BLE_Flash by
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?
User | Revision | Line number | New 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 |