Justin B / Mbed 2 deprecated TinyBLE_motion_sensorBufferTemplate

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BLE_API MPU6050 mbed nRF51822

main.cpp@2:bf1a19d489eb, 2018-09-14 (annotated)

Committer:
pietermaljaars
Date:
Fri Sep 14 20:06:28 2018 +0000
Revision:
2:bf1a19d489eb
Parent:
1:b529e733ab71
Child:
3:7875f062a4ea
Add battery service and device information service

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pietermaljaars 0:7c12d0922052 1
pietermaljaars 0:7c12d0922052 2 #include "mbed.h"
pietermaljaars 0:7c12d0922052 3 #include "mbed_i2c.h"
pietermaljaars 0:7c12d0922052 4 #include "inv_mpu.h"
pietermaljaars 0:7c12d0922052 5 #include "inv_mpu_dmp_motion_driver.h"
pietermaljaars 0:7c12d0922052 6 #include "nrf51.h"
pietermaljaars 0:7c12d0922052 7 #include "nrf51_bitfields.h"
pietermaljaars 0:7c12d0922052 8
pietermaljaars 0:7c12d0922052 9 #include "BLE.h"
pietermaljaars 0:7c12d0922052 10 #include "DFUService.h"
pietermaljaars 0:7c12d0922052 11 #include "UARTService.h"
pietermaljaars 2:bf1a19d489eb 12 #include "BatteryService.h"
pietermaljaars 2:bf1a19d489eb 13 #include "DeviceInformationService.h"
pietermaljaars 0:7c12d0922052 14
pietermaljaars 0:7c12d0922052 15
pietermaljaars 0:7c12d0922052 16 #define LOG(...) { pc.printf(__VA_ARGS__); }
pietermaljaars 0:7c12d0922052 17
pietermaljaars 0:7c12d0922052 18 #define LED_GREEN p21
pietermaljaars 0:7c12d0922052 19 #define LED_RED p22
pietermaljaars 0:7c12d0922052 20 #define LED_BLUE p23
pietermaljaars 0:7c12d0922052 21 #define BUTTON_PIN p17
pietermaljaars 0:7c12d0922052 22 #define BATTERY_PIN p1
pietermaljaars 0:7c12d0922052 23
pietermaljaars 0:7c12d0922052 24 #define MPU6050_SDA p12
pietermaljaars 0:7c12d0922052 25 #define MPU6050_SCL p13
pietermaljaars 0:7c12d0922052 26
pietermaljaars 0:7c12d0922052 27 #define UART_TX p9
pietermaljaars 0:7c12d0922052 28 #define UART_RX p11
pietermaljaars 0:7c12d0922052 29 #define UART_CTS p8
pietermaljaars 0:7c12d0922052 30 #define UART_RTS p10
pietermaljaars 0:7c12d0922052 31
pietermaljaars 0:7c12d0922052 32 /* Starting sampling rate. */
pietermaljaars 0:7c12d0922052 33 #define DEFAULT_MPU_HZ (100)
pietermaljaars 0:7c12d0922052 34
pietermaljaars 2:bf1a19d489eb 35
pietermaljaars 2:bf1a19d489eb 36 #define MANUFACTURER "ALTEN"
pietermaljaars 2:bf1a19d489eb 37 #define MODELNUMBER "IoT BLE (PM)"
pietermaljaars 2:bf1a19d489eb 38 #define SERIALNUMBER "serialnumber"
pietermaljaars 2:bf1a19d489eb 39 #define HARDWAREREVISION "demo1"
pietermaljaars 2:bf1a19d489eb 40 #define FIRMWAREREVISION "1.0"
pietermaljaars 2:bf1a19d489eb 41 #define SOFTWAREREVISION "1.0"
pietermaljaars 2:bf1a19d489eb 42
pietermaljaars 0:7c12d0922052 43 DigitalOut blue(LED_BLUE);
pietermaljaars 0:7c12d0922052 44 DigitalOut green(LED_GREEN);
pietermaljaars 0:7c12d0922052 45 DigitalOut red(LED_RED);
pietermaljaars 0:7c12d0922052 46
pietermaljaars 0:7c12d0922052 47 InterruptIn button(BUTTON_PIN);
pietermaljaars 0:7c12d0922052 48 AnalogIn battery(BATTERY_PIN);
pietermaljaars 0:7c12d0922052 49 Serial pc(UART_TX, UART_RX);
pietermaljaars 0:7c12d0922052 50
pietermaljaars 0:7c12d0922052 51 InterruptIn motion_probe(p14);
pietermaljaars 0:7c12d0922052 52
pietermaljaars 0:7c12d0922052 53 int read_none_count = 0;
pietermaljaars 0:7c12d0922052 54
pietermaljaars 0:7c12d0922052 55 BLEDevice ble;
pietermaljaars 0:7c12d0922052 56 UARTService *uartServicePtr;
pietermaljaars 0:7c12d0922052 57
pietermaljaars 2:bf1a19d489eb 58 // variables to monitor the battery voltage
pietermaljaars 2:bf1a19d489eb 59 float batteryVoltage = 100.0f;
pietermaljaars 2:bf1a19d489eb 60 bool batteryVoltageChanged = false;
pietermaljaars 2:bf1a19d489eb 61
pietermaljaars 0:7c12d0922052 62 volatile bool bleIsConnected = false;
pietermaljaars 0:7c12d0922052 63 volatile uint8_t tick_event = 0;
pietermaljaars 0:7c12d0922052 64 volatile uint8_t motion_event = 0;
pietermaljaars 0:7c12d0922052 65 static signed char board_orientation[9] = {
pietermaljaars 0:7c12d0922052 66 1, 0, 0,
pietermaljaars 0:7c12d0922052 67 0, 1, 0,
pietermaljaars 0:7c12d0922052 68 0, 0, 1
pietermaljaars 0:7c12d0922052 69 };
pietermaljaars 0:7c12d0922052 70
pietermaljaars 0:7c12d0922052 71
pietermaljaars 0:7c12d0922052 72 void check_i2c_bus(void);
pietermaljaars 0:7c12d0922052 73 unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
pietermaljaars 0:7c12d0922052 74
pietermaljaars 0:7c12d0922052 75
pietermaljaars 0:7c12d0922052 76 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
pietermaljaars 0:7c12d0922052 77 {
pietermaljaars 0:7c12d0922052 78 LOG("Connected!\n");
pietermaljaars 0:7c12d0922052 79 bleIsConnected = true;
pietermaljaars 0:7c12d0922052 80 }
pietermaljaars 0:7c12d0922052 81
pietermaljaars 0:7c12d0922052 82 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams)
pietermaljaars 0:7c12d0922052 83 {
pietermaljaars 0:7c12d0922052 84 LOG("Disconnected!\n");
pietermaljaars 0:7c12d0922052 85 LOG("Restarting the advertising process\n");
pietermaljaars 0:7c12d0922052 86 ble.startAdvertising();
pietermaljaars 0:7c12d0922052 87 bleIsConnected = false;
pietermaljaars 0:7c12d0922052 88 }
pietermaljaars 0:7c12d0922052 89
pietermaljaars 0:7c12d0922052 90 void tick(void)
pietermaljaars 0:7c12d0922052 91 {
pietermaljaars 0:7c12d0922052 92 static uint32_t count = 0;
pietermaljaars 0:7c12d0922052 93
pietermaljaars 0:7c12d0922052 94 LOG("%d\r\n", count++);
pietermaljaars 0:7c12d0922052 95 green = !green;
pietermaljaars 0:7c12d0922052 96 }
pietermaljaars 0:7c12d0922052 97
pietermaljaars 2:bf1a19d489eb 98 // timer callback function to measure the ADC battery level
pietermaljaars 2:bf1a19d489eb 99 void batteryMonitorCallback(void)
pietermaljaars 2:bf1a19d489eb 100 {
pietermaljaars 2:bf1a19d489eb 101 float sample;
pietermaljaars 2:bf1a19d489eb 102
pietermaljaars 2:bf1a19d489eb 103 sample = battery.read();
pietermaljaars 2:bf1a19d489eb 104 /* cannot use (uart.)printf() in a ISR like this. */
pietermaljaars 2:bf1a19d489eb 105 batteryVoltage = sample;
pietermaljaars 2:bf1a19d489eb 106 batteryVoltageChanged = true;
pietermaljaars 2:bf1a19d489eb 107 }
pietermaljaars 2:bf1a19d489eb 108
pietermaljaars 0:7c12d0922052 109 void detect(void)
pietermaljaars 0:7c12d0922052 110 {
pietermaljaars 0:7c12d0922052 111 LOG("Button pressed\n");
pietermaljaars 0:7c12d0922052 112 blue = !blue;
pietermaljaars 0:7c12d0922052 113 }
pietermaljaars 0:7c12d0922052 114
pietermaljaars 0:7c12d0922052 115 void motion_interrupt_handle(void)
pietermaljaars 0:7c12d0922052 116 {
pietermaljaars 0:7c12d0922052 117 motion_event = 1;
pietermaljaars 0:7c12d0922052 118 }
pietermaljaars 0:7c12d0922052 119
pietermaljaars 0:7c12d0922052 120 void tap_cb(unsigned char direction, unsigned char count)
pietermaljaars 0:7c12d0922052 121 {
pietermaljaars 0:7c12d0922052 122 LOG("Tap motion detected\n");
pietermaljaars 0:7c12d0922052 123 }
pietermaljaars 0:7c12d0922052 124
pietermaljaars 0:7c12d0922052 125 void android_orient_cb(unsigned char orientation)
pietermaljaars 0:7c12d0922052 126 {
pietermaljaars 0:7c12d0922052 127 LOG("Oriention changed\n");
pietermaljaars 0:7c12d0922052 128 }
pietermaljaars 0:7c12d0922052 129
pietermaljaars 0:7c12d0922052 130
pietermaljaars 0:7c12d0922052 131 int main(void)
pietermaljaars 0:7c12d0922052 132 {
pietermaljaars 0:7c12d0922052 133 blue = 1;
pietermaljaars 0:7c12d0922052 134 green = 1;
pietermaljaars 0:7c12d0922052 135 red = 1;
pietermaljaars 0:7c12d0922052 136
pietermaljaars 0:7c12d0922052 137 pc.baud(115200);
pietermaljaars 0:7c12d0922052 138
pietermaljaars 0:7c12d0922052 139 wait(1);
pietermaljaars 0:7c12d0922052 140
pietermaljaars 0:7c12d0922052 141 LOG("---- Seeed Tiny BLE ----\n");
pietermaljaars 0:7c12d0922052 142
pietermaljaars 0:7c12d0922052 143 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
pietermaljaars 0:7c12d0922052 144 mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
pietermaljaars 0:7c12d0922052 145
pietermaljaars 0:7c12d0922052 146
pietermaljaars 0:7c12d0922052 147 if (mpu_init(0)) {
pietermaljaars 0:7c12d0922052 148 LOG("failed to initialize mpu6050\r\n");
pietermaljaars 0:7c12d0922052 149 }
pietermaljaars 0:7c12d0922052 150
pietermaljaars 0:7c12d0922052 151 /* Get/set hardware configuration. Start gyro. */
pietermaljaars 0:7c12d0922052 152 /* Wake up all sensors. */
pietermaljaars 0:7c12d0922052 153 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
pietermaljaars 0:7c12d0922052 154 /* Push both gyro and accel data into the FIFO. */
pietermaljaars 0:7c12d0922052 155 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
pietermaljaars 0:7c12d0922052 156 mpu_set_sample_rate(DEFAULT_MPU_HZ);
pietermaljaars 0:7c12d0922052 157
pietermaljaars 0:7c12d0922052 158 /* Read back configuration in case it was set improperly. */
pietermaljaars 0:7c12d0922052 159 unsigned char accel_fsr;
pietermaljaars 0:7c12d0922052 160 unsigned short gyro_rate, gyro_fsr;
pietermaljaars 0:7c12d0922052 161 mpu_get_sample_rate(&gyro_rate);
pietermaljaars 0:7c12d0922052 162 mpu_get_gyro_fsr(&gyro_fsr);
pietermaljaars 0:7c12d0922052 163 mpu_get_accel_fsr(&accel_fsr);
pietermaljaars 0:7c12d0922052 164
pietermaljaars 0:7c12d0922052 165 dmp_load_motion_driver_firmware();
pietermaljaars 0:7c12d0922052 166 dmp_set_orientation(
pietermaljaars 0:7c12d0922052 167 inv_orientation_matrix_to_scalar(board_orientation));
pietermaljaars 0:7c12d0922052 168 dmp_register_tap_cb(tap_cb);
pietermaljaars 0:7c12d0922052 169 dmp_register_android_orient_cb(android_orient_cb);
pietermaljaars 0:7c12d0922052 170
pietermaljaars 0:7c12d0922052 171 uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
pietermaljaars 0:7c12d0922052 172 DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
pietermaljaars 0:7c12d0922052 173 DMP_FEATURE_GYRO_CAL;
pietermaljaars 0:7c12d0922052 174 dmp_enable_feature(dmp_features);
pietermaljaars 0:7c12d0922052 175 dmp_set_fifo_rate(DEFAULT_MPU_HZ);
pietermaljaars 0:7c12d0922052 176 mpu_set_dmp_state(1);
pietermaljaars 0:7c12d0922052 177
pietermaljaars 0:7c12d0922052 178 dmp_set_interrupt_mode(DMP_INT_GESTURE);
pietermaljaars 0:7c12d0922052 179 dmp_set_tap_thresh(TAP_XYZ, 50);
pietermaljaars 0:7c12d0922052 180
pietermaljaars 0:7c12d0922052 181
pietermaljaars 0:7c12d0922052 182 motion_probe.fall(motion_interrupt_handle);
pietermaljaars 0:7c12d0922052 183
pietermaljaars 0:7c12d0922052 184
pietermaljaars 0:7c12d0922052 185
pietermaljaars 0:7c12d0922052 186 Ticker ticker;
pietermaljaars 0:7c12d0922052 187 ticker.attach(tick, 3);
pietermaljaars 2:bf1a19d489eb 188
pietermaljaars 2:bf1a19d489eb 189 Ticker batteryMonitorTicker;
pietermaljaars 2:bf1a19d489eb 190 batteryMonitorTicker.attach(batteryMonitorCallback, 60.0f);
pietermaljaars 0:7c12d0922052 191
pietermaljaars 0:7c12d0922052 192 button.fall(detect);
pietermaljaars 0:7c12d0922052 193
pietermaljaars 0:7c12d0922052 194 LOG("Initialising the nRF51822\n");
pietermaljaars 0:7c12d0922052 195 ble.init();
pietermaljaars 0:7c12d0922052 196 ble.gap().onDisconnection(disconnectionCallback);
pietermaljaars 0:7c12d0922052 197 ble.gap().onConnection(connectionCallback);
pietermaljaars 1:b529e733ab71 198
pietermaljaars 1:b529e733ab71 199 uint8_t name[] = "iot aabbccddeeff";
pietermaljaars 1:b529e733ab71 200
pietermaljaars 1:b529e733ab71 201 Gap::AddressType_t addr_type;
pietermaljaars 1:b529e733ab71 202 Gap::Address_t address;
pietermaljaars 1:b529e733ab71 203 ble_error_t error = ble.gap().getAddress(&addr_type, address);
pietermaljaars 1:b529e733ab71 204 if (error == BLE_ERROR_NONE) {
pietermaljaars 1:b529e733ab71 205 for (int i = 5; i >= 0; i--){
pietermaljaars 1:b529e733ab71 206 //sprintf(name[4 + ((5-i)*2)], "%02x", (uint8_t)address[i]);
pietermaljaars 1:b529e733ab71 207 //sprintf(buffer, "%02x", address[i]);
pietermaljaars 1:b529e733ab71 208 char buffer[3];
pietermaljaars 1:b529e733ab71 209 sprintf(buffer, "%02x", address[i]);
pietermaljaars 1:b529e733ab71 210 name[4 + ((5-i)*2)] = buffer[0];
pietermaljaars 1:b529e733ab71 211 name[4 + ((5-i)*2) + 1] = buffer[1];
pietermaljaars 1:b529e733ab71 212 }
pietermaljaars 1:b529e733ab71 213 }
pietermaljaars 1:b529e733ab71 214 LOG("name = %s\n", name);
pietermaljaars 1:b529e733ab71 215
pietermaljaars 0:7c12d0922052 216 /* setup advertising */
pietermaljaars 0:7c12d0922052 217 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
pietermaljaars 0:7c12d0922052 218 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
pietermaljaars 0:7c12d0922052 219 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
pietermaljaars 1:b529e733ab71 220 name, sizeof(name));
pietermaljaars 0:7c12d0922052 221 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
pietermaljaars 0:7c12d0922052 222 (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
pietermaljaars 0:7c12d0922052 223 DFUService dfu(ble);
pietermaljaars 0:7c12d0922052 224 UARTService uartService(ble);
pietermaljaars 0:7c12d0922052 225 uartServicePtr = &uartService;
pietermaljaars 0:7c12d0922052 226 //uartService.retargetStdout();
pietermaljaars 2:bf1a19d489eb 227 BatteryService battery(ble);
pietermaljaars 2:bf1a19d489eb 228 DeviceInformationService deviceInfo(ble, MANUFACTURER, MODELNUMBER, SERIALNUMBER, HARDWAREREVISION, FIRMWAREREVISION, SOFTWAREREVISION);
pietermaljaars 0:7c12d0922052 229
pietermaljaars 0:7c12d0922052 230 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
pietermaljaars 0:7c12d0922052 231 ble.gap().startAdvertising();
pietermaljaars 0:7c12d0922052 232
pietermaljaars 0:7c12d0922052 233 while (true) {
pietermaljaars 0:7c12d0922052 234 if (motion_event) {
pietermaljaars 0:7c12d0922052 235
pietermaljaars 0:7c12d0922052 236 unsigned long sensor_timestamp;
pietermaljaars 0:7c12d0922052 237 short gyro[3], accel[3], sensors;
pietermaljaars 0:7c12d0922052 238 long quat[4];
pietermaljaars 0:7c12d0922052 239 unsigned char more = 1;
pietermaljaars 0:7c12d0922052 240
pietermaljaars 0:7c12d0922052 241 while (more) {
pietermaljaars 0:7c12d0922052 242 /* This function gets new data from the FIFO when the DMP is in
pietermaljaars 0:7c12d0922052 243 * use. The FIFO can contain any combination of gyro, accel,
pietermaljaars 0:7c12d0922052 244 * quaternion, and gesture data. The sensors parameter tells the
pietermaljaars 0:7c12d0922052 245 * caller which data fields were actually populated with new data.
pietermaljaars 0:7c12d0922052 246 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
pietermaljaars 0:7c12d0922052 247 * the FIFO isn't being filled with accel data.
pietermaljaars 0:7c12d0922052 248 * The driver parses the gesture data to determine if a gesture
pietermaljaars 0:7c12d0922052 249 * event has occurred; on an event, the application will be notified
pietermaljaars 0:7c12d0922052 250 * via a callback (assuming that a callback function was properly
pietermaljaars 0:7c12d0922052 251 * registered). The more parameter is non-zero if there are
pietermaljaars 0:7c12d0922052 252 * leftover packets in the FIFO.
pietermaljaars 0:7c12d0922052 253 */
pietermaljaars 0:7c12d0922052 254 dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
pietermaljaars 0:7c12d0922052 255 &more);
pietermaljaars 0:7c12d0922052 256
pietermaljaars 0:7c12d0922052 257
pietermaljaars 0:7c12d0922052 258 /* Gyro and accel data are written to the FIFO by the DMP in chip
pietermaljaars 0:7c12d0922052 259 * frame and hardware units. This behavior is convenient because it
pietermaljaars 0:7c12d0922052 260 * keeps the gyro and accel outputs of dmp_read_fifo and
pietermaljaars 0:7c12d0922052 261 * mpu_read_fifo consistent.
pietermaljaars 0:7c12d0922052 262 */
pietermaljaars 0:7c12d0922052 263 if (sensors & INV_XYZ_GYRO) {
pietermaljaars 0:7c12d0922052 264 // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
pietermaljaars 0:7c12d0922052 265 }
pietermaljaars 0:7c12d0922052 266 if (sensors & INV_XYZ_ACCEL) {
pietermaljaars 0:7c12d0922052 267 //LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
pietermaljaars 0:7c12d0922052 268 }
pietermaljaars 0:7c12d0922052 269
pietermaljaars 0:7c12d0922052 270 /* Unlike gyro and accel, quaternions are written to the FIFO in
pietermaljaars 0:7c12d0922052 271 * the body frame, q30. The orientation is set by the scalar passed
pietermaljaars 0:7c12d0922052 272 * to dmp_set_orientation during initialization.
pietermaljaars 0:7c12d0922052 273 */
pietermaljaars 0:7c12d0922052 274 if (sensors & INV_WXYZ_QUAT) {
pietermaljaars 0:7c12d0922052 275 // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
pietermaljaars 0:7c12d0922052 276 }
pietermaljaars 0:7c12d0922052 277
pietermaljaars 0:7c12d0922052 278 if (sensors) {
pietermaljaars 0:7c12d0922052 279 read_none_count = 0;
pietermaljaars 0:7c12d0922052 280 } else {
pietermaljaars 0:7c12d0922052 281 read_none_count++;
pietermaljaars 0:7c12d0922052 282 if (read_none_count > 3) {
pietermaljaars 0:7c12d0922052 283 read_none_count = 0;
pietermaljaars 0:7c12d0922052 284
pietermaljaars 0:7c12d0922052 285 LOG("I2C may be stuck @ %d\r\n", sensor_timestamp);
pietermaljaars 0:7c12d0922052 286 mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
pietermaljaars 0:7c12d0922052 287 }
pietermaljaars 0:7c12d0922052 288 }
pietermaljaars 0:7c12d0922052 289 }
pietermaljaars 0:7c12d0922052 290
pietermaljaars 0:7c12d0922052 291 motion_event = 0;
pietermaljaars 0:7c12d0922052 292 } else {
pietermaljaars 0:7c12d0922052 293 ble.waitForEvent();
pietermaljaars 2:bf1a19d489eb 294
pietermaljaars 2:bf1a19d489eb 295 // update battery level after the level is measured
pietermaljaars 2:bf1a19d489eb 296 if (batteryVoltageChanged == true) {
pietermaljaars 2:bf1a19d489eb 297 LOG("VBat: %4.3f, ADC: %4.3f, Vadc: %4.3f\n", batteryVoltage*2.0f, batteryVoltage, batteryVoltage*3.3f);
pietermaljaars 2:bf1a19d489eb 298 battery.updateBatteryLevel((uint8_t)(batteryVoltage*100.0f)); // input is 0-1.0 of 3.3V -> *100 = percentage of 3.3V
pietermaljaars 2:bf1a19d489eb 299 batteryVoltageChanged = false;
pietermaljaars 2:bf1a19d489eb 300 }
pietermaljaars 0:7c12d0922052 301 }
pietermaljaars 0:7c12d0922052 302 }
pietermaljaars 0:7c12d0922052 303 }
pietermaljaars 0:7c12d0922052 304
pietermaljaars 0:7c12d0922052 305 /* These next two functions converts the orientation matrix (see
pietermaljaars 0:7c12d0922052 306 * gyro_orientation) to a scalar representation for use by the DMP.
pietermaljaars 0:7c12d0922052 307 * NOTE: These functions are borrowed from Invensense's MPL.
pietermaljaars 0:7c12d0922052 308 */
pietermaljaars 0:7c12d0922052 309 static inline unsigned short inv_row_2_scale(const signed char *row)
pietermaljaars 0:7c12d0922052 310 {
pietermaljaars 0:7c12d0922052 311 unsigned short b;
pietermaljaars 0:7c12d0922052 312
pietermaljaars 0:7c12d0922052 313 if (row[0] > 0)
pietermaljaars 0:7c12d0922052 314 b = 0;
pietermaljaars 0:7c12d0922052 315 else if (row[0] < 0)
pietermaljaars 0:7c12d0922052 316 b = 4;
pietermaljaars 0:7c12d0922052 317 else if (row[1] > 0)
pietermaljaars 0:7c12d0922052 318 b = 1;
pietermaljaars 0:7c12d0922052 319 else if (row[1] < 0)
pietermaljaars 0:7c12d0922052 320 b = 5;
pietermaljaars 0:7c12d0922052 321 else if (row[2] > 0)
pietermaljaars 0:7c12d0922052 322 b = 2;
pietermaljaars 0:7c12d0922052 323 else if (row[2] < 0)
pietermaljaars 0:7c12d0922052 324 b = 6;
pietermaljaars 0:7c12d0922052 325 else
pietermaljaars 0:7c12d0922052 326 b = 7; // error
pietermaljaars 0:7c12d0922052 327 return b;
pietermaljaars 0:7c12d0922052 328 }
pietermaljaars 0:7c12d0922052 329
pietermaljaars 0:7c12d0922052 330 unsigned short inv_orientation_matrix_to_scalar(
pietermaljaars 0:7c12d0922052 331 const signed char *mtx)
pietermaljaars 0:7c12d0922052 332 {
pietermaljaars 0:7c12d0922052 333 unsigned short scalar;
pietermaljaars 0:7c12d0922052 334
pietermaljaars 0:7c12d0922052 335 /*
pietermaljaars 0:7c12d0922052 336 XYZ 010_001_000 Identity Matrix
pietermaljaars 0:7c12d0922052 337 XZY 001_010_000
pietermaljaars 0:7c12d0922052 338 YXZ 010_000_001
pietermaljaars 0:7c12d0922052 339 YZX 000_010_001
pietermaljaars 0:7c12d0922052 340 ZXY 001_000_010
pietermaljaars 0:7c12d0922052 341 ZYX 000_001_010
pietermaljaars 0:7c12d0922052 342 */
pietermaljaars 0:7c12d0922052 343
pietermaljaars 0:7c12d0922052 344 scalar = inv_row_2_scale(mtx);
pietermaljaars 0:7c12d0922052 345 scalar |= inv_row_2_scale(mtx + 3) << 3;
pietermaljaars 0:7c12d0922052 346 scalar |= inv_row_2_scale(mtx + 6) << 6;
pietermaljaars 0:7c12d0922052 347
pietermaljaars 0:7c12d0922052 348
pietermaljaars 0:7c12d0922052 349 return scalar;
pietermaljaars 0:7c12d0922052 350 }
pietermaljaars 0:7c12d0922052 351