Diff: source/main.cpp

Revision:

4:357e8a209777

Parent:

3:7ad91f59dcfa

Child:

5:a06ee79f2c4b

--- a/source/main.cpp	Wed Oct 23 12:57:13 2019 +0000
+++ b/source/main.cpp	Mon Oct 28 17:51:04 2019 +0000
@@ -19,8 +19,10 @@
 #include "ble/BLE.h"
 #include "ble/Gap.h"
 #include "ble/services/BatteryService.h"
+#include "MPU9250.h"
 #include "pretty_printer.h"
 
+
 static DigitalOut led1(LED1, 1);
 
 const static char DEVICE_NAME[] = "BATTERY GROUP F";
@@ -35,13 +37,25 @@
         _battery_uuid(GattService::UUID_BATTERY_SERVICE),
         _battery_level(50),
         _battery_service(ble, _battery_level),
-        _adv_data_builder(_adv_buffer) { }
+        _adv_data_builder(_adv_buffer),
+        _mpu(p26, p27) { }
 
     void start() {
         _ble.gap().setEventHandler(this);
 
         _ble.init(this, &BatteryDemo::on_init_complete);
+        
+        printf("Initializing sensor\n\r");
+        _mpu.Ascale = AFS_2G;
+        _mpu.Gscale = GFS_250DPS;
+        _mpu.Mscale = MFS_14BITS;
+        _mpu.initMPU9250();
+        printf("Initialization finished\n\r");
+        _mpu.getAres();
+        printf("MPU aRes = %f, Ascale = %d\n", _mpu.aRes, _mpu.Ascale);
+        
 
+        _event_queue.call_every(1500, this, &BatteryDemo::hello);
         _event_queue.call_every(500, this, &BatteryDemo::blink);
         _event_queue.call_every(1000, this, &BatteryDemo::update_sensor_value);
 
@@ -115,6 +129,15 @@
             _battery_service.updateBatteryLevel(_battery_level);
         }
     }
+    
+    void hello() {
+//        printf("Hello world!\n");
+        int16_t acc[3] = {0,0,0};
+        _mpu.readAccelData(acc);
+        float scale = _mpu.aRes;
+        printf("acc: %f, %f, %f\n",
+               acc[0]*scale, acc[1]*scale, acc[2]*scale);
+    }
 
     void blink(void) {
         led1 = !led1;
@@ -138,6 +161,8 @@
 
     uint8_t _adv_buffer[ble::LEGACY_ADVERTISING_MAX_SIZE];
     ble::AdvertisingDataBuilder _adv_data_builder;
+    
+    MPU9250 _mpu;
 };
 
 /** Schedule processing of events from the BLE middleware in the event queue. */