nmelone/GoldenCheetah
1
0
Fork 0
mirror of https://github.com/GoldenCheetah/GoldenCheetah.git synced 2026-02-13 16:18:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fortius calibration (#3790)

Browse Source 
Added calibration trainer command, delayed check for user stopped pedaling with visual feedback on progress and saved calibration for next use on success.
This commit is contained in:
mattipee
2021-01-21 12:17:18 +00:00
committed by GitHub
parent a298715bb5
commit 0d3a8c911a
7 changed files with 356 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/Core/Settings.h
View File

@@ -227,6 +227,7 @@
#define GC_DEV_VIRTUAL "<global-trainmode>devicepostProcess"
#define GC_DEV_VIRTUALPOWER "<global-trainmode>devicevirtualPower"
#define FORTIUS_FIRMWARE "<global-trainmode>fortius/firmware"
#define FORTIUS_CALIBRATION "<global-trainmode>fortius/calibration"
#define IMAGIC_FIRMWARE "<global-trainmode>imagic/firmware"
#define TRAIN_MULTI "<global-trainmode>train/multi"
#define TRAIN_AUTOCONNECT "<global-trainmode>train/autoconnect"

1
src/Train/CalibrationData.h
View File

@@ -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

52
src/Train/Fortius.cpp
View File

@@ -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<quint16>(&buf[32])));
// If this is torque, we could also compute power from distance and time
const double curForce_N = rawForce_to_N(qFromLittleEndian<qint16>(&buf[38]));
curForce_N = rawForce_to_N(qFromLittleEndian<qint16>(&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<int16_t>(limitedRawForce, &ERGO_Command[4]);
ERGO_Command[6] = pedalSensor;
qToLittleEndian<int16_t>((int16_t)(130 * brakeCalibrationFactor + 1040), &ERGO_Command[10]);
qToLittleEndian<int16_t>((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>((int16_t)(130 * brakeCalibrationFactor + 1040), &SLOPE_Command[10]);
qToLittleEndian<int16_t>((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;

12
src/Train/Fortius.h
View File

@@ -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

179
src/Train/FortiusController.cpp
View File

@@ -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);
}

67
src/Train/FortiusController.h
View File

@@ -25,6 +25,56 @@
#ifndef _GC_FortiusController_h
#define _GC_FortiusController_h 1
template <size_t N>
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<double>(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

56
src/Train/TrainSidebar.cpp
View File

@@ -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;