SmartSpin2k/src/SensorCollector.cpp at 48914a758881ebfde8879ae4b46801ef7f6403a5 · doudar/SmartSpin2k · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
/*
 * Copyright (C) 2020  Anthony Doud & Joel Baranick
 * All rights reserved
 *
 * SPDX-License-Identifier: GPL-2.0-only
 */

#include "Main.h"
#include "SS2KLog.h"
#include "Constants.h"
#include "BLE_Common.h"

#include <sensors/SensorData.h>
#include <sensors/SensorDataFactory.h>

SensorDataFactory sensorDataFactory;

void collectAndSet(NimBLEUUID charUUID, NimBLEUUID serviceUUID, std::string& uniqueName, uint8_t* pData, size_t length) {
  // Update the timestamp for disconnect detection
  for (size_t i = 0; i < NUM_BLE_DEVICES; i++) {
    if (spinBLEClient.myBLEDevices[i].uniqueName == uniqueName) {
      spinBLEClient.myBLEDevices[i].lastDataUpdateTime = millis();
      break;
    }
  }

  const int kLogBufMaxLength = 250;
  char logBuf[kLogBufMaxLength];
  SS2K_LOGD(BLE_COMMON_LOG_TAG, "Data length: %d", length);
  int logBufLength = ss2k_log_hex_to_buffer(pData, length, logBuf, 0, kLogBufMaxLength);

  logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, "<- %.8s | %.8s", serviceUUID.toString().c_str(), charUUID.toString().c_str());

  std::shared_ptr<SensorData> sensorData = sensorDataFactory.getSensorData(charUUID, uniqueName, pData, length);

  logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " | %s[", sensorData->getId().c_str());
  if (sensorData->hasHeartRate() && !rtConfig->hr.getSimulate()) {
    int heartRate        = sensorData->getHeartRate();
    static int zeroCount = 0;
    zeroCount++;
    if (heartRate > 0) {
      rtConfig->hr.setValue(heartRate);
      logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " HR(%d)", heartRate % 1000);
      spinBLEClient.connectedHRM = true;
      zeroCount                  = 0;
    } else {
      // require 10 readings in a row before setting the HR to 0
      logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " HR IGNORED");
      if (zeroCount > 10) {
        rtConfig->hr.setValue(0);
        spinBLEClient.connectedHRM = false;
        zeroCount                  = 0;
      }
    }
  }

  if (sensorData->hasCadence() && !rtConfig->cad.getSimulate()) {
    if ((charUUID == PELOTON_DATA_UUID) && !(strcmp(userConfig->getConnectedPowerMeter(), NONE) == 0 || strcmp(userConfig->getConnectedPowerMeter(), ANY) == 0)) {
      // Peloton connected but using BLE Power Meter. So skip cad for Peloton UUID.
    } else {
      int cadence = round(sensorData->getCadence());
      if (cadence > 0.0 && cadence < 250.0) {
        rtConfig->cad.setValue(cadence);
        spinBLEClient.connectedCD = true;
        logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " CD(%.2f)", fmodf(cadence, 1000.0));
      } else {
        rtConfig->cad.setValue(0);
        // log cadence ignored
        logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " CD IGNORED");
      }
    }
  }

  if (sensorData->hasPower() && !rtConfig->watts.getSimulate() && !userConfig->getPTab4Pwr()) {
    if ((charUUID == PELOTON_DATA_UUID) && !((strcmp(userConfig->getConnectedPowerMeter(), NONE) == 0) || (strcmp(userConfig->getConnectedPowerMeter(), ANY) == 0))) {
      // Peloton connected but using BLE Power Meter. So skip power for Peloton UUID.
    } else {
      int power = round(sensorData->getPower() * userConfig->getPowerCorrectionFactor());
      if (power > 0 && power < 3000) {
        rtConfig->watts.setValue(power);
        spinBLEClient.connectedPM = true;
        logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " PW(%d)", power % 10000);
      } else {
        rtConfig->watts.setValue(0);
        logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " PW IGNORED");
      }
    }
  }

  if (sensorData->hasSpeed()) {
    rtConfig->setSimulatedSpeed(sensorData->getSpeed());
    spinBLEClient.connectedSpeed = true;
    logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " SD(%.2f)", fmodf(sensorData->getSpeed(), 1000.0));
  }

  if (sensorData->hasResistance()) {
    rtConfig->resistance.setSimulate(false);  // Mark as real data
    if ((ss2k->pelotonIsConnected) && (charUUID != PELOTON_DATA_UUID)) {
      // Peloton connected but using BLE Power Meter. So skip resistance for UUID's that aren't Peloton.
    } else {
      rtConfig->resistance.setValue(sensorData->getResistance());
      logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " RS(%d)", sensorData->getResistance() % 1000);
    }
  }

  // adding incline so that i can plot it
  logBufLength += snprintf(logBuf + logBufLength, kLogBufMaxLength - logBufLength, " POS(%d)", ss2k->getCurrentPosition());
  strncat(logBuf + logBufLength, " ]", kLogBufMaxLength - logBufLength);

// Peloton data screams, so only log one per second.
#ifdef DEBUG_BLE_TX_RX
  static long int lastTime = millis();
  if ((charUUID == PELOTON_DATA_UUID) && (millis() - lastTime < 1000)) return;

  SS2K_LOG(BLE_COMMON_LOG_TAG, "%s", logBuf);

  if (charUUID == PELOTON_DATA_UUID) lastTime = millis();
#endif
}