Petr Manas / Mbed OS pdiot-f-mbed

PDIOT Group F project

Dependencies:   MPU9250

source/main.cpp@8:ed92317b2c9b, 2019-11-05 (annotated)

Committer:
brano2
Date:
Tue Nov 05 13:54:48 2019 +0000
Revision:
8:ed92317b2c9b
Parent:
7:db23f12fcd02 
Print acceleration at slow rate

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fasand 3:7ad91f59dcfa 1 /* mbed Microcontroller Library
fasand 3:7ad91f59dcfa 2 * Copyright (c) 2006-2014 ARM Limited
fasand 3:7ad91f59dcfa 3 *
fasand 3:7ad91f59dcfa 4 * Licensed under the Apache License, Version 2.0 (the "License");
fasand 3:7ad91f59dcfa 5 * you may not use this file except in compliance with the License.
fasand 3:7ad91f59dcfa 6 * You may obtain a copy of the License at
fasand 3:7ad91f59dcfa 7 *
fasand 3:7ad91f59dcfa 8 * http://www.apache.org/licenses/LICENSE-2.0
fasand 3:7ad91f59dcfa 9 *
fasand 3:7ad91f59dcfa 10 * Unless required by applicable law or agreed to in writing, software
fasand 3:7ad91f59dcfa 11 * distributed under the License is distributed on an "AS IS" BASIS,
fasand 3:7ad91f59dcfa 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
fasand 3:7ad91f59dcfa 13 * See the License for the specific language governing permissions and
fasand 3:7ad91f59dcfa 14 * limitations under the License.
fasand 3:7ad91f59dcfa 15 */
fasand 3:7ad91f59dcfa 16
fasand 3:7ad91f59dcfa 17 #include <events/mbed_events.h>
fasand 3:7ad91f59dcfa 18 #include <mbed.h>
fasand 1:eb38d5821d79 19 #include "ble/BLE.h"
fasand 3:7ad91f59dcfa 20 #include "ble/Gap.h"
fasand 3:7ad91f59dcfa 21 #include "ble/services/BatteryService.h"
brano2 7:db23f12fcd02 22 #include "ImuService.h"
brano2 4:357e8a209777 23 #include "MPU9250.h"
fasand 3:7ad91f59dcfa 24 #include "pretty_printer.h"
fasand 3:7ad91f59dcfa 25
brano2 4:357e8a209777 26
fasand 3:7ad91f59dcfa 27 static DigitalOut led1(LED1, 1);
fasand 3:7ad91f59dcfa 28
fasand 3:7ad91f59dcfa 29 const static char DEVICE_NAME[] = "BATTERY GROUP F";
fasand 3:7ad91f59dcfa 30
fasand 3:7ad91f59dcfa 31 static events::EventQueue event_queue(/* event count */ 16 * EVENTS_EVENT_SIZE);
fasand 1:eb38d5821d79 32
fasand 3:7ad91f59dcfa 33 class BatteryDemo : ble::Gap::EventHandler {
fasand 3:7ad91f59dcfa 34 public:
fasand 3:7ad91f59dcfa 35 BatteryDemo(BLE &ble, events::EventQueue &event_queue) :
fasand 3:7ad91f59dcfa 36 _ble(ble),
fasand 3:7ad91f59dcfa 37 _event_queue(event_queue),
fasand 3:7ad91f59dcfa 38 _battery_uuid(GattService::UUID_BATTERY_SERVICE),
fasand 3:7ad91f59dcfa 39 _battery_level(50),
fasand 3:7ad91f59dcfa 40 _battery_service(ble, _battery_level),
brano2 4:357e8a209777 41 _adv_data_builder(_adv_buffer),
brano2 5:a06ee79f2c4b 42 _mpu(p26, p27),
brano2 5:a06ee79f2c4b 43 _imu_service(NULL) { }
fasand 3:7ad91f59dcfa 44
fasand 3:7ad91f59dcfa 45 void start() {
fasand 3:7ad91f59dcfa 46 _ble.gap().setEventHandler(this);
fasand 3:7ad91f59dcfa 47
fasand 3:7ad91f59dcfa 48 _ble.init(this, &BatteryDemo::on_init_complete);
brano2 4:357e8a209777 49
brano2 5:a06ee79f2c4b 50 printf("Initializing sensor\r\n");
brano2 4:357e8a209777 51 _mpu.Ascale = AFS_2G;
brano2 4:357e8a209777 52 _mpu.Gscale = GFS_250DPS;
brano2 4:357e8a209777 53 _mpu.Mscale = MFS_14BITS;
brano2 4:357e8a209777 54 _mpu.initMPU9250();
brano2 5:a06ee79f2c4b 55 printf("Initialization finished\r\n");
brano2 4:357e8a209777 56 _mpu.getAres();
brano2 5:a06ee79f2c4b 57 printf("MPU aRes = %f, Ascale = %d\r\n", _mpu.aRes, _mpu.Ascale);
brano2 4:357e8a209777 58
fasand 3:7ad91f59dcfa 59
brano2 7:db23f12fcd02 60 _event_queue.call_every(50, this, &BatteryDemo::send_acceleration);
brano2 8:ed92317b2c9b 61 _event_queue.call_every(2000, this, &BatteryDemo::print_last_acceleration);
fasand 3:7ad91f59dcfa 62 _event_queue.call_every(500, this, &BatteryDemo::blink);
fasand 3:7ad91f59dcfa 63 _event_queue.call_every(1000, this, &BatteryDemo::update_sensor_value);
fasand 3:7ad91f59dcfa 64
fasand 3:7ad91f59dcfa 65 _event_queue.dispatch_forever();
fasand 3:7ad91f59dcfa 66 }
fasand 1:eb38d5821d79 67
fasand 3:7ad91f59dcfa 68 private:
fasand 3:7ad91f59dcfa 69 /** Callback triggered when the ble initialization process has finished */
fasand 3:7ad91f59dcfa 70 void on_init_complete(BLE::InitializationCompleteCallbackContext *params) {
fasand 3:7ad91f59dcfa 71 if (params->error != BLE_ERROR_NONE) {
fasand 3:7ad91f59dcfa 72 print_error(params->error, "Ble initialization failed.");
fasand 3:7ad91f59dcfa 73 return;
fasand 3:7ad91f59dcfa 74 }
fasand 3:7ad91f59dcfa 75
fasand 3:7ad91f59dcfa 76 print_mac_address();
fasand 3:7ad91f59dcfa 77
brano2 5:a06ee79f2c4b 78 /* Setup primary service. */
brano2 5:a06ee79f2c4b 79 int16_t acc[3] = {0, 0, 0};
brano2 5:a06ee79f2c4b 80 _imu_service = new ImuService(_ble, acc /* initial value */);
brano2 5:a06ee79f2c4b 81
fasand 3:7ad91f59dcfa 82 start_advertising();
fasand 3:7ad91f59dcfa 83 }
fasand 3:7ad91f59dcfa 84
fasand 3:7ad91f59dcfa 85 void start_advertising() {
fasand 3:7ad91f59dcfa 86 /* Create advertising parameters and payload */
fasand 3:7ad91f59dcfa 87
fasand 3:7ad91f59dcfa 88 ble::AdvertisingParameters adv_parameters(
fasand 3:7ad91f59dcfa 89 ble::advertising_type_t::CONNECTABLE_UNDIRECTED,
fasand 3:7ad91f59dcfa 90 ble::adv_interval_t(ble::millisecond_t(1000))
fasand 3:7ad91f59dcfa 91 );
fasand 1:eb38d5821d79 92
fasand 3:7ad91f59dcfa 93 _adv_data_builder.setFlags();
fasand 3:7ad91f59dcfa 94 _adv_data_builder.setLocalServiceList(mbed::make_Span(&_battery_uuid, 1));
fasand 3:7ad91f59dcfa 95 _adv_data_builder.setName(DEVICE_NAME);
fasand 3:7ad91f59dcfa 96
fasand 3:7ad91f59dcfa 97 /* Setup advertising */
fasand 3:7ad91f59dcfa 98
fasand 3:7ad91f59dcfa 99 ble_error_t error = _ble.gap().setAdvertisingParameters(
fasand 3:7ad91f59dcfa 100 ble::LEGACY_ADVERTISING_HANDLE,
fasand 3:7ad91f59dcfa 101 adv_parameters
fasand 3:7ad91f59dcfa 102 );
fasand 3:7ad91f59dcfa 103
fasand 3:7ad91f59dcfa 104 if (error) {
fasand 3:7ad91f59dcfa 105 print_error(error, "_ble.gap().setAdvertisingParameters() failed");
fasand 3:7ad91f59dcfa 106 return;
fasand 3:7ad91f59dcfa 107 }
fasand 0:68da41d2fe5c 108
fasand 3:7ad91f59dcfa 109 error = _ble.gap().setAdvertisingPayload(
fasand 3:7ad91f59dcfa 110 ble::LEGACY_ADVERTISING_HANDLE,
fasand 3:7ad91f59dcfa 111 _adv_data_builder.getAdvertisingData()
fasand 3:7ad91f59dcfa 112 );
fasand 3:7ad91f59dcfa 113
fasand 3:7ad91f59dcfa 114 if (error) {
fasand 3:7ad91f59dcfa 115 print_error(error, "_ble.gap().setAdvertisingPayload() failed");
fasand 3:7ad91f59dcfa 116 return;
fasand 3:7ad91f59dcfa 117 }
fasand 3:7ad91f59dcfa 118
fasand 3:7ad91f59dcfa 119 /* Start advertising */
fasand 3:7ad91f59dcfa 120
fasand 3:7ad91f59dcfa 121 error = _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
fasand 3:7ad91f59dcfa 122
fasand 3:7ad91f59dcfa 123 if (error) {
fasand 3:7ad91f59dcfa 124 print_error(error, "_ble.gap().startAdvertising() failed");
fasand 3:7ad91f59dcfa 125 return;
fasand 3:7ad91f59dcfa 126 }
fasand 3:7ad91f59dcfa 127 }
fasand 1:eb38d5821d79 128
fasand 3:7ad91f59dcfa 129 void update_sensor_value() {
fasand 3:7ad91f59dcfa 130 if (_ble.gap().getState().connected) {
fasand 3:7ad91f59dcfa 131 _battery_level++;
fasand 3:7ad91f59dcfa 132 if (_battery_level > 100) {
fasand 3:7ad91f59dcfa 133 _battery_level = 20;
fasand 3:7ad91f59dcfa 134 }
fasand 1:eb38d5821d79 135
fasand 3:7ad91f59dcfa 136 _battery_service.updateBatteryLevel(_battery_level);
fasand 3:7ad91f59dcfa 137 }
fasand 3:7ad91f59dcfa 138 }
brano2 4:357e8a209777 139
brano2 7:db23f12fcd02 140 void send_acceleration() {
brano2 8:ed92317b2c9b 141 _mpu.readAccelData(_acc);
brano2 8:ed92317b2c9b 142 _imu_service->updateImuState(_acc);
brano2 8:ed92317b2c9b 143 // _event_queue.call(Callback<void(int16_t*)>(_imu_service, &ImuService::updateImuState), acc);
brano2 8:ed92317b2c9b 144 }
brano2 8:ed92317b2c9b 145
brano2 8:ed92317b2c9b 146 void print_last_acceleration() {
brano2 4:357e8a209777 147 float scale = _mpu.aRes;
brano2 8:ed92317b2c9b 148 printf("acc: %f, %f, %f\r\n",
brano2 8:ed92317b2c9b 149 _acc[0]*scale, _acc[1]*scale, _acc[2]*scale);
brano2 4:357e8a209777 150 }
fasand 3:7ad91f59dcfa 151
fasand 3:7ad91f59dcfa 152 void blink(void) {
fasand 3:7ad91f59dcfa 153 led1 = !led1;
fasand 3:7ad91f59dcfa 154 }
fasand 3:7ad91f59dcfa 155
fasand 3:7ad91f59dcfa 156 private:
fasand 3:7ad91f59dcfa 157 /* Event handler */
fasand 0:68da41d2fe5c 158
fasand 3:7ad91f59dcfa 159 void onDisconnectionComplete(const ble::DisconnectionCompleteEvent&) {
fasand 3:7ad91f59dcfa 160 _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
fasand 3:7ad91f59dcfa 161 }
fasand 3:7ad91f59dcfa 162
fasand 3:7ad91f59dcfa 163 private:
fasand 3:7ad91f59dcfa 164 BLE &_ble;
fasand 3:7ad91f59dcfa 165 events::EventQueue &_event_queue;
fasand 3:7ad91f59dcfa 166
fasand 3:7ad91f59dcfa 167 UUID _battery_uuid;
fasand 3:7ad91f59dcfa 168
fasand 3:7ad91f59dcfa 169 uint8_t _battery_level;
fasand 3:7ad91f59dcfa 170 BatteryService _battery_service;
fasand 3:7ad91f59dcfa 171
fasand 3:7ad91f59dcfa 172 uint8_t _adv_buffer[ble::LEGACY_ADVERTISING_MAX_SIZE];
fasand 3:7ad91f59dcfa 173 ble::AdvertisingDataBuilder _adv_data_builder;
brano2 4:357e8a209777 174
brano2 4:357e8a209777 175 MPU9250 _mpu;
brano2 5:a06ee79f2c4b 176 ImuService* _imu_service;
brano2 8:ed92317b2c9b 177 int16_t _acc[3] = {0,0,0};
fasand 3:7ad91f59dcfa 178 };
fasand 3:7ad91f59dcfa 179
fasand 3:7ad91f59dcfa 180 /** Schedule processing of events from the BLE middleware in the event queue. */
fasand 3:7ad91f59dcfa 181 void schedule_ble_events(BLE::OnEventsToProcessCallbackContext *context) {
fasand 3:7ad91f59dcfa 182 event_queue.call(Callback<void()>(&context->ble, &BLE::processEvents));
fasand 1:eb38d5821d79 183 }
fasand 0:68da41d2fe5c 184
fasand 3:7ad91f59dcfa 185 int main()
fasand 1:eb38d5821d79 186 {
fasand 3:7ad91f59dcfa 187 BLE &ble = BLE::Instance();
fasand 3:7ad91f59dcfa 188 ble.onEventsToProcess(schedule_ble_events);
fasand 0:68da41d2fe5c 189
fasand 3:7ad91f59dcfa 190 BatteryDemo demo(ble, event_queue);
fasand 3:7ad91f59dcfa 191 demo.start();
fasand 0:68da41d2fe5c 192
fasand 3:7ad91f59dcfa 193 return 0;
fasand 1:eb38d5821d79 194 }