mirror of
https://github.com/GoldenCheetah/GoldenCheetah.git
synced 2026-02-15 00:49:55 +00:00
401 lines
14 KiB
C++
401 lines
14 KiB
C++
//
|
|
// SmartControl.c
|
|
//
|
|
// Copyright © 2017 Kinetic. All rights reserved.
|
|
//
|
|
// Integrated into golden cheetah by Eric Christoffersen 2020 (impolexg@outlook.com)
|
|
|
|
#include "KurtSmartControl.h"
|
|
|
|
#include <random>
|
|
#include <mutex>
|
|
|
|
#include <QRandomGenerator>
|
|
|
|
#include "CalibrationData.h"
|
|
|
|
#define SensorHz 10000
|
|
|
|
uint8_t smart_control_state_to_calibration_state(smart_control_calibration_state calibration_state) {
|
|
|
|
switch(calibration_state) {
|
|
case SMART_CONTROL_CALIBRATION_STATE_NOT_PERFORMED:
|
|
return CALIBRATION_STATE_FAILURE;
|
|
|
|
case SMART_CONTROL_CALIBRATION_STATE_INITIALIZING:
|
|
return CALIBRATION_STATE_STARTING;
|
|
|
|
case SMART_CONTROL_CALIBRATION_STATE_SPEED_UP:
|
|
return CALIBRATION_STATE_POWER;
|
|
|
|
case SMART_CONTROL_CALIBRATION_STATE_START_COASTING:
|
|
case SMART_CONTROL_CALIBRATION_STATE_COASTING:
|
|
return CALIBRATION_STATE_COAST;
|
|
|
|
case SMART_CONTROL_CALIBRATION_STATE_SPEED_UP_DETECTED:
|
|
return CALIBRATION_STATE_POWER;
|
|
|
|
case SMART_CONTROL_CALIBRATION_STATE_COMPLETE:
|
|
return CALIBRATION_STATE_SUCCESS;
|
|
}
|
|
|
|
return CALIBRATION_STATE_FAILURE;
|
|
}
|
|
|
|
typedef enum smart_control_command
|
|
{
|
|
SMART_CONTROL_COMMAND_SET_PERFORMANCE = 0x00,
|
|
SMART_CONTROL_COMMAND_SPINDOWN_CALIBRATION = 0x03
|
|
} smart_control_command;
|
|
|
|
uint8_t hash8WithSeed(uint8_t hash, const uint8_t *buffer, uint8_t length)
|
|
{
|
|
const uint8_t crc8_table[256] = {
|
|
0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
|
|
0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
|
|
0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
|
|
0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,
|
|
0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
|
|
0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
|
|
0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
|
|
0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,
|
|
0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
|
|
0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
|
|
0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
|
|
0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,
|
|
0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
|
|
0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
|
|
0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
|
|
0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,
|
|
0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
|
|
0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
|
|
0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
|
|
0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,
|
|
0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
|
|
0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
|
|
0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
|
|
0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,
|
|
0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
|
|
0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
|
|
0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
|
|
0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,
|
|
0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
|
|
0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
|
|
0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
|
|
0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf
|
|
};
|
|
|
|
for (uint8_t byte_index = 0; byte_index < length; byte_index++) {
|
|
hash = crc8_table[hash ^ buffer[byte_index]];
|
|
}
|
|
return hash;
|
|
}
|
|
|
|
double smart_control_speed_for_ticks(uint16_t ticks)
|
|
{
|
|
if (ticks == 0 || ticks == 65535) {
|
|
return 0;
|
|
}
|
|
return (6107.2561186) / ((double)ticks);
|
|
}
|
|
|
|
double smart_control_ticks_to_seconds(uint32_t ticks)
|
|
{
|
|
return (double)ticks / (double)SensorHz;
|
|
}
|
|
|
|
smart_control_power_data smart_control_process_power_data(const uint8_t *data, size_t size)
|
|
{
|
|
smart_control_power_data powerData;
|
|
|
|
if (size >= 14 && size <= 20) {
|
|
|
|
uint8_t hashSeed = 0x42;
|
|
uint8_t inData[20];
|
|
for (size_t i = 0; i < size; ++i) {
|
|
inData[i] = data[i];
|
|
}
|
|
uint8_t hash = hash8WithSeed(hashSeed, &inData[size - 1], 1);
|
|
for (unsigned index = 0; index < size - 1; index++) {
|
|
inData[index] ^= hash;
|
|
hash = hash8WithSeed(hash, &inData[index], 1);
|
|
}
|
|
|
|
powerData.mode = (smart_control_mode)inData[0];
|
|
powerData.targetResistance = ((uint16_t)inData[1] << 8) | (uint16_t)inData[2];
|
|
powerData.power = ((uint16_t)inData[3] << 8) | (uint16_t)inData[4];
|
|
powerData.cadenceRPM = inData[12];
|
|
|
|
if (size >= 18) {
|
|
uint32_t metersPerHour = ((uint32_t)inData[13] << 24) | ((uint32_t)inData[14] << 16) | ((uint32_t)inData[15] << 8) | (uint32_t)inData[16];
|
|
powerData.speedKPH = metersPerHour / 1000.0;
|
|
} else {
|
|
uint16_t rollerTicks = ((uint16_t)inData[5] << 8) | (uint16_t)inData[6];
|
|
powerData.speedKPH = smart_control_speed_for_ticks(rollerTicks);
|
|
}
|
|
} else {
|
|
powerData.mode = SMART_CONTROL_MODE_ERG;
|
|
powerData.targetResistance = 0;
|
|
powerData.cadenceRPM = 0;
|
|
powerData.power = 0;
|
|
powerData.speedKPH = 0;
|
|
}
|
|
|
|
return powerData;
|
|
}
|
|
|
|
smart_control_config_data smart_control_process_config_data(const uint8_t *data, size_t size)
|
|
{
|
|
uint8_t hashSeed = 0x42;
|
|
uint8_t* inData = (uint8_t*)malloc(size);
|
|
for (size_t i = 0; i < size; ++i) {
|
|
inData[i] = data[i];
|
|
}
|
|
uint8_t hash = hash8WithSeed(hashSeed, &inData[size - 1], 1);
|
|
for (unsigned index = 0; index < size - 1; index++) {
|
|
inData[index] ^= hash;
|
|
hash = hash8WithSeed(hash, &inData[index], 1);
|
|
}
|
|
|
|
smart_control_config_data configData;
|
|
|
|
if (size >= 5) {
|
|
|
|
configData.updateRate = inData[0];
|
|
configData.tickRate = ((uint32_t)inData[1] << 16) | ((uint32_t)inData[2] << 8) | (uint32_t)inData[3];
|
|
configData.firmwareUpdateState = inData[4];
|
|
|
|
if (size >= 13) {
|
|
configData.systemStatus = ((uint16_t)inData[5] << 8) | (uint16_t)inData[6];
|
|
configData.calibrationState = (smart_control_calibration_state)inData[7];
|
|
uint32_t spindownTicks = ((uint32_t)inData[8] << 24) | ((uint32_t)inData[9] << 16) | ((uint32_t)inData[10] << 8) | (uint32_t)inData[11];
|
|
configData.spindownTime = smart_control_ticks_to_seconds(spindownTicks);
|
|
}
|
|
if (size >= 15) {
|
|
uint16_t metersPerHour = ((uint16_t)inData[12] << 8) | (uint16_t)inData[13];
|
|
configData.calibrationThresholdKPH = metersPerHour / 1000.0;
|
|
} else {
|
|
configData.calibrationThresholdKPH = 33.8;
|
|
}
|
|
if (size >= 18) {
|
|
uint16_t metersPerHour = ((uint16_t)inData[14] << 8) | (uint16_t)inData[15];
|
|
configData.brakeCalibrationThresholdKPH = metersPerHour / 1000.0;
|
|
configData.brakeStrength = inData[16];
|
|
} else {
|
|
configData.brakeCalibrationThresholdKPH = 45;
|
|
configData.brakeStrength = 55;
|
|
}
|
|
if (size >= 19) {
|
|
configData.brakeOffset = inData[17];
|
|
} else {
|
|
configData.brakeOffset = 128;
|
|
}
|
|
if (size >= 20) {
|
|
configData.noiseFilter = inData[18];
|
|
} else {
|
|
configData.noiseFilter = 1;
|
|
}
|
|
} else {
|
|
configData.updateRate = 1;
|
|
configData.tickRate = 10000;
|
|
configData.firmwareUpdateState = 0;
|
|
configData.systemStatus = 0;
|
|
configData.calibrationState = SMART_CONTROL_CALIBRATION_STATE_NOT_PERFORMED;
|
|
configData.spindownTime = 0;
|
|
configData.calibrationThresholdKPH = 33.8;
|
|
configData.brakeCalibrationThresholdKPH = 45;
|
|
configData.brakeStrength = 55;
|
|
configData.brakeOffset = 128;
|
|
configData.noiseFilter = 1;
|
|
}
|
|
|
|
free(inData);
|
|
|
|
return configData;
|
|
}
|
|
|
|
|
|
|
|
#ifndef MIN
|
|
#pragma GCC diagnostic ignored "-Wtype-limits"
|
|
#define MIN(a,b) (((a)<(b))?(a):(b))
|
|
#define MAX(a,b) (((a)>(b))?(a):(b))
|
|
#endif
|
|
|
|
// Helper that generates random byte in range low to high
|
|
uint8_t RandomByteInRange(unsigned low, unsigned high) {
|
|
return (uint8_t)QRandomGenerator::global()->bounded(low, high);
|
|
}
|
|
|
|
QByteArray smart_control_set_mode_erg_command(uint16_t targetWatts)
|
|
{
|
|
smart_control_set_mode_erg_data data;
|
|
|
|
uint16_t clamped = MAX(0, MIN(0xFFFF, targetWatts));
|
|
|
|
data.bytes[0] = SMART_CONTROL_COMMAND_SET_PERFORMANCE;
|
|
data.bytes[1] = SMART_CONTROL_MODE_ERG;
|
|
data.bytes[2] = (uint8_t)((uint16_t)clamped >> 8);
|
|
data.bytes[3] = (uint8_t)((uint16_t)clamped);
|
|
data.bytes[4] = RandomByteInRange(0, 0x100 - 1); // distrib(gen); // nonce arc4random_uniform(0x100); // nonce
|
|
uint8_t dataLength = 5;
|
|
|
|
// Encode Packet
|
|
uint8_t hashSeed = 0x42;
|
|
uint8_t hash = hash8WithSeed(hashSeed, &data.bytes[dataLength - 1], 1);
|
|
for (uint8_t index = 0; index < dataLength - 1; index++) {
|
|
uint8_t temp = data.bytes[index];
|
|
data.bytes[index] ^= hash;
|
|
hash = hash8WithSeed(hash, &temp, 1);
|
|
}
|
|
|
|
return QByteArray((const char*)&data, sizeof(data));
|
|
}
|
|
|
|
|
|
|
|
QByteArray smart_control_set_mode_fluid_command(uint8_t level)
|
|
{
|
|
smart_control_set_mode_fluid_data data;
|
|
|
|
uint8_t clamped = MAX(0, MIN(9, level));
|
|
|
|
data.bytes[0] = SMART_CONTROL_COMMAND_SET_PERFORMANCE;
|
|
data.bytes[1] = SMART_CONTROL_MODE_FLUID;
|
|
data.bytes[2] = clamped;
|
|
data.bytes[3] = RandomByteInRange(0, 0x100 - 1); //arc4random_uniform(0x100); // nonce
|
|
uint8_t dataLength = 4;
|
|
|
|
// Encode Packet
|
|
uint8_t hashSeed = 0x42;
|
|
uint8_t hash = hash8WithSeed(hashSeed, &data.bytes[dataLength - 1], 1);
|
|
for (uint8_t index = 0; index < dataLength - 1; index++) {
|
|
uint8_t temp = data.bytes[index];
|
|
data.bytes[index] ^= hash;
|
|
hash = hash8WithSeed(hash, &temp, 1);
|
|
}
|
|
|
|
return QByteArray((const char*)&data, sizeof(data));
|
|
}
|
|
|
|
QByteArray smart_control_set_mode_brake_command(float percent)
|
|
{
|
|
smart_control_set_mode_brake_data data;
|
|
|
|
// normalize to 0-65535
|
|
float clamped = MAX(0, MIN(1, percent));
|
|
uint16_t normalized = (uint16_t) round(65535 * clamped);
|
|
|
|
data.bytes[0] = SMART_CONTROL_COMMAND_SET_PERFORMANCE;
|
|
data.bytes[1] = SMART_CONTROL_MODE_BRAKE;
|
|
data.bytes[2] = normalized >> 8;
|
|
data.bytes[3] = normalized;
|
|
data.bytes[4] = RandomByteInRange(0, 0x100 - 1); //arc4random_uniform(0x100); // nonce
|
|
uint8_t dataLength = 5;
|
|
|
|
// Encode Packet
|
|
uint8_t hashSeed = 0x42;
|
|
uint8_t hash = hash8WithSeed(hashSeed, &data.bytes[dataLength - 1], 1);
|
|
for (uint8_t index = 0; index < dataLength - 1; index++) {
|
|
uint8_t temp = data.bytes[index];
|
|
data.bytes[index] ^= hash;
|
|
hash = hash8WithSeed(hash, &temp, 1);
|
|
}
|
|
|
|
return QByteArray((const char*)&data, sizeof(data));
|
|
}
|
|
|
|
QByteArray smart_control_set_mode_simulation_command(float weightKG, float rollingCoeff, float windCoeff, float grade, float windSpeedMPS)
|
|
{
|
|
smart_control_set_mode_simulation_data data;
|
|
|
|
data.bytes[0] = SMART_CONTROL_COMMAND_SET_PERFORMANCE;
|
|
data.bytes[1] = SMART_CONTROL_MODE_SIMULATION;
|
|
|
|
// weight is in KGs ... multiply by 100 to get 2 points of precision
|
|
uint16_t weight100 = (uint16_t) roundf(MIN(655.36, weightKG) * 100);
|
|
data.bytes[2] = weight100 >> 8;
|
|
data.bytes[3] = weight100;
|
|
|
|
// Rolling coeff is < 1. multiply by 10,000 to get 5 points of precision
|
|
// coeff cannot be larger than 6.5536 otherwise it rolls over ...
|
|
uint16_t rr10000 = (uint16_t) roundf(MIN(6.5536, rollingCoeff) * 10000);
|
|
data.bytes[4] = rr10000 >> 8;
|
|
data.bytes[5] = rr10000;
|
|
|
|
// Wind coeff is typically < 1. multiply by 10,000 to get 5 points of precision
|
|
// coeff cannot be larger than 6.5536 otherwise it rolls over ...
|
|
uint16_t wr10000 = (uint16_t) roundf(MIN(6.5536, windCoeff) * 10000);
|
|
data.bytes[6] = wr10000 >> 8;
|
|
data.bytes[7] = wr10000;
|
|
|
|
// Grade is between -45.0 and 45.0
|
|
// Mulitply by 100 to get 2 points of precision
|
|
int16_t grade100 = (int16_t) roundf(MAX(-45, MIN(45, grade)) * 100);
|
|
data.bytes[8] = grade100 >> 8;
|
|
data.bytes[9] = grade100;
|
|
|
|
// windspeed is in meters / second. convert to CM / second
|
|
int16_t windSpeedCM = (int16_t) roundf(windSpeedMPS * 100);
|
|
data.bytes[10] = windSpeedCM >> 8;
|
|
data.bytes[11] = windSpeedCM;
|
|
|
|
data.bytes[12] = RandomByteInRange(0, 0x100 - 1); //arc4random_uniform(0x100); // nonce
|
|
uint8_t dataLength = 13;
|
|
|
|
// Encode Packet
|
|
uint8_t hashSeed = 0x42;
|
|
uint8_t hash = hash8WithSeed(hashSeed, &data.bytes[dataLength - 1], 1);
|
|
for (uint8_t index = 0; index < dataLength - 1; index++) {
|
|
uint8_t temp = data.bytes[index];
|
|
data.bytes[index] ^= hash;
|
|
hash = hash8WithSeed(hash, &temp, 1);
|
|
}
|
|
|
|
return QByteArray((const char*)&data, sizeof(data));
|
|
}
|
|
|
|
QByteArray smart_control_start_calibration_command(bool brakeCalibration)
|
|
{
|
|
smart_control_calibration_command_data data;
|
|
|
|
data.bytes[0] = SMART_CONTROL_COMMAND_SPINDOWN_CALIBRATION;
|
|
data.bytes[1] = 0x01;
|
|
data.bytes[2] = brakeCalibration ? 0x01 : 0x00;
|
|
data.bytes[3] = RandomByteInRange(0, 0x100 - 1); //arc4random_uniform(0x100); // nonce
|
|
uint8_t dataLength = 4;
|
|
|
|
// Encode Packet
|
|
uint8_t hashSeed = 0x42;
|
|
uint8_t hash = hash8WithSeed(hashSeed, &data.bytes[dataLength - 1], 1);
|
|
for (uint8_t index = 0; index < dataLength - 1; index++) {
|
|
uint8_t temp = data.bytes[index];
|
|
data.bytes[index] ^= hash;
|
|
hash = hash8WithSeed(hash, &temp, 1);
|
|
}
|
|
|
|
return QByteArray((const char*)&data, sizeof(data));
|
|
}
|
|
|
|
QByteArray smart_control_stop_calibration_command()
|
|
{
|
|
smart_control_calibration_command_data data;
|
|
|
|
data.bytes[0] = SMART_CONTROL_COMMAND_SPINDOWN_CALIBRATION;
|
|
data.bytes[1] = 0x00;
|
|
data.bytes[2] = 0x00;
|
|
data.bytes[3] = RandomByteInRange(0, 0x100 - 1); //arc4random_uniform(0x100); // nonce
|
|
uint8_t dataLength = 4;
|
|
|
|
// Encode Packet
|
|
uint8_t hashSeed = 0x42;
|
|
uint8_t hash = hash8WithSeed(hashSeed, &data.bytes[dataLength - 1], 1);
|
|
for (uint8_t index = 0; index < dataLength - 1; index++) {
|
|
uint8_t temp = data.bytes[index];
|
|
data.bytes[index] ^= hash;
|
|
hash = hash8WithSeed(hash, &temp, 1);
|
|
}
|
|
|
|
return QByteArray((const char*)&data, sizeof(data));
|
|
}
|