Use Tiny BLE as a joystick. The data is got from an external data source via a serial port.
Dependencies: BLE_API BLE_HID eMPL_MPU6050 mbed nRF51822
Fork of Seeed_Tiny_BLE_Get_Started by
Diff: main.cpp
- Revision:
- 4:89f37a17eba6
- Parent:
- 3:24e365bd1b97
--- a/main.cpp Thu Nov 05 06:58:30 2015 +0000 +++ b/main.cpp Mon Jun 12 04:10:04 2017 +0000 @@ -1,298 +1,140 @@ - #include "mbed.h" -#include "mbed_i2c.h" -#include "inv_mpu.h" -#include "inv_mpu_dmp_motion_driver.h" #include "nrf51.h" #include "nrf51_bitfields.h" -#include "BLE.h" +#include "ble/BLE.h" +#include "JoystickService.h" #include "DFUService.h" #include "UARTService.h" +#include "examples_common.h" #define LOG(...) { pc.printf(__VA_ARGS__); } #define LED_GREEN p21 #define LED_RED p22 #define LED_BLUE p23 -#define BUTTON_PIN p17 -#define BATTERY_PIN p1 -#define MPU6050_SDA p12 -#define MPU6050_SCL p13 +#define LED_OFF 1 +#define LED_ON 0 #define UART_TX p9 #define UART_RX p11 #define UART_CTS p8 #define UART_RTS p10 -/* Starting sampling rate. */ -#define DEFAULT_MPU_HZ (100) +#define DATA_TX p3 +#define DATA_RX p6 DigitalOut blue(LED_BLUE); DigitalOut green(LED_GREEN); DigitalOut red(LED_RED); -InterruptIn button(BUTTON_PIN); -AnalogIn battery(BATTERY_PIN); Serial pc(UART_TX, UART_RX); +Serial data(DATA_TX, DATA_RX); -InterruptIn motion_probe(p14); - -int read_none_count = 0; - -BLEDevice ble; -UARTService *uartServicePtr; +BLE ble; +JoystickService *joystickServicePtr; +static const char DEVICE_NAME[] = "BunnyJoystick"; +static const char SHORT_DEVICE_NAME[] = "joystick0"; volatile bool bleIsConnected = false; volatile uint8_t tick_event = 0; -volatile uint8_t motion_event = 0; -static signed char board_orientation[9] = { - 1, 0, 0, - 0, 1, 0, - 0, 0, 1 -}; - -void check_i2c_bus(void); -unsigned short inv_orientation_matrix_to_scalar( const signed char *mtx); - - -void connectionCallback(const Gap::ConnectionCallbackParams_t *params) -{ - LOG("Connected!\n"); - bleIsConnected = true; -} - -void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *cbParams) -{ - LOG("Disconnected!\n"); - LOG("Restarting the advertising process\n"); - ble.startAdvertising(); +static void onDisconnect(const Gap::DisconnectionCallbackParams_t *params) { + LOG("disconnected\r\n"); bleIsConnected = false; + red = LED_OFF; + blue = LED_OFF; + green = LED_ON; + ble.gap().startAdvertising(); // restart advertising } -void tick(void) -{ - static uint32_t count = 0; - - LOG("%d\r\n", count++); - green = !green; -} -void detect(void) -{ - LOG("Button pressed\n"); - blue = !blue; +static void onConnect(const Gap::ConnectionCallbackParams_t *params) { + LOG("connected\r\n"); + bleIsConnected = true; + red = LED_OFF; + blue = LED_ON; + green = LED_OFF; } -void motion_interrupt_handle(void) -{ - motion_event = 1; -} - -void tap_cb(unsigned char direction, unsigned char count) -{ - LOG("Tap motion detected\n"); -} - -void android_orient_cb(unsigned char orientation) -{ - LOG("Oriention changed\n"); +static void waiting() { + if (!joystickServicePtr->isConnected()) { + green = !green; + } else { + blue = !blue; + } } -int main(void) -{ - blue = 1; - green = 1; - red = 1; - - pc.baud(115200); - - wait(1); - - LOG("---- Seeed Tiny BLE ----\n"); - - mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL); - mbed_i2c_init(MPU6050_SDA, MPU6050_SCL); - - - if (mpu_init(0)) { - LOG("failed to initialize mpu6050\r\n"); - } - - /* Get/set hardware configuration. Start gyro. */ - /* Wake up all sensors. */ - mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); - /* Push both gyro and accel data into the FIFO. */ - mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); - mpu_set_sample_rate(DEFAULT_MPU_HZ); - - /* Read back configuration in case it was set improperly. */ - unsigned char accel_fsr; - unsigned short gyro_rate, gyro_fsr; - mpu_get_sample_rate(&gyro_rate); - mpu_get_gyro_fsr(&gyro_fsr); - mpu_get_accel_fsr(&accel_fsr); - - dmp_load_motion_driver_firmware(); - dmp_set_orientation( - inv_orientation_matrix_to_scalar(board_orientation)); - dmp_register_tap_cb(tap_cb); - dmp_register_android_orient_cb(android_orient_cb); +int main() { + blue = LED_OFF; + green = LED_OFF; + red = LED_OFF; - uint16_t dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | - DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | - DMP_FEATURE_GYRO_CAL; - dmp_enable_feature(dmp_features); - dmp_set_fifo_rate(DEFAULT_MPU_HZ); - mpu_set_dmp_state(1); - - dmp_set_interrupt_mode(DMP_INT_GESTURE); - dmp_set_tap_thresh(TAP_XYZ, 50); - - - motion_probe.fall(motion_interrupt_handle); + data.baud(115200); + + wait(4); + LOG("Bunny Joystick started.\n"); + LOG("initialising ticker\r\n"); + Ticker heartbeat; + heartbeat.attach(waiting, 1); - - Ticker ticker; - ticker.attach(tick, 3); + LOG("initialising ble\r\n"); + ble.init(); - button.fall(detect); + ble.gap().onDisconnection(onDisconnect); + ble.gap().onConnection(onConnect); + + initializeSecurity(ble); - LOG("Initialising the nRF51822\n"); - ble.init(); - ble.gap().onDisconnection(disconnectionCallback); - ble.gap().onConnection(connectionCallback); + LOG("adding hid service\r\n"); + JoystickService joystickService(ble); + joystickServicePtr = &joystickService; + LOG("adding dev info and battery service\r\n"); + initializeHOGP(ble); - /* 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(); + LOG("setting up gap\r\n"); + ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::JOYSTICK); + ble.gap().accumulateAdvertisingPayload( + GapAdvertisingData::COMPLETE_LOCAL_NAME, + (const uint8_t *)DEVICE_NAME, + sizeof(DEVICE_NAME)); + ble.gap().accumulateAdvertisingPayload( + GapAdvertisingData::SHORTENED_LOCAL_NAME, + (const uint8_t *)SHORT_DEVICE_NAME, + sizeof(SHORT_DEVICE_NAME)); + ble.gap().setDeviceName((const uint8_t *)DEVICE_NAME); - ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */ + LOG("advertising\r\n"); ble.gap().startAdvertising(); + int xraw, yraw, zraw; + while (true) { - if (motion_event) { - - 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 @ %d\r\n", sensor_timestamp); - mbed_i2c_clear(MPU6050_SDA, MPU6050_SCL); - } + if (data.readable()) { + char buffer[256] = {0}; + char c; + int i = 0; + while(data.readable() && i < 256) { + buffer[i++] = c = data.getc(); + if (c == '\n') { + buffer[i] = 0; + break; } } + LOG("Received data from FTHR:\n"); + LOG(buffer); + sscanf(buffer, "%d,%d,%d", &xraw, &yraw, &zraw); + LOG("%d,%d,%d", xraw, yraw, zraw); + joystickServicePtr->setSpeed(xraw, yraw, zraw); - motion_event = 0; - } else { - ble.waitForEvent(); + wait(0.5); } + + ble.waitForEvent(); } } - -/* 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. - */ -static inline unsigned short inv_row_2_scale(const signed char *row) -{ - unsigned short b; - - if (row[0] > 0) - b = 0; - else if (row[0] < 0) - b = 4; - else if (row[1] > 0) - b = 1; - else if (row[1] < 0) - b = 5; - else if (row[2] > 0) - b = 2; - else if (row[2] < 0) - b = 6; - else - b = 7; // error - return b; -} - -unsigned short inv_orientation_matrix_to_scalar( - const signed char *mtx) -{ - unsigned short scalar; - - /* - XYZ 010_001_000 Identity Matrix - XZY 001_010_000 - YXZ 010_000_001 - YZX 000_010_001 - ZXY 001_000_010 - ZYX 000_001_010 - */ - - scalar = inv_row_2_scale(mtx); - scalar |= inv_row_2_scale(mtx + 3) << 3; - scalar |= inv_row_2_scale(mtx + 6) << 6; - - - return scalar; -} -