From 0d3a8c911a44b0da0dbfc2fd514c96fc72c970d0 Mon Sep 17 00:00:00 2001 From: mattipee Date: Thu, 21 Jan 2021 12:17:18 +0000 Subject: [PATCH] Fortius calibration (#3790) Added calibration trainer command, delayed check for user stopped pedaling with visual feedback on progress and saved calibration for next use on success. --- src/Core/Settings.h | 1 + src/Train/CalibrationData.h | 1 + src/Train/Fortius.cpp | 52 ++++++++-- src/Train/Fortius.h | 12 ++- src/Train/FortiusController.cpp | 179 +++++++++++++++++++++++++++++++- src/Train/FortiusController.h | 67 ++++++++++++ src/Train/TrainSidebar.cpp | 56 ++++++++++ 7 files changed, 356 insertions(+), 12 deletions(-) diff --git a/src/Core/Settings.h b/src/Core/Settings.h index 5d495ec7f..29f008cb8 100644 --- a/src/Core/Settings.h +++ b/src/Core/Settings.h @@ -227,6 +227,7 @@ #define GC_DEV_VIRTUAL "devicepostProcess" #define GC_DEV_VIRTUALPOWER "devicevirtualPower" #define FORTIUS_FIRMWARE "fortius/firmware" +#define FORTIUS_CALIBRATION "fortius/calibration" #define IMAGIC_FIRMWARE "imagic/firmware" #define TRAIN_MULTI "train/multi" #define TRAIN_AUTOCONNECT "train/autoconnect" diff --git a/src/Train/CalibrationData.h b/src/Train/CalibrationData.h index bff80b5cb..4a4fdbbe7 100644 --- a/src/Train/CalibrationData.h +++ b/src/Train/CalibrationData.h @@ -26,6 +26,7 @@ #define CALIBRATION_TYPE_ZERO_OFFSET 0x02 #define CALIBRATION_TYPE_ZERO_OFFSET_SRM 0x04 #define CALIBRATION_TYPE_SPINDOWN 0x08 +#define CALIBRATION_TYPE_FORTIUS 0x10 #define CALIBRATION_STATE_IDLE 0x00 #define CALIBRATION_STATE_PENDING 0x01 diff --git a/src/Train/Fortius.cpp b/src/Train/Fortius.cpp index 7c2738cdd..818bee219 100644 --- a/src/Train/Fortius.cpp +++ b/src/Train/Fortius.cpp @@ -17,6 +17,7 @@ */ #include "Fortius.h" +#include "Settings.h" // // Outbound control message has the format: @@ -62,7 +63,8 @@ Fortius::Fortius(QObject *parent) : QThread(parent) windSpeed = DEFAULT_WINDSPEED; rollingResistance = DEFAULT_Crr; windResistance = DEFAULT_CdA; - brakeCalibrationFactor = DEFAULT_CALIBRATION; + brakeCalibrationForce_N = appsettings->value(this, FORTIUS_CALIBRATION, DEFAULT_CALIBRATION_FORCE_N).toDouble(); + brakeCalibrationFactor = DEFAULT_CALIBRATION_FACTOR; powerScaleFactor = DEFAULT_SCALING; deviceStatus=0; this->parent = parent; @@ -95,6 +97,18 @@ void Fortius::setMode(int mode) pvars.unlock(); } +// Compensate for trainer friction (at 20 kph) +void Fortius::setBrakeCalibrationForce(double force_N) +{ + // persist calibration value in global settings + appsettings->setValue(FORTIUS_CALIBRATION, force_N); + + // update variable used to construct trainer commands + pvars.lock(); + brakeCalibrationForce_N = force_N; + pvars.unlock(); +} + // Alters the relationship between brake setpoint at load. void Fortius::setBrakeCalibrationFactor(double brakeCalibrationFactor) { @@ -176,11 +190,12 @@ void Fortius::setWindResistance(double windResistance) /* ---------------------------------------------------------------------- * GET * ---------------------------------------------------------------------- */ -void Fortius::getTelemetry(double &power, double &heartrate, double &cadence, double &speed, double &distance, int &buttons, int &steering, int &status) +void Fortius::getTelemetry(double &power, double &force, double &heartrate, double &cadence, double &speed, double &distance, int &buttons, int &steering, int &status) { pvars.lock(); power = devicePower; + force = deviceForce_N; heartrate = deviceHeartRate; cadence = deviceCadence; speed = deviceSpeed; @@ -232,6 +247,15 @@ double Fortius::getWeight() return tmp; } +double Fortius::getBrakeCalibrationForce() const +{ + double tmp; + pvars.lock(); + tmp = brakeCalibrationForce_N; + pvars.unlock(); + return tmp; +} + double Fortius::getBrakeCalibrationFactor() { double tmp; @@ -353,6 +377,7 @@ void Fortius::run() // variables for telemetry, copied to fields on each brake update double curPower; // current output power in Watts + double curForce_N; // current output force in Newtons double curHeartRate; // current heartrate in BPM double curCadence; // current cadence in RPM double curSpeed; // current speed in KPH @@ -373,6 +398,7 @@ void Fortius::run() pvars.lock(); // UNUSED curStatus = this->deviceStatus; curPower = this->devicePower = 0; + curForce_N = this->deviceForce_N = 0; curHeartRate = this->deviceHeartRate = 0; curCadence = this->deviceCadence = 0; curSpeed = this->deviceSpeed = 0; @@ -471,7 +497,7 @@ void Fortius::run() curSpeed = ms_to_kph(rawSpeed_to_ms(qFromLittleEndian(&buf[32]))); // If this is torque, we could also compute power from distance and time - const double curForce_N = rawForce_to_N(qFromLittleEndian(&buf[38])); + curForce_N = rawForce_to_N(qFromLittleEndian(&buf[38])); curPower = curForce_N * kph_to_ms(curSpeed); if (curPower < 0.0) curPower = 0.0; // brake power can be -ve when coasting. @@ -492,6 +518,7 @@ void Fortius::run() deviceSpeed = curSpeed; deviceCadence = curCadence; deviceHeartRate = curHeartRate; + deviceForce_N = curForce_N; devicePower = curPower; pvars.unlock(); } @@ -545,6 +572,7 @@ void Fortius::run() * sendOpenCommand() - initialises training session * sendCloseCommand() - finalises training session * sendRunCommand(int) - update brake setpoint + * sendCalibrateCommand() - start calibration at 20 kph * * ---------------------------------------------------------------------- */ int Fortius::sendOpenCommand() @@ -566,6 +594,15 @@ int Fortius::sendCloseCommand() return retCode; } +int Fortius::sendCalibrateCommand() +{ + uint8_t calibrate_command[] = {0x01,0x08,0x01,0x00,0xa3,0x16,0x00,0x00,0x03,0x00,0x00,0x00}; + + int retCode = rawWrite(calibrate_command, 12); + //qDebug() << "usb status " << retCode; + return retCode; +} + int Fortius::sendRunCommand(int16_t pedalSensor) { int retCode = 0; @@ -574,6 +611,7 @@ int Fortius::sendRunCommand(int16_t pedalSensor) double gradient = this->gradient; double load = this->load; double weight = this->weight; + double brakeCalibrationForce_N = this->brakeCalibrationForce_N; double brakeCalibrationFactor = this->brakeCalibrationFactor; pvars.unlock(); @@ -589,7 +627,7 @@ int Fortius::sendRunCommand(int16_t pedalSensor) qToLittleEndian(limitedRawForce, &ERGO_Command[4]); ERGO_Command[6] = pedalSensor; - qToLittleEndian((int16_t)(130 * brakeCalibrationFactor + 1040), &ERGO_Command[10]); + qToLittleEndian((int16_t)(130 * brakeCalibrationFactor + N_to_rawForce(brakeCalibrationForce_N)), &ERGO_Command[10]); retCode = rawWrite(ERGO_Command, 12); } @@ -606,7 +644,7 @@ int Fortius::sendRunCommand(int16_t pedalSensor) SLOPE_Command[6] = pedalSensor; SLOPE_Command[9] = (unsigned int)weight; - qToLittleEndian((int16_t)(130 * brakeCalibrationFactor + 1040), &SLOPE_Command[10]); + qToLittleEndian((int16_t)(130 * brakeCalibrationFactor + N_to_rawForce(brakeCalibrationForce_N)), &SLOPE_Command[10]); retCode = rawWrite(SLOPE_Command, 12); // qDebug() << "Send Gradient " << gradient << ", Weight " << weight << ", Command " << QByteArray((const char *)SLOPE_Command, 12).toHex(':'); @@ -617,9 +655,7 @@ int Fortius::sendRunCommand(int16_t pedalSensor) } else if (mode == FT_CALIBRATE) { - // Not yet implemented, easy enough to start calibration but appears that the calibration factor needs - // to be calculated by observing the brake power and speed after calibration starts (i.e. it's not returned - // by the brake). + retCode = sendCalibrateCommand(); } //qDebug() << "usb status " << retCode; diff --git a/src/Train/Fortius.h b/src/Train/Fortius.h index 390c202f2..b060d86cc 100644 --- a/src/Train/Fortius.h +++ b/src/Train/Fortius.h @@ -72,7 +72,8 @@ #define DEFAULT_LOAD 100.00 #define DEFAULT_GRADIENT 2.00 #define DEFAULT_WEIGHT 77 -#define DEFAULT_CALIBRATION 0.00 +#define DEFAULT_CALIBRATION_FORCE_N (rawForce_to_N(1040)) +#define DEFAULT_CALIBRATION_FACTOR 0.00 #define DEFAULT_SCALING 1.00 #define DEFAULT_WINDSPEED 0.00 #define DEFAULT_Crr 0.004 @@ -102,6 +103,7 @@ public: // SET void setLoad(double load); // set the load to generate in ERGOMODE void setGradient(double gradient); // set the load to generate in SSMODE + void setBrakeCalibrationForce(double value); // set the calibration force (N) for ERGOMODE and SSMODE void setBrakeCalibrationFactor(double calibrationFactor); // Impacts relationship between brake setpoint and load void setPowerScaleFactor(double calibrationFactor); // Scales output power, so user can adjust to match hub or crank power meter void setMode(int mode); @@ -113,6 +115,7 @@ public: int getMode(); double getGradient(); double getLoad(); + double getBrakeCalibrationForce() const; double getBrakeCalibrationFactor(); double getPowerScaleFactor(); double getWeight(); @@ -120,7 +123,7 @@ public: // GET TELEMETRY AND STATUS // direct access to class variables is not allowed because we need to use wait conditions // to sync data read/writes between the run() thread and the main gui thread - void getTelemetry(double &power, double &heartrate, double &cadence, double &speed, double &distance, int &buttons, int &steering, int &status); + void getTelemetry(double &power, double &force, double &heartrate, double &cadence, double &speed, double &distance, int &buttons, int &steering, int &status); private: void run(); // called by start to kick off the CT comtrol thread @@ -136,6 +139,7 @@ private: int sendRunCommand(int16_t pedalSensor); int sendOpenCommand(); int sendCloseCommand(); + int sendCalibrateCommand(); // Protocol decoding int readMessage(); @@ -146,6 +150,7 @@ private: // INBOUND TELEMETRY - all volatile since it is updated by the run() thread volatile double devicePower; // current output power in Watts + volatile double deviceForce_N; // current output force in Newtons volatile double deviceHeartRate; // current heartrate in BPM volatile double deviceCadence; // current cadence in RPM volatile double deviceSpeed; // current speed in KPH @@ -158,6 +163,7 @@ private: volatile int mode; volatile double load; volatile double gradient; + volatile double brakeCalibrationForce_N; volatile double brakeCalibrationFactor; volatile double powerScaleFactor; volatile double weight; @@ -176,6 +182,7 @@ private: int rawRead(uint8_t *bytes, int size); // unix!! +public: // Unit conversion routines static inline double kph_to_ms (double kph) { return kph / 3.6; } static inline double ms_to_kph (double ms) { return ms * 3.6; } @@ -185,6 +192,7 @@ private: static inline double rawForce_to_N (double raw) { return raw / 137.; } static inline double N_to_rawForce (double N) { return N * 137.; } +private: // Source: https://github.com/totalreverse/ttyT1941/wiki // - "speed = 'kph * 289.75'" static inline double rawSpeed_to_ms (double raw) { return raw / 1043.1; } // 289.75*3.6 diff --git a/src/Train/FortiusController.cpp b/src/Train/FortiusController.cpp index a8a197221..768e88095 100644 --- a/src/Train/FortiusController.cpp +++ b/src/Train/FortiusController.cpp @@ -79,7 +79,7 @@ FortiusController::getRealtimeData(RealtimeData &rtData) // Added Distance and Steering here but yet to RealtimeData int Buttons, Status, Steering; - double Power, HeartRate, Cadence, Speed, Distance; + double Power, Force_N, HeartRate, Cadence, Speed, Distance; if(!myFortius->isRunning()) { @@ -88,7 +88,7 @@ FortiusController::getRealtimeData(RealtimeData &rtData) return; } // get latest telemetry - myFortius->getTelemetry(Power, HeartRate, Cadence, Speed, Distance, Buttons, Steering, Status); + myFortius->getTelemetry(Power, Force_N, HeartRate, Cadence, Speed, Distance, Buttons, Steering, Status); // // PASS BACK TELEMETRY @@ -179,3 +179,178 @@ FortiusController::setWindResistance(double windResistance) myFortius->setWindResistance(windResistance); } + + +// Calibration + +uint8_t +FortiusController::getCalibrationType() +{ + return CALIBRATION_TYPE_FORTIUS; +} + +double +FortiusController::getCalibrationTargetSpeed() +{ + return 20; // kph +} + +uint8_t +FortiusController::getCalibrationState() +{ + return m_calibrationState; +} + +void +FortiusController::setCalibrationState(uint8_t state) +{ + m_calibrationState = state; + switch (state) + { + case CALIBRATION_STATE_IDLE: + myFortius->setMode(FT_IDLE); + break; + + case CALIBRATION_STATE_PENDING: + myFortius->setMode(FT_CALIBRATE); + m_calibrationState = CALIBRATION_STATE_REQUESTED; + break; + + default: + break; + } +} + +uint16_t +FortiusController::getCalibrationZeroOffset() +{ + switch (m_calibrationState) + { + // Waiting for user to kick pedal... + case CALIBRATION_STATE_REQUESTED: + { + int Buttons, Status, Steering; + double Power, Force_N, HeartRate, Cadence, SpeedKmh, Distance; + myFortius->getTelemetry(Power, Force_N, HeartRate, Cadence, SpeedKmh, Distance, Buttons, Steering, Status); + + // Once we're up to speed (only takes a second or so), move on to next stage + if (SpeedKmh > 0.995 * getCalibrationTargetSpeed()) // Within 0.005 of the target speed + { + m_calibrationValues.reset(); + m_calibrationState = CALIBRATION_STATE_STARTING; + } + + // Return value for presentation in the GUI + return 0; + } + + // Calibration starting, waiting until we have enough values + case CALIBRATION_STATE_STARTING: + { + // Get current force value + int Buttons, Status, Steering; + double Power, Force_N, HeartRate, Cadence, SpeedKmh, Distance; + myFortius->getTelemetry(Power, Force_N, HeartRate, Cadence, SpeedKmh, Distance, Buttons, Steering, Status); + + // unexpected drop in wheel speed will cause calibration to terminate + if (SpeedKmh < 0.9 * getCalibrationTargetSpeed()) // 10% underspeed would be considered an error case + { + m_calibrationState = CALIBRATION_STATE_FAILURE; + return 0; + } + + // Push onto the list of recent values + m_calibrationValues.update(Force_N); + + // If we have enough values, move on to next stage + if (m_calibrationValues.is_full()) + { + m_calibrationState = CALIBRATION_STATE_STARTED; + m_calibrationStarted = time(nullptr); + } + + // Return value for presentation in the GUI + // Need to return a uint16_t, use +ve rawForce + return Fortius::N_to_rawForce(-Force_N); + } + + // Calibration started, runs until standard deviation is below some threshold + case CALIBRATION_STATE_STARTED: + { + // Get current force value + int Buttons, Status, Steering; + double Power, Force_N, HeartRate, Cadence, SpeedKmh, Distance; + myFortius->getTelemetry(Power, Force_N, HeartRate, Cadence, SpeedKmh, Distance, Buttons, Steering, Status); + + // Push onto the list of recent values + m_calibrationValues.update(Force_N); + + // unexpected resistance (pedalling) will cause calibration to terminate + if (Force_N > 0) + { + m_calibrationState = CALIBRATION_STATE_FAILURE; + return 0; + } + + // unexpected drop in wheel speed will cause calibration to terminate + if (SpeedKmh < 0.9 * getCalibrationTargetSpeed()) // 10% underspeed would be considered an error case + { + m_calibrationState = CALIBRATION_STATE_FAILURE; + return 0; + } + + // calculate the average + const double mean = m_calibrationValues.mean(); + const double stddev = m_calibrationValues.stddev(); + + // wait until stddev within threshold + // perhaps this isn't the best numerical solution to detect settling + // but it's better than the previous attempt which was based on diff(min/max) + // I'd prefer a tighter threshold, eg 0.02 + // but runtime would be too long for users, especially from cold + static const double stddev_threshold = 0.05; + + // termination (settling) conditions + const time_t calibrationDuration = time(nullptr) - m_calibrationStarted; + if (stddev < stddev_threshold || calibrationDuration > calibrationDurationLimit_s) + { + // TODO... just because we have determined the current value + // doesn't mean it's "right"... TTS4 had a red-green bar and + // plotted the value on that bar, and user was to adjust wheel + // pressure if it was outside the green range. + + // Source: https://github.com/totalreverse/ttyT1941/wiki + // ; If the 'gauge' of the calibration screen is perfect in the + // ; "green" middle of the calibration bar, then TTS4 sets the + // ; calibration value to 0x0492 (= 90*13) + + + // accept the current average as the final valibration value + // Note: call setBrakeCalibrationForce() with +ve value + myFortius->setBrakeCalibrationForce(-mean); + + // complete calibration + m_calibrationState = CALIBRATION_STATE_SUCCESS; + myFortius->setMode(FT_IDLE); + } + + // Return value for presentation in the GUI + // Need to return a uint16_t, use +ve rawForce + return Fortius::N_to_rawForce(-mean); + } + + case CALIBRATION_STATE_SUCCESS: + return Fortius::N_to_rawForce(myFortius->getBrakeCalibrationForce()); + + default: + return 0; + } +} + +void +FortiusController::resetCalibrationState() +{ + m_calibrationState = CALIBRATION_STATE_IDLE; + myFortius->setMode(FT_IDLE); +} + diff --git a/src/Train/FortiusController.h b/src/Train/FortiusController.h index 3cb1a8348..7aa782917 100644 --- a/src/Train/FortiusController.h +++ b/src/Train/FortiusController.h @@ -25,6 +25,56 @@ #ifndef _GC_FortiusController_h #define _GC_FortiusController_h 1 +template +class NSampleSmoothing +{ +private: + int nSamples = 0; + double samples[N]; + int index = 0; + double total = 0; + bool full = false; + +public: + NSampleSmoothing() { reset(); } + + void reset() + { + for (int i = 0; i < N; ++i) + samples[i] = 0.; + nSamples = 0; + index = 0; + total = 0; + full = false; + } + + void update(double newVal) + { + ++nSamples; + + total += newVal - samples[index]; + samples[index] = newVal; + if (++index == N) { index = 0; full = true; } + } + + bool is_full() const + { + return full; + } + + double mean() const + { + // average if we have enough values, otherwise return latest + return total / N;//(nSamples > N) ? (total / N) : samples[(index + (N-1))%N]; + } + + double stddev() const + { + const double avg = mean(); + const double sum_squares = std::accumulate(std::begin(samples), std::end(samples), 0.0, [avg](double acc, double sample) {return acc + (sample - avg) * (sample - avg); }); + return sqrt(sum_squares / static_cast(N)); + } +}; class FortiusController : public RealtimeController { Q_OBJECT @@ -54,6 +104,23 @@ public: void setWindSpeed(double); void setRollingResistance(double); void setWindResistance(double); + + // calibration + static const int calibrationDurationLimit_s = 60; + + uint8_t getCalibrationType() override; + double getCalibrationTargetSpeed() override; + uint8_t getCalibrationState() override; + void setCalibrationState(uint8_t state) override; + uint16_t getCalibrationZeroOffset() override; + void resetCalibrationState() override; + +private: + // calibration data for use by controller + uint8_t m_calibrationState; + time_t m_calibrationStarted; + NSampleSmoothing<100> m_calibrationValues; + }; #endif // _GC_FortiusController_h diff --git a/src/Train/TrainSidebar.cpp b/src/Train/TrainSidebar.cpp index 55cf08f99..a3ec4a934 100644 --- a/src/Train/TrainSidebar.cpp +++ b/src/Train/TrainSidebar.cpp @@ -2408,6 +2408,62 @@ void TrainSidebar::updateCalibration() } break; + case CALIBRATION_TYPE_FORTIUS: + + switch (calibrationState) { + + case CALIBRATION_STATE_IDLE: + case CALIBRATION_STATE_PENDING: + break; + + case CALIBRATION_STATE_REQUESTED: + status = QString(tr("Give the pedal a kick to start calibration...\nThe motor will run until calibration is complete.")); + break; + + case CALIBRATION_STATE_STARTING: + status = QString(tr("Calibrating... DO NOT PEDAL, remain seated...\nGathering enough samples to calculate average: %1")) + .arg(QString::number((int16_t)calibrationZeroOffset)); + break; + + case CALIBRATION_STATE_STARTED: + { + const double calibrationPower_W = Fortius::rawForce_to_N(calibrationZeroOffset) * Fortius::kph_to_ms(calibrationTargetSpeed); + status = QString(tr("Calibrating... DO NOT PEDAL, remain seated...\nWaiting for value to stabilize (max %1s): %2 (%3W @ %4kph)")) + .arg(QString::number((int16_t)FortiusController::calibrationDurationLimit_s), + QString::number((int16_t)calibrationZeroOffset), + QString::number((int16_t)calibrationPower_W), + QString::number((int16_t)calibrationTargetSpeed)); + } + break; + + case CALIBRATION_STATE_POWER: + case CALIBRATION_STATE_COAST: + break; + + case CALIBRATION_STATE_SUCCESS: + { + const double calibrationPower_W = Fortius::rawForce_to_N(calibrationZeroOffset) * Fortius::kph_to_ms(calibrationTargetSpeed); + status = QString(tr("Calibration completed successfully!\nFinal calibration value %1 (%2W @ %3kph)")) + .arg(QString::number((int16_t)calibrationZeroOffset), + QString::number((int16_t)calibrationPower_W), + QString::number((int16_t)calibrationTargetSpeed)); + + // No further ANT messages to set state, so must move ourselves on.. + if ((stateCount % 25) == 0) + finishCalibration = true; + } + break; + + case CALIBRATION_STATE_FAILURE: + status = QString(tr("Calibration failed! Do not pedal while calibration is taking place.\nAllow wheel to run freely.")); + + // No further ANT messages to set state, so must move ourselves on.. + if ((stateCount % 25) == 0) + finishCalibration = true; + break; + } + break; + } lastState = calibrationState;