diff options
Diffstat (limited to 'src/components')
-rw-r--r-- | src/components/battery/BatteryController.cpp | 8 | ||||
-rw-r--r-- | src/components/battery/BatteryController.h | 39 | ||||
-rw-r--r-- | src/components/ble/HeartRateService.cpp | 82 | ||||
-rw-r--r-- | src/components/ble/HeartRateService.h | 44 | ||||
-rw-r--r-- | src/components/ble/MusicService.cpp | 88 | ||||
-rw-r--r-- | src/components/ble/MusicService.h | 30 | ||||
-rw-r--r-- | src/components/ble/NavigationService.cpp | 137 | ||||
-rw-r--r-- | src/components/ble/NavigationService.h | 96 | ||||
-rw-r--r-- | src/components/ble/NimbleController.cpp | 9 | ||||
-rw-r--r-- | src/components/ble/NimbleController.h | 8 | ||||
-rw-r--r-- | src/components/heartrate/Biquad.cpp | 27 | ||||
-rw-r--r-- | src/components/heartrate/Biquad.h | 22 | ||||
-rw-r--r-- | src/components/heartrate/HeartRateController.cpp | 41 | ||||
-rw-r--r-- | src/components/heartrate/HeartRateController.h | 38 | ||||
-rw-r--r-- | src/components/heartrate/Ppg.cpp | 106 | ||||
-rw-r--r-- | src/components/heartrate/Ppg.h | 31 | ||||
-rw-r--r-- | src/components/heartrate/Ptagc.cpp | 28 | ||||
-rw-r--r-- | src/components/heartrate/Ptagc.h | 18 |
18 files changed, 803 insertions, 49 deletions
diff --git a/src/components/battery/BatteryController.cpp b/src/components/battery/BatteryController.cpp index 3e3d65b4..aaa245e4 100644 --- a/src/components/battery/BatteryController.cpp +++ b/src/components/battery/BatteryController.cpp @@ -34,9 +34,11 @@ void Battery::Update() { // see https://forum.pine64.org/showthread.php?tid=8147 voltage = (value * 2.0f) / (1024/3.0f); - percentRemaining = ((voltage - 3.55f)*100.0f)*3.9f; - percentRemaining = std::max(percentRemaining, 0.0f); - percentRemaining = std::min(percentRemaining, 100.0f); + int percentRemaining = ((voltage - 3.55f)*100.0f)*3.9f; + percentRemaining = std::max(percentRemaining, 0); + percentRemaining = std::min(percentRemaining, 100); + + percentRemainingBuffer.insert(percentRemaining); // NRF_LOG_INFO("BATTERY " NRF_LOG_FLOAT_MARKER " %% - " NRF_LOG_FLOAT_MARKER " v", NRF_LOG_FLOAT(percentRemaining), NRF_LOG_FLOAT(voltage)); // NRF_LOG_INFO("POWER Charging : %d - Power : %d", isCharging, isPowerPresent); diff --git a/src/components/battery/BatteryController.h b/src/components/battery/BatteryController.h index 7cc964e4..86250a57 100644 --- a/src/components/battery/BatteryController.h +++ b/src/components/battery/BatteryController.h @@ -1,14 +1,48 @@ #pragma once #include <cstdint> #include <drivers/include/nrfx_saadc.h> +#include <array> +#include <numeric> namespace Pinetime { namespace Controllers { + /** A simple circular buffer that can be used to average + out the sensor values. The total capacity of the CircBuffer + is given as the template parameter N. + */ + template <int N> + class CircBuffer { + public: + CircBuffer() : arr{}, sz{}, cap{N}, head{} {} + /** + insert member function overwrites the next data to the current + HEAD and moves the HEAD to the newly inserted value. + */ + void insert(const int num) { + head %= cap; + arr[head++] = num; + if (sz != cap) { + sz++; + } + } + + int GetAverage() const { + int sum = std::accumulate(arr.begin(), arr.end(), 0); + return (sum / sz); + } + + private: + std::array<int, N> arr; /**< internal array used to store the values*/ + uint8_t sz; /**< The current size of the array.*/ + uint8_t cap; /**< Total capacity of the CircBuffer.*/ + uint8_t head; /**< The current head of the CircBuffer*/ + }; + class Battery { public: void Init(); void Update(); - float PercentRemaining() const { return percentRemaining; } + int PercentRemaining() const { return percentRemainingBuffer.GetAverage(); } float Voltage() const { return voltage; } bool IsCharging() const { return isCharging; } bool IsPowerPresent() const { return isPowerPresent; } @@ -17,8 +51,9 @@ namespace Pinetime { static constexpr uint32_t chargingPin = 12; static constexpr uint32_t powerPresentPin = 19; static constexpr nrf_saadc_input_t batteryVoltageAdcInput = NRF_SAADC_INPUT_AIN7; + static constexpr uint8_t percentRemainingSamples = 10; static void SaadcEventHandler(nrfx_saadc_evt_t const * p_event); - float percentRemaining = 0.0f; + CircBuffer<percentRemainingSamples> percentRemainingBuffer {}; float voltage = 0.0f; bool isCharging = false; bool isPowerPresent = false; diff --git a/src/components/ble/HeartRateService.cpp b/src/components/ble/HeartRateService.cpp new file mode 100644 index 00000000..ecd6235d --- /dev/null +++ b/src/components/ble/HeartRateService.cpp @@ -0,0 +1,82 @@ +#include "HeartRateService.h" +#include "components/heartrate/HeartRateController.h" +#include "systemtask/SystemTask.h" + +using namespace Pinetime::Controllers; + +constexpr ble_uuid16_t HeartRateService::heartRateServiceUuid; +constexpr ble_uuid16_t HeartRateService::heartRateMeasurementUuid; + +namespace { + int HeartRateServiceServiceCallback(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { + auto* heartRateService = static_cast<HeartRateService*>(arg); + return heartRateService->OnHeartRateRequested(conn_handle, attr_handle, ctxt); + } +} + +// TODO Refactoring - remove dependency to SystemTask +HeartRateService::HeartRateService(Pinetime::System::SystemTask &system, Controllers::HeartRateController& heartRateController) : + system{system}, + heartRateController{heartRateController}, + characteristicDefinition{ + { + .uuid = (ble_uuid_t *) &heartRateMeasurementUuid, + .access_cb = HeartRateServiceServiceCallback, + .arg = this, + .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_NOTIFY, + .val_handle = &heartRateMeasurementHandle + }, + { + 0 + } + }, + serviceDefinition{ + { + /* Device Information Service */ + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = (ble_uuid_t *) &heartRateServiceUuid, + .characteristics = characteristicDefinition + }, + { + 0 + }, + }{ + // TODO refactor to prevent this loop dependency (service depends on controller and controller depends on service) + heartRateController.SetService(this); +} + +void HeartRateService::Init() { + int res = 0; + res = ble_gatts_count_cfg(serviceDefinition); + ASSERT(res == 0); + + res = ble_gatts_add_svcs(serviceDefinition); + ASSERT(res == 0); +} + +int HeartRateService::OnHeartRateRequested(uint16_t connectionHandle, uint16_t attributeHandle, + ble_gatt_access_ctxt *context) { + if(attributeHandle == heartRateMeasurementHandle) { + NRF_LOG_INFO("BATTERY : handle = %d", heartRateMeasurementHandle); + static uint8_t batteryValue = heartRateController.HeartRate(); + + uint8_t buffer[2] = {0, heartRateController.HeartRate()}; // [0] = flags, [1] = hr value + + int res = os_mbuf_append(context->om, buffer, 2); + return (res == 0) ? 0 : BLE_ATT_ERR_INSUFFICIENT_RES; + } + return 0; +} + +void HeartRateService::OnNewHeartRateValue(uint8_t heartRateValue) { + uint8_t buffer[2] = {0, heartRateController.HeartRate()}; // [0] = flags, [1] = hr value + auto *om = ble_hs_mbuf_from_flat(buffer, 2); + + uint16_t connectionHandle = system.nimble().connHandle(); + + if (connectionHandle == 0 || connectionHandle == BLE_HS_CONN_HANDLE_NONE) { + return; + } + + ble_gattc_notify_custom(connectionHandle, heartRateMeasurementHandle, om); +} diff --git a/src/components/ble/HeartRateService.h b/src/components/ble/HeartRateService.h new file mode 100644 index 00000000..835e2941 --- /dev/null +++ b/src/components/ble/HeartRateService.h @@ -0,0 +1,44 @@ +#pragma once +#define min // workaround: nimble's min/max macros conflict with libstdc++ +#define max +#include <host/ble_gap.h> +#undef max +#undef min + +namespace Pinetime { + namespace System { + class SystemTask; + } + namespace Controllers { + class HeartRateController; + class HeartRateService { + public: + HeartRateService(Pinetime::System::SystemTask &system, Controllers::HeartRateController& heartRateController); + void Init(); + int OnHeartRateRequested(uint16_t connectionHandle, uint16_t attributeHandle, ble_gatt_access_ctxt *context); + void OnNewHeartRateValue(uint8_t hearRateValue); + + private: + Pinetime::System::SystemTask &system; + Controllers::HeartRateController& heartRateController; + static constexpr uint16_t heartRateServiceId {0x180D}; + static constexpr uint16_t heartRateMeasurementId {0x2A37}; + + static constexpr ble_uuid16_t heartRateServiceUuid { + .u {.type = BLE_UUID_TYPE_16}, + .value = heartRateServiceId + }; + + static constexpr ble_uuid16_t heartRateMeasurementUuid { + .u {.type = BLE_UUID_TYPE_16}, + .value = heartRateMeasurementId + }; + + struct ble_gatt_chr_def characteristicDefinition[3]; + struct ble_gatt_svc_def serviceDefinition[2]; + + uint16_t heartRateMeasurementHandle; + + }; + } +} diff --git a/src/components/ble/MusicService.cpp b/src/components/ble/MusicService.cpp index fdecb6b3..bd6e27fb 100644 --- a/src/components/ble/MusicService.cpp +++ b/src/components/ble/MusicService.cpp @@ -24,32 +24,68 @@ int MSCallback(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_acces } Pinetime::Controllers::MusicService::MusicService(Pinetime::System::SystemTask &system) : m_system(system) { - msUuid.value[11] = msId[0]; - msUuid.value[12] = msId[1]; - msEventCharUuid.value[11] = msEventCharId[0]; - msEventCharUuid.value[12] = msEventCharId[1]; - msStatusCharUuid.value[11] = msStatusCharId[0]; - msStatusCharUuid.value[12] = msStatusCharId[1]; - msTrackCharUuid.value[11] = msTrackCharId[0]; - msTrackCharUuid.value[12] = msTrackCharId[1]; - msArtistCharUuid.value[11] = msArtistCharId[0]; - msArtistCharUuid.value[12] = msArtistCharId[1]; - msAlbumCharUuid.value[11] = msAlbumCharId[0]; - msAlbumCharUuid.value[12] = msAlbumCharId[1]; - msPositionCharUuid.value[11] = msPositionCharId[0]; - msPositionCharUuid.value[12] = msPositionCharId[1]; - msTotalLengthCharUuid.value[11] = msTotalLengthCharId[0]; - msTotalLengthCharUuid.value[12] = msTotalLengthCharId[1]; - msTrackNumberCharUuid.value[11] = msTrackNumberCharId[0]; - msTrackNumberCharUuid.value[12] = msTrackNumberCharId[1]; - msTrackTotalCharUuid.value[11] = msTrackTotalCharId[0]; - msTrackTotalCharUuid.value[12] = msTrackTotalCharId[1]; - msPlaybackSpeedCharUuid.value[11] = msPlaybackSpeedCharId[0]; - msPlaybackSpeedCharUuid.value[12] = msPlaybackSpeedCharId[1]; - msRepeatCharUuid.value[11] = msRepeatCharId[0]; - msRepeatCharUuid.value[12] = msRepeatCharId[1]; - msShuffleCharUuid.value[11] = msShuffleCharId[0]; - msShuffleCharUuid.value[12] = msShuffleCharId[1]; + msUuid.value[14] = msId[0]; + msUuid.value[15] = msId[1]; + + msEventCharUuid.value[12] = msEventCharId[0]; + msEventCharUuid.value[13] = msEventCharId[1]; + msEventCharUuid.value[14] = msId[0]; + msEventCharUuid.value[15] = msId[1]; + + msStatusCharUuid.value[12] = msStatusCharId[0]; + msStatusCharUuid.value[13] = msStatusCharId[1]; + msStatusCharUuid.value[14] = msId[0]; + msStatusCharUuid.value[15] = msId[1]; + + msTrackCharUuid.value[12] = msTrackCharId[0]; + msTrackCharUuid.value[13] = msTrackCharId[1]; + msTrackCharUuid.value[14] = msId[0]; + msTrackCharUuid.value[15] = msId[1]; + + msArtistCharUuid.value[12] = msArtistCharId[0]; + msArtistCharUuid.value[13] = msArtistCharId[1]; + msArtistCharUuid.value[14] = msId[0]; + msArtistCharUuid.value[15] = msId[1]; + + msAlbumCharUuid.value[12] = msAlbumCharId[0]; + msAlbumCharUuid.value[13] = msAlbumCharId[1]; + msAlbumCharUuid.value[14] = msId[0]; + msAlbumCharUuid.value[15] = msId[1]; + + msPositionCharUuid.value[12] = msPositionCharId[0]; + msPositionCharUuid.value[13] = msPositionCharId[1]; + msPositionCharUuid.value[14] = msId[0]; + msPositionCharUuid.value[15] = msId[1]; + + msTotalLengthCharUuid.value[12] = msTotalLengthCharId[0]; + msTotalLengthCharUuid.value[13] = msTotalLengthCharId[1]; + msTotalLengthCharUuid.value[14] = msId[0]; + msTotalLengthCharUuid.value[15] = msId[1]; + + msTrackNumberCharUuid.value[12] = msTrackNumberCharId[0]; + msTrackNumberCharUuid.value[13] = msTrackNumberCharId[1]; + msTrackNumberCharUuid.value[14] = msId[0]; + msTrackNumberCharUuid.value[15] = msId[1]; + + msTrackTotalCharUuid.value[12] = msTrackTotalCharId[0]; + msTrackTotalCharUuid.value[13] = msTrackTotalCharId[1]; + msTrackTotalCharUuid.value[14] = msId[0]; + msTrackTotalCharUuid.value[15] = msId[1]; + + msPlaybackSpeedCharUuid.value[12] = msPlaybackSpeedCharId[0]; + msPlaybackSpeedCharUuid.value[13] = msPlaybackSpeedCharId[1]; + msPlaybackSpeedCharUuid.value[14] = msId[0]; + msPlaybackSpeedCharUuid.value[15] = msId[1]; + + msRepeatCharUuid.value[12] = msRepeatCharId[0]; + msRepeatCharUuid.value[13] = msRepeatCharId[1]; + msRepeatCharUuid.value[14] = msId[0]; + msRepeatCharUuid.value[15] = msId[1]; + + msShuffleCharUuid.value[12] = msShuffleCharId[0]; + msShuffleCharUuid.value[13] = msShuffleCharId[1]; + msShuffleCharUuid.value[14] = msId[0]; + msShuffleCharUuid.value[15] = msId[1]; characteristicDefinition[0] = {.uuid = (ble_uuid_t *) (&msEventCharUuid), .access_cb = MSCallback, diff --git a/src/components/ble/MusicService.h b/src/components/ble/MusicService.h index ab8dc2b1..172ab61c 100644 --- a/src/components/ble/MusicService.h +++ b/src/components/ble/MusicService.h @@ -26,8 +26,8 @@ #undef max #undef min -//c7e50000-78fc-48fe-8e23-433b3a1942d0 -#define MUSIC_SERVICE_UUID_BASE {0xd0, 0x42, 0x19, 0x3a, 0x3b, 0x43, 0x23, 0x8e, 0xfe, 0x48, 0xfc, 0x78, 0x00, 0x00, 0xe5, 0xc7} +//00000000-78fc-48fe-8e23-433b3a1942d0 +#define MUSIC_SERVICE_UUID_BASE {0xd0, 0x42, 0x19, 0x3a, 0x3b, 0x43, 0x23, 0x8e, 0xfe, 0x48, 0xfc, 0x78, 0x00, 0x00, 0x00, 0x00} namespace Pinetime { namespace System { @@ -73,19 +73,19 @@ namespace Pinetime { Playing = 0x01 }; private: - static constexpr uint8_t msId[2] = {0x00, 0x01}; - static constexpr uint8_t msEventCharId[2] = {0x00, 0x02}; - static constexpr uint8_t msStatusCharId[2] = {0x00, 0x03}; - static constexpr uint8_t msArtistCharId[2] = {0x00, 0x04}; - static constexpr uint8_t msTrackCharId[2] = {0x00, 0x05}; - static constexpr uint8_t msAlbumCharId[2] = {0x00, 0x06}; - static constexpr uint8_t msPositionCharId[2] = {0x00, 0x07}; - static constexpr uint8_t msTotalLengthCharId[2] = {0x00, 0x08}; - static constexpr uint8_t msTrackNumberCharId[2] = {0x00, 0x09}; - static constexpr uint8_t msTrackTotalCharId[2] = {0x00, 0x0a}; - static constexpr uint8_t msPlaybackSpeedCharId[2] = {0x00, 0x0b}; - static constexpr uint8_t msRepeatCharId[2] = {0x00, 0x0c}; - static constexpr uint8_t msShuffleCharId[2] = {0x00, 0x0d}; + static constexpr uint8_t msId[2] = {0x00, 0x00}; + static constexpr uint8_t msEventCharId[2] = {0x01, 0x00}; + static constexpr uint8_t msStatusCharId[2] = {0x02, 0x00}; + static constexpr uint8_t msArtistCharId[2] = {0x03, 0x00}; + static constexpr uint8_t msTrackCharId[2] = {0x04, 0x00}; + static constexpr uint8_t msAlbumCharId[2] = {0x05, 0x00}; + static constexpr uint8_t msPositionCharId[2] = {0x06, 0x00}; + static constexpr uint8_t msTotalLengthCharId[2] = {0x07, 0x00}; + static constexpr uint8_t msTrackNumberCharId[2] = {0x08, 0x00}; + static constexpr uint8_t msTrackTotalCharId[2] = {0x09, 0x00}; + static constexpr uint8_t msPlaybackSpeedCharId[2] = {0x0a, 0x00}; + static constexpr uint8_t msRepeatCharId[2] = {0x0b, 0x00}; + static constexpr uint8_t msShuffleCharId[2] = {0x0c, 0x00}; ble_uuid128_t msUuid{ .u = {.type = BLE_UUID_TYPE_128}, diff --git a/src/components/ble/NavigationService.cpp b/src/components/ble/NavigationService.cpp new file mode 100644 index 00000000..3c1fd162 --- /dev/null +++ b/src/components/ble/NavigationService.cpp @@ -0,0 +1,137 @@ +/* Copyright (C) 2021 Adam Pigg + + This file is part of InfiniTime. + + InfiniTime is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published + by the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + InfiniTime is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <https://www.gnu.org/licenses/>. +*/ + +#include "NavigationService.h" + +#include "systemtask/SystemTask.h" + +int NAVCallback(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg) { + auto navService = static_cast<Pinetime::Controllers::NavigationService *>(arg); + return navService->OnCommand(conn_handle, attr_handle, ctxt); +} + +Pinetime::Controllers::NavigationService::NavigationService(Pinetime::System::SystemTask &system) : m_system(system) { + navUuid.value[14] = navId[0]; + navUuid.value[15] = navId[1]; + + navFlagCharUuid.value[12] = navFlagCharId[0]; + navFlagCharUuid.value[13] = navFlagCharId[1]; + navFlagCharUuid.value[14] = navId[0]; + navFlagCharUuid.value[15] = navId[1]; + + navNarrativeCharUuid.value[12] = navNarrativeCharId[0]; + navNarrativeCharUuid.value[13] = navNarrativeCharId[1]; + navNarrativeCharUuid.value[14] = navId[0]; + navNarrativeCharUuid.value[15] = navId[1]; + + navManDistCharUuid.value[12] = navManDistCharId[0]; + navManDistCharUuid.value[13] = navManDistCharId[1]; + navManDistCharUuid.value[14] = navId[0]; + navManDistCharUuid.value[15] = navId[1]; + + navProgressCharUuid.value[12] = navProgressCharId[0]; + navProgressCharUuid.value[13] = navProgressCharId[1]; + navProgressCharUuid.value[14] = navId[0]; + navProgressCharUuid.value[15] = navId[1]; + + characteristicDefinition[0] = {.uuid = (ble_uuid_t *) (&navFlagCharUuid), + .access_cb = NAVCallback, + .arg = this, + .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_READ + }; + + characteristicDefinition[1] = {.uuid = (ble_uuid_t *) (&navNarrativeCharUuid), + .access_cb = NAVCallback, + .arg = this, + .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_READ + }; + characteristicDefinition[2] = {.uuid = (ble_uuid_t *) (&navManDistCharUuid), + .access_cb = NAVCallback, + .arg = this, + .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_READ + }; + characteristicDefinition[3] = {.uuid = (ble_uuid_t *) (&navProgressCharUuid), + .access_cb = NAVCallback, + .arg = this, + .flags = BLE_GATT_CHR_F_WRITE | BLE_GATT_CHR_F_READ + }; + + characteristicDefinition[4] = {0}; + + serviceDefinition[0] = { + .type = BLE_GATT_SVC_TYPE_PRIMARY, + .uuid = (ble_uuid_t *) &navUuid, + .characteristics = characteristicDefinition + }; + serviceDefinition[1] = {0}; + + m_progress = 0; +} + +void Pinetime::Controllers::NavigationService::Init() { + int res = 0; + res = ble_gatts_count_cfg(serviceDefinition); + ASSERT(res == 0); + + res = ble_gatts_add_svcs(serviceDefinition); + ASSERT(res == 0); +} + +int Pinetime::Controllers::NavigationService::OnCommand(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt) { + + if (ctxt->op == BLE_GATT_ACCESS_OP_WRITE_CHR) { + size_t notifSize = OS_MBUF_PKTLEN(ctxt->om); + uint8_t data[notifSize + 1]; + data[notifSize] = '\0'; + os_mbuf_copydata(ctxt->om, 0, notifSize, data); + char *s = (char *) &data[0]; + NRF_LOG_INFO("DATA : %s", s); + if (ble_uuid_cmp(ctxt->chr->uuid, (ble_uuid_t *) &navFlagCharUuid) == 0) { + m_flag = s; + } else if (ble_uuid_cmp(ctxt->chr->uuid, (ble_uuid_t *) &navNarrativeCharUuid) == 0) { + m_narrative = s; + } else if (ble_uuid_cmp(ctxt->chr->uuid, (ble_uuid_t *) &navManDistCharUuid) == 0) { + m_manDist = s; + } else if (ble_uuid_cmp(ctxt->chr->uuid, (ble_uuid_t *) &navProgressCharUuid) == 0) { + m_progress = data[0]; + } + } + return 0; +} + +std::string Pinetime::Controllers::NavigationService::getFlag() +{ + return m_flag; +} + +std::string Pinetime::Controllers::NavigationService::getNarrative() +{ + return m_narrative; +} + +std::string Pinetime::Controllers::NavigationService::getManDist() +{ + return m_manDist; +} + +int Pinetime::Controllers::NavigationService::getProgress() +{ + return m_progress; +} + diff --git a/src/components/ble/NavigationService.h b/src/components/ble/NavigationService.h new file mode 100644 index 00000000..29b17582 --- /dev/null +++ b/src/components/ble/NavigationService.h @@ -0,0 +1,96 @@ +/* Copyright (C) 2021 Adam Pigg + + This file is part of InfiniTime. + + InfiniTime is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published + by the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + InfiniTime is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <https://www.gnu.org/licenses/>. +*/ +#pragma once + +#include <cstdint> +#include <string> +#define min // workaround: nimble's min/max macros conflict with libstdc++ +#define max +#include <host/ble_gap.h> +#include <host/ble_uuid.h> +#undef max +#undef min + +//c7e60000-78fc-48fe-8e23-433b3a1942d0 +#define NAVIGATION_SERVICE_UUID_BASE {0xd0, 0x42, 0x19, 0x3a, 0x3b, 0x43, 0x23, 0x8e, 0xfe, 0x48, 0xfc, 0x78, 0x00, 0x00, 0x00, 0x00} + +namespace Pinetime { + namespace System { + class SystemTask; + } + namespace Controllers { + + class NavigationService { + public: + explicit NavigationService(Pinetime::System::SystemTask &system); + + void Init(); + + int OnCommand(uint16_t conn_handle, uint16_t attr_handle, + struct ble_gatt_access_ctxt *ctxt); + + std::string getFlag(); + + std::string getNarrative(); + + std::string getManDist(); + + int getProgress(); + + private: + static constexpr uint8_t navId[2] = {0x01, 0x00}; + static constexpr uint8_t navFlagCharId[2] = {0x01, 0x00}; + static constexpr uint8_t navNarrativeCharId[2] = {0x02, 0x00}; + static constexpr uint8_t navManDistCharId[2] = {0x03, 0x00}; + static constexpr uint8_t navProgressCharId[2] = {0x04, 0x00}; + + ble_uuid128_t navUuid{ + .u = {.type = BLE_UUID_TYPE_128}, + .value = NAVIGATION_SERVICE_UUID_BASE + }; + + ble_uuid128_t navFlagCharUuid{ + .u = {.type = BLE_UUID_TYPE_128}, + .value = NAVIGATION_SERVICE_UUID_BASE + }; + ble_uuid128_t navNarrativeCharUuid{ + .u = {.type = BLE_UUID_TYPE_128}, + .value = NAVIGATION_SERVICE_UUID_BASE + }; + ble_uuid128_t navManDistCharUuid{ + .u = {.type = BLE_UUID_TYPE_128}, + .value = NAVIGATION_SERVICE_UUID_BASE + }; + ble_uuid128_t navProgressCharUuid{ + .u = {.type = BLE_UUID_TYPE_128}, + .value = NAVIGATION_SERVICE_UUID_BASE + }; + + struct ble_gatt_chr_def characteristicDefinition[5]; + struct ble_gatt_svc_def serviceDefinition[2]; + + std::string m_flag; + std::string m_narrative; + std::string m_manDist; + int m_progress; + + Pinetime::System::SystemTask &m_system; + }; + } +} + diff --git a/src/components/ble/NimbleController.cpp b/src/components/ble/NimbleController.cpp index a6f3cc39..f2786ea1 100644 --- a/src/components/ble/NimbleController.cpp +++ b/src/components/ble/NimbleController.cpp @@ -22,7 +22,8 @@ NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask, DateTime& dateTimeController, Pinetime::Controllers::NotificationManager& notificationManager, Controllers::Battery& batteryController, - Pinetime::Drivers::SpiNorFlash& spiNorFlash) : + Pinetime::Drivers::SpiNorFlash& spiNorFlash, + Controllers::HeartRateController& heartRateController) : systemTask{systemTask}, bleController{bleController}, dateTimeController{dateTimeController}, @@ -34,9 +35,11 @@ NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask, alertNotificationClient{systemTask, notificationManager}, currentTimeService{dateTimeController}, musicService{systemTask}, + navService{systemTask}, batteryInformationService{batteryController}, immediateAlertService{systemTask, notificationManager}, - serviceDiscovery({¤tTimeClient, &alertNotificationClient}) { + serviceDiscovery({¤tTimeClient, &alertNotificationClient}), + heartRateService{systemTask, heartRateController} { } int GAPEventCallback(struct ble_gap_event *event, void *arg) { @@ -54,10 +57,12 @@ void NimbleController::Init() { currentTimeClient.Init(); currentTimeService.Init(); musicService.Init(); + navService.Init(); anService.Init(); dfuService.Init(); batteryInformationService.Init(); immediateAlertService.Init(); + heartRateService.Init(); int res; res = ble_hs_util_ensure_addr(0); ASSERT(res == 0); diff --git a/src/components/ble/NimbleController.h b/src/components/ble/NimbleController.h index 914f11e6..a109800c 100644 --- a/src/components/ble/NimbleController.h +++ b/src/components/ble/NimbleController.h @@ -16,7 +16,9 @@ #include "DfuService.h" #include "ImmediateAlertService.h" #include "MusicService.h" +#include "NavigationService.h" #include "ServiceDiscovery.h" +#include "HeartRateService.h" namespace Pinetime { namespace Drivers { @@ -37,7 +39,8 @@ namespace Pinetime { public: NimbleController(Pinetime::System::SystemTask& systemTask, Pinetime::Controllers::Ble& bleController, DateTime& dateTimeController, Pinetime::Controllers::NotificationManager& notificationManager, - Controllers::Battery& batteryController, Pinetime::Drivers::SpiNorFlash& spiNorFlash); + Controllers::Battery& batteryController, Pinetime::Drivers::SpiNorFlash& spiNorFlash, + Controllers::HeartRateController& heartRateController); void Init(); void StartAdvertising(); int OnGAPEvent(ble_gap_event *event); @@ -54,6 +57,7 @@ namespace Pinetime { void StartDiscovery(); Pinetime::Controllers::MusicService& music() {return musicService;}; + Pinetime::Controllers::NavigationService& navigation() {return navService;}; uint16_t connHandle(); @@ -72,8 +76,10 @@ namespace Pinetime { AlertNotificationClient alertNotificationClient; CurrentTimeService currentTimeService; MusicService musicService; + NavigationService navService; BatteryInformationService batteryInformationService; ImmediateAlertService immediateAlertService; + HeartRateService heartRateService; uint8_t addrType; // 1 = Random, 0 = PUBLIC uint16_t connectionHandle = 0; diff --git a/src/components/heartrate/Biquad.cpp b/src/components/heartrate/Biquad.cpp new file mode 100644 index 00000000..6a4b8181 --- /dev/null +++ b/src/components/heartrate/Biquad.cpp @@ -0,0 +1,27 @@ +/* + SPDX-License-Identifier: LGPL-3.0-or-later + Original work Copyright (C) 2020 Daniel Thompson + C++ port Copyright (C) 2021 Jean-François Milants +*/ + +#include "Biquad.h" + +using namespace Pinetime::Controllers; + +/** Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/ppg.py */ +Biquad::Biquad(float b0, float b1, float b2, float a1, float a2) : b0{b0}, b1{b1}, b2{b2}, a1{a1}, a2{a2} { + +} + +float Biquad::Step(float x) { + auto v1 = this->v1; + auto v2 = this->v2; + + auto v = x - (a1 * v1) - (a2 * v2); + auto y = (b0 * v) + (b1 * v1) + (b2 * v2); + + this->v2 = v1; + this->v1 = v; + + return y; +} diff --git a/src/components/heartrate/Biquad.h b/src/components/heartrate/Biquad.h new file mode 100644 index 00000000..dc9b97f6 --- /dev/null +++ b/src/components/heartrate/Biquad.h @@ -0,0 +1,22 @@ +#pragma once + +namespace Pinetime { + namespace Controllers { + /// Direct Form II Biquad Filter + class Biquad { + public: + Biquad(float b0, float b1, float b2, float a1, float a2); + float Step(float x); + + private: + float b0; + float b1; + float b2; + float a1; + float a2; + + float v1 = 0.0f; + float v2 = 0.0f; + }; + } +} diff --git a/src/components/heartrate/HeartRateController.cpp b/src/components/heartrate/HeartRateController.cpp new file mode 100644 index 00000000..d0b0d513 --- /dev/null +++ b/src/components/heartrate/HeartRateController.cpp @@ -0,0 +1,41 @@ +#include "HeartRateController.h" +#include <heartratetask/HeartRateTask.h> +#include <systemtask/SystemTask.h> + +using namespace Pinetime::Controllers; + +HeartRateController::HeartRateController(Pinetime::System::SystemTask &systemTask) : systemTask{systemTask} { + +} + + +void HeartRateController::Update(HeartRateController::States newState, uint8_t heartRate) { + this->state = newState; + if(this->heartRate != heartRate) { + this->heartRate = heartRate; + service->OnNewHeartRateValue(heartRate); + } +} + +void HeartRateController::Start() { + if(task != nullptr) { + state = States::NotEnoughData; + task->PushMessage(Pinetime::Applications::HeartRateTask::Messages::StartMeasurement); + } +} + +void HeartRateController::Stop() { + if(task != nullptr) { + state = States::Stopped; + task->PushMessage(Pinetime::Applications::HeartRateTask::Messages::StopMeasurement); + } +} + +void HeartRateController::SetHeartRateTask(Pinetime::Applications::HeartRateTask *task) { + this->task = task; +} + +void HeartRateController::SetService(Pinetime::Controllers::HeartRateService *service) { + this->service = service; +} + diff --git a/src/components/heartrate/HeartRateController.h b/src/components/heartrate/HeartRateController.h new file mode 100644 index 00000000..001111b5 --- /dev/null +++ b/src/components/heartrate/HeartRateController.h @@ -0,0 +1,38 @@ +#pragma once + +#include <cstdint> +#include <components/ble/HeartRateService.h> + +namespace Pinetime { + namespace Applications { + class HeartRateTask; + } + namespace System { + class SystemTask; + } + namespace Controllers { + class HeartRateController { + public: + enum class States { Stopped, NotEnoughData, NoTouch, Running}; + + explicit HeartRateController(System::SystemTask& systemTask); + + void Start(); + void Stop(); + void Update(States newState, uint8_t heartRate); + + void SetHeartRateTask(Applications::HeartRateTask* task); + States State() const { return state; } + uint8_t HeartRate() const { return heartRate; } + + void SetService(Pinetime::Controllers::HeartRateService *service); + + private: + System::SystemTask& systemTask; + Applications::HeartRateTask* task = nullptr; + States state = States::Stopped; + uint8_t heartRate = 0; + Pinetime::Controllers::HeartRateService* service = nullptr; + }; + } +}
\ No newline at end of file diff --git a/src/components/heartrate/Ppg.cpp b/src/components/heartrate/Ppg.cpp new file mode 100644 index 00000000..233c3003 --- /dev/null +++ b/src/components/heartrate/Ppg.cpp @@ -0,0 +1,106 @@ +/* + SPDX-License-Identifier: LGPL-3.0-or-later + Original work Copyright (C) 2020 Daniel Thompson + C++ port Copyright (C) 2021 Jean-François Milants +*/ + +#include <vector> +#include <nrf_log.h> +#include "Ppg.h" +using namespace Pinetime::Controllers; + +/** Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/ppg.py */ +namespace { + int Compare(int* d1, int* d2, size_t count) { + int e = 0; + for(int i = 0; i < count; i++) { + auto d = d1[i] - d2[i]; + e += d * d; + } + return e; + } + + int CompareShift(int* d, int shift, size_t count) { + return Compare(d +shift, d, count - shift); + } + + int Trough(int* d, size_t size, float mn, float mx) { + auto z2 = CompareShift(d, mn-2, size); + auto z1 = CompareShift(d, mn-1, size); + for(int i = mn; i < mx + 1; i++) { + auto z = CompareShift(d, i, size); + if(z2 > z1 && z1 < z) + return i; + z2 = z1; + z1 = z; + } + return -1; + } +} + +Ppg::Ppg(float spl) : offset{spl}, + hpf{0.87033078, -1.74066156, 0.87033078,-1.72377617, 0.75754694}, + agc{20, 0.971, 2}, + lpf{0.11595249, 0.23190498, 0.11595249,-0.72168143, 0.18549138} { + +} + +int Ppg::Preprocess(float spl) { + spl -= offset; + spl = hpf.Step(spl); + spl = agc.Step(spl); + spl = lpf.Step(spl); + + auto spl_int = static_cast<int>(spl); + + if(dataIndex < 200) + data[dataIndex++] = spl_int; + return spl_int; +} + +float Ppg::HeartRate() { + if(dataIndex < 200) + return 0; + + NRF_LOG_INFO("PREPROCESS, offset = %d", offset); + auto hr = ProcessHeartRate(); + dataIndex = 0; + return hr; +} + +int cccount = 0; +float Ppg::ProcessHeartRate() { + + if(cccount > 2) + asm("nop"); + cccount ++; + auto t0 = Trough(data.data(), dataIndex, 7, 48); + if(t0 < 0) + return 0; + + float t1 = t0 * 2; + t1 = Trough(data.data(), dataIndex, t1-5, t1+5); + if(t1 < 0) + return 0; + + float t2 = static_cast<int>(t1 * 3) / 2; + t2 = Trough(data.data(), dataIndex, t2 - 5, t2 + 5); + if(t2 < 0) + return 0; + + float t3 = static_cast<int>(t2 * 4) / 3; + t3 = Trough(data.data(), dataIndex, t3 - 4, t3 + 4); + if(t3 < 0) + return static_cast<int>(60 * 24 * 3) / static_cast<int>(t2); + + return static_cast<int>(60 * 24 * 4) / static_cast<int>(t3); +} + +void Ppg::SetOffset(uint16_t offset) { + this->offset = offset; + dataIndex = 0; +} + +void Ppg::Reset() { + dataIndex = 0; +} diff --git a/src/components/heartrate/Ppg.h b/src/components/heartrate/Ppg.h new file mode 100644 index 00000000..747ae019 --- /dev/null +++ b/src/components/heartrate/Ppg.h @@ -0,0 +1,31 @@ +#pragma once + +#include <array> +#include "Biquad.h" +#include "Ptagc.h" + +namespace Pinetime { + namespace Controllers { + class Ppg { + public: + explicit Ppg(float spl); + + int Preprocess(float spl); + float HeartRate(); + + void SetOffset(uint16_t i); + void Reset(); + + private: + std::array<int, 200> data; + size_t dataIndex = 0; + float offset; + Biquad hpf; + Ptagc agc; + Biquad lpf; + + + float ProcessHeartRate(); + }; + } +}
\ No newline at end of file diff --git a/src/components/heartrate/Ptagc.cpp b/src/components/heartrate/Ptagc.cpp new file mode 100644 index 00000000..dd7c4411 --- /dev/null +++ b/src/components/heartrate/Ptagc.cpp @@ -0,0 +1,28 @@ +/* + SPDX-License-Identifier: LGPL-3.0-or-later + Original work Copyright (C) 2020 Daniel Thompson + C++ port Copyright (C) 2021 Jean-François Milants +*/ + +#include <cmath> +#include "Ptagc.h" + +using namespace Pinetime::Controllers; + +/** Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/ppg.py */ +Ptagc::Ptagc(float start, float decay, float threshold) : peak{start}, decay{decay}, boost{1.0f/decay}, threshold{threshold} { + +} + +float Ptagc::Step(float spl) { + if(std::abs(spl) > peak) + peak *= boost; + else + peak *= decay; + + if((spl > (peak * threshold)) || (spl < (peak * -threshold))) + return 0.0f; + + spl = 100.0f * spl / (2.0f * peak); + return spl; +} diff --git a/src/components/heartrate/Ptagc.h b/src/components/heartrate/Ptagc.h new file mode 100644 index 00000000..c20de4c0 --- /dev/null +++ b/src/components/heartrate/Ptagc.h @@ -0,0 +1,18 @@ +#pragma once + +namespace Pinetime { + namespace Controllers { + class Ptagc { + public: + Ptagc(float start, float decay, float threshold); + float Step(float spl); + + private: + float peak; + float decay; + float boost; + float threshold; + + }; + } +} |