Alexey Krasnoperov
Test of 100 Hz
Dependencies: mbed BLE_API nRF51822 eMPL_MPU6050
Diff: main.cpp
- Revision:
- 2:b61ddbb8528e
- Parent:
- 0:26da608265f8
- Child:
- 3:24e365bd1b97
--- a/main.cpp Wed Apr 22 07:47:17 2015 +0000
+++ b/main.cpp Thu Nov 05 02:46:37 2015 +0000
@@ -3,6 +3,8 @@
#include "mbed_i2c.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
+#include "nrf51.h"
+#include "nrf51_bitfields.h"
#include "BLEDevice.h"
#include "DFUService.h"
@@ -26,7 +28,7 @@
#define UART_RTS p10
/* Starting sampling rate. */
-#define DEFAULT_MPU_HZ (100)
+#define DEFAULT_MPU_HZ (200)
DigitalOut blue(LED_BLUE);
DigitalOut green(LED_GREEN);
@@ -38,6 +40,8 @@
InterruptIn motion_probe(p14);
+int read_none_count = 0;
+
BLEDevice ble;
UARTService *uartServicePtr;
@@ -54,6 +58,7 @@
void check_i2c_bus(void);
unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx);
+
void connectionCallback(Gap::Handle_t handle, Gap::addr_type_t peerAddrType, const Gap::address_t peerAddr, const Gap::ConnectionParams_t *params)
{
LOG("Connected!\n");
@@ -75,14 +80,68 @@
void detect(void)
{
- LOG("Button pressed\n");
-
+ LOG("Button pressed\n");
blue = !blue;
}
void motion_interrupt_handle(void)
{
motion_event = 1;
+
+ {
+ unsigned long sensor_timestamp;
+ short gyro[3], accel[3], sensors;
+ long quat[4];
+ unsigned char more = 1;
+
+ while (more) {
+ /* This function gets new data from the FIFO when the DMP is in
+ * use. The FIFO can contain any combination of gyro, accel,
+ * quaternion, and gesture data. The sensors parameter tells the
+ * caller which data fields were actually populated with new data.
+ * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
+ * the FIFO isn't being filled with accel data.
+ * The driver parses the gesture data to determine if a gesture
+ * event has occurred; on an event, the application will be notified
+ * via a callback (assuming that a callback function was properly
+ * registered). The more parameter is non-zero if there are
+ * leftover packets in the FIFO.
+ */
+ dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
+ &more);
+ /* Gyro and accel data are written to the FIFO by the DMP in chip
+ * frame and hardware units. This behavior is convenient because it
+ * keeps the gyro and accel outputs of dmp_read_fifo and
+ * mpu_read_fifo consistent.
+ */
+ if (sensors & INV_XYZ_GYRO) {
+ // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
+ }
+ if (sensors & INV_XYZ_ACCEL) {
+ LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
+ }
+
+ /* Unlike gyro and accel, quaternions are written to the FIFO in
+ * the body frame, q30. The orientation is set by the scalar passed
+ * to dmp_set_orientation during initialization.
+ */
+ if (sensors & INV_WXYZ_QUAT) {
+ // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
+ }
+
+ if (sensors) {
+ read_none_count = 0;
+ } else {
+ read_none_count++;
+ if (read_none_count > 3) {
+ read_none_count = 0;
+
+ LOG("I2C may be stuck\r\n");
+ mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
+ }
+ }
+ }
+ }
}
void tap_cb(unsigned char direction, unsigned char count)
@@ -103,42 +162,17 @@
red = 1;
pc.baud(115200);
+
+ wait(1);
+
LOG("---- Seeed Tiny BLE ----\n");
- Ticker ticker;
- ticker.attach(tick, 3);
-
- button.fall(detect);
-
- LOG("Initialising the nRF51822\n");
- ble.init();
- ble.onDisconnection(disconnectionCallback);
- ble.onConnection(connectionCallback);
-
-
- /* setup advertising */
- ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
- ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
- ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
- (const uint8_t *)"smurfs", sizeof("smurfs"));
- ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
- (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
- DFUService dfu(ble);
- UARTService uartService(ble);
- uartServicePtr = &uartService;
- //uartService.retargetStdout();
-
- ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
- ble.startAdvertising();
-
- check_i2c_bus();
-
+ mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL);
mbed_i2c_init(MPU6050_SDA, MPU6050_SCL);
- motion_probe.fall(motion_interrupt_handle);
if (mpu_init(0)) {
-// LOG("failed to initialize mpu6050\r\n");
+ LOG("failed to initialize mpu6050\r\n");
}
/* Get/set hardware configuration. Start gyro. */
@@ -170,88 +204,48 @@
// dmp_set_interrupt_mode(DMP_INT_GESTURE);
dmp_set_tap_thresh(TAP_XYZ, 50);
+
+
+ motion_probe.fall(motion_interrupt_handle);
+
+ Ticker ticker;
+ ticker.attach(tick, 3);
+
+ button.fall(detect);
+
+ LOG("Initialising the nRF51822\n");
+ ble.init();
+ ble.onDisconnection(disconnectionCallback);
+ ble.onConnection(connectionCallback);
+
+
+ /* setup advertising */
+ ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
+ ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+ ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
+ (const uint8_t *)"smurfs", sizeof("smurfs"));
+ ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
+ (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
+ DFUService dfu(ble);
+ UARTService uartService(ble);
+ uartServicePtr = &uartService;
+ //uartService.retargetStdout();
+
+ ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
+ ble.startAdvertising();
+
while (true) {
if (motion_event) {
motion_event = 0;
- unsigned long sensor_timestamp;
- short gyro[3], accel[3], sensors;
- long quat[4];
- unsigned char more = 1;
-
- while (more) {
- /* This function gets new data from the FIFO when the DMP is in
- * use. The FIFO can contain any combination of gyro, accel,
- * quaternion, and gesture data. The sensors parameter tells the
- * caller which data fields were actually populated with new data.
- * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
- * the FIFO isn't being filled with accel data.
- * The driver parses the gesture data to determine if a gesture
- * event has occurred; on an event, the application will be notified
- * via a callback (assuming that a callback function was properly
- * registered). The more parameter is non-zero if there are
- * leftover packets in the FIFO.
- */
- dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,
- &more);
- /* Gyro and accel data are written to the FIFO by the DMP in chip
- * frame and hardware units. This behavior is convenient because it
- * keeps the gyro and accel outputs of dmp_read_fifo and
- * mpu_read_fifo consistent.
- */
- if (sensors & INV_XYZ_GYRO) {
- // LOG("GYRO: %d, %d, %d\n", gyro[0], gyro[1], gyro[2]);
- }
- if (sensors & INV_XYZ_ACCEL) {
- // LOG("ACC: %d, %d, %d\n", accel[0], accel[1], accel[2]);
- }
-
- /* Unlike gyro and accel, quaternions are written to the FIFO in
- * the body frame, q30. The orientation is set by the scalar passed
- * to dmp_set_orientation during initialization.
- */
- if (sensors & INV_WXYZ_QUAT) {
- // LOG("QUAT: %ld, %ld, %ld, %ld\n", quat[0], quat[1], quat[2], quat[3]);
- }
- }
+
} else {
ble.waitForEvent();
}
}
}
-void check_i2c_bus(void)
-{
-
- DigitalInOut scl(MPU6050_SCL);
- DigitalInOut sda(MPU6050_SDA);
-
- scl.input();
- sda.input();
- int scl_level = scl;
- int sda_level = sda;
- if (scl_level == 0 || sda_level == 0) {
- printf("scl: %d, sda: %d, i2c bus is not released\r\n", scl_level, sda_level);
-
- scl.output();
- for (int i = 0; i < 8; i++) {
- scl = 0;
- wait_us(10);
- scl = 1;
- wait_us(10);
- }
- }
-
- scl.input();
-
- scl_level = scl;
- sda_level = sda;
- if (scl_level == 0 || sda_level == 0) {
- printf("scl: %d, sda: %d, i2c bus is still not released\r\n", scl_level, sda_level);
- }
-}
-
/* These next two functions converts the orientation matrix (see
* gyro_orientation) to a scalar representation for use by the DMP.
* NOTE: These functions are borrowed from Invensense's MPL.