Support for SRM manual zero offset calibration

As there is no notification of calibration completion in the SRM profile,
we progress the calibration state on to completion five seconds after we
start receiving calibration messages.

Temporarily removes support for auto-zero, as was not used, and current
code did not follow the standards set in the ANT+ documentation.

src/ANT/ANT.h

@@ -521,6 +521,11 @@ public slots:
         return calibration.getZeroOffset();
     }
 
+    uint16_t getCalibrationSlope()
+    {
+        return calibration.getSlope();
+    }
+
     uint16_t getCalibrationSpindownTime()
     {
         return calibration.getZeroOffset();
@@ -546,6 +551,11 @@ public slots:
         calibration.setZeroOffset(offset);
     }
 
+    void setCalibrationSlope(uint16_t slope)
+    {
+        calibration.setSlope(slope);
+    }
+
     void setCalibrationSpindownTime(uint16_t time)
     {
         calibration.setSpindownTime(time);

src/ANT/ANTChannel.cpp

@@ -43,6 +43,7 @@ ANTChannel::init()
     is_old_cinqo=0;
     is_alt=0;
     is_master=0;
+    is_srm=0;
     manufacturer_id=0;
     product_id=0;
     product_version=0;
@@ -283,6 +284,16 @@ void ANTChannel::checkMoxy()
         is_moxy=1;
 }
 
+// are we an SRM?
+// has different calibration flow to other power meters
+void ANTChannel::checkSRM()
+{
+    if (!is_srm && manufacturer_id==6) {
+        qDebug() << "SRM device detected on channel" << number;
+        is_srm=1;
+    }
+}
+
 // notify we have a cinqo, does nothing
 void ANTChannel::sendCinqoSuccess() {}
 
@@ -331,6 +342,7 @@ void ANTChannel::broadcastEvent(unsigned char *ant_message)
         product_version|=(PRODUCT_SW_REV(message))<<8;
         checkCinqo();
         checkMoxy();
+        checkSRM();
 
         // TODO in some case ? eg a cadence sensor we have telemetry too
         //telemetry = true;
@@ -369,7 +381,11 @@ void ANTChannel::broadcastEvent(unsigned char *ant_message)
            case CHANNEL_TYPE_POWER:
 
                 // Device is a power meter, so assume we support manual zero offset calibration
-                parent->setCalibrationType(number, CALIBRATION_TYPE_ZERO_OFFSET);
+                // (unique type for SRM as CTF devices show different output in train view)
+                if (is_srm)
+                    parent->setCalibrationType(number, CALIBRATION_TYPE_ZERO_OFFSET_SRM);
+                else
+                    parent->setCalibrationType(number, CALIBRATION_TYPE_ZERO_OFFSET);
 
                 // calibration has been manually requested
                 if (parent->modeCALIBRATE() && (parent->getCalibrationChannel() == number) && (parent->getCalibrationState() == CALIBRATION_STATE_REQUESTED)) {
@@ -378,11 +394,29 @@ void ANTChannel::broadcastEvent(unsigned char *ant_message)
                     //qDebug() << "Setting last calibration timestamp for channel" << number;
                     parent->setCalibrationTimestamp(number, get_timestamp());
 
-                    // note: no obvious feedback that calibration is underway, therefore go straight to COAST
-                    parent->setCalibrationState(CALIBRATION_STATE_COAST);
+                    // No obvious feedback that calibration is underway for wheel torque or crank torque devices, therefore go straight to COAST
+                    // Crank torque frequency devices (SRM) do send updates during calibration, so can react to these to change state later..
+                    if (is_srm)
+                        parent->setCalibrationState(CALIBRATION_STATE_STARTING);
+                    else
+                        parent->setCalibrationState(CALIBRATION_STATE_COAST);
+
                     parent->requestPwrCalibration(number, ANT_SPORT_CALIBRATION_REQUEST_MANUALZERO);
                 }
 
+                // Crank torque frequency (SRM) devices do not signal that calibration has completed, therefore we need to
+                // check whether a manual calibration is in progress, and just wait 5 seconds before calling it done..
+                if (is_srm && parent->modeCALIBRATE() && (parent->getCalibrationChannel() == number) && (parent->getCalibrationState() == CALIBRATION_STATE_COAST)) {
+                    if (get_timestamp() - srm_calibration_timestamp > 5) {
+                        // 5 seconds have elapsed since starting calibration, so assume completed
+                        qDebug() << "ANT Sport calibration succeeded";
+                        srm_offset = srm_offset_instant;
+                        parent->setCalibrationZeroOffset(srm_offset);
+                        parent->setCalibrationSlope(srm_slope);
+                        parent->setCalibrationState(CALIBRATION_STATE_SUCCESS);
+                    }
+                }
+
                 // what kind of power device is this?
                 switch(antMessage.data_page) {
 
@@ -405,13 +439,40 @@ void ANTChannel::broadcastEvent(unsigned char *ant_message)
                         switch (antMessage.ctfID) {
 
                             case 0x01: // offset
-                                // if we're getting calibration messages then
-                                // we should be coasting, so power and cadence
-                                // will be zero
-                                srm_offset = antMessage.srmOffset;
-                                is_alt ? parent->setAltWatts(0) : parent->setWatts(0);
-                                parent->setSecondaryCadence(0);
-                                value2=value=0;
+
+                                // Can either be manually calibrating this device, or receiving auto zero messages
+                                if (parent->modeCALIBRATE() && (parent->getCalibrationChannel() == number)) {
+                                    // Manually requested calibration on this channel, note that srm_offset
+                                    // is only set after final calibration message
+
+                                    srm_offset_instant = antMessage.srmOffset;
+
+                                    // set these now so they can be displayed in notification area progress messages
+                                    parent->setCalibrationZeroOffset(srm_offset_instant);
+                                    parent->setCalibrationSlope(srm_slope);
+
+                                    if (parent->getCalibrationState() == CALIBRATION_STATE_STARTING) {
+                                        // First calibration message received, move the state on to indicate progress..
+                                        srm_calibration_timestamp = get_timestamp();
+                                        parent->setCalibrationState(CALIBRATION_STATE_COAST);
+                                    }
+
+                                } else {
+                                    // Auto zero
+
+                                    // Note that original code does not follow the spec in the ANT docs, should average
+                                    // multiple readings and only adjust within 4Hz, comment out until auto-zero support
+                                    // added in for all power meter types...
+
+                                    // if we're getting calibration messages then
+                                    // we should be coasting, so power and cadence
+                                    // will be zero
+                                    //srm_offset = antMessage.srmOffset;
+                                    //is_alt ? parent->setAltWatts(0) : parent->setWatts(0);
+                                    //parent->setSecondaryCadence(0);
+                                    //value2=value=0;
+                                }
+
                                 break;
 
                             case 0x02: // slope
@@ -465,17 +526,24 @@ void ANTChannel::broadcastEvent(unsigned char *ant_message)
                 //
                 // SRM - crank torque frequency
                 //
-                case ANT_CRANKSRM_POWER: // 0x20 - crank torque (SRM)
+                case ANT_CRANKSRM_POWER: // 0x20 - crank torque frequency (SRM)
                 {
+                    // for SimulANT testing
+                    //if (!is_srm) is_srm=1;
+
                     uint16_t period = antMessage.period - lastMessage.period;
                     uint16_t torque = antMessage.torque - lastMessage.torque;
                     float time = (float)period / (float)2000.00;
 
+                    // note: can't calculate/display calibration torque if crank not turning, as time=0
+                    //qDebug() << "period:" << period << "time:" << time << "torque:" << torque << "slope:" << antMessage.slope  << "offset:" << srm_offset;
+
                     if (time && antMessage.slope && period) {
 
                         nullCount = 0;
-                        float torque_freq = torque / time - 420/*srm_offset*/;
-                        float nm_torque = 10.0 * torque_freq / antMessage.slope;
+                        srm_slope = antMessage.slope;
+                        float torque_freq = torque / time - srm_offset;
+                        float nm_torque = 10.0 * torque_freq / srm_slope;
                         float cadence = 2000.0 * 60 * (antMessage.eventCount - lastMessage.eventCount) / period;
                         float power = 3.14159 * nm_torque * cadence / 30;
 

src/ANT/ANTChannel.h

@@ -162,12 +162,17 @@ class ANTChannel : public QObject {
         bool is_moxy; // bool
         bool is_cinqo; // bool
         bool is_old_cinqo; // bool, set for cinqo needing separate control channel
+        bool is_srm;
         bool is_fec;
         bool is_alt; // is alternative channel for power
         bool is_master; // is a master channel (for remote control)
 
         int search_type;
-        int srm_offset;
+
+        int      srm_offset;
+        int      srm_offset_instant;
+        double   srm_calibration_timestamp;
+        uint16_t srm_slope;
 
         ANTChannel(int number, ANT *parent);
 
@@ -208,6 +213,7 @@ class ANTChannel : public QObject {
         void sendCinqoSuccess();
         void checkCinqo();
         void checkMoxy();
+        void checkSRM();
 
         void setAlt(bool value) { is_alt = value; }
 

src/ANT/ANTlocalController.cpp

@@ -180,6 +180,12 @@ ANTlocalController::getCalibrationZeroOffset()
     return myANTlocal->getCalibrationZeroOffset();
 }
 
+uint16_t
+ANTlocalController::getCalibrationSlope()
+{
+    return myANTlocal->getCalibrationSlope();
+}
+
 void
 ANTlocalController::setCalibrationState(uint8_t state)
 {

src/ANT/ANTlocalController.h

@@ -73,6 +73,7 @@ class ANTlocalController : public RealtimeController
     void     setCalibrationState(uint8_t);
     uint16_t getCalibrationSpindownTime();
     uint16_t getCalibrationZeroOffset();
+    uint16_t getCalibrationSlope();
     void     resetCalibrationState();
 
 signals:

src/Train/CalibrationData.cpp

@@ -34,7 +34,7 @@ void CalibrationData::resetCalibrationState()
         requested[i] = false;
     }
     state = CALIBRATION_STATE_IDLE;
-    activechannel = targetspeed = spindowntime = zerooffset = 0;
+    activechannel = targetspeed = spindowntime = zerooffset = slope = 0;
 }
 
 uint8_t CalibrationData::getType()
@@ -93,6 +93,19 @@ void CalibrationData::setZeroOffset(uint16_t offset)
     }
 }
 
+uint16_t CalibrationData::getSlope()
+{
+    return this->slope;
+}
+
+void CalibrationData::setSlope(uint16_t slope)
+{
+    if (this->slope != slope) {
+        qDebug() << "Calibration slope changing to" << slope;
+        this->slope = slope;
+    }
+}
+
 double CalibrationData::getTargetSpeed()
 {
     return this->targetspeed;

src/Train/CalibrationData.h

@@ -21,10 +21,11 @@
 
 #include <stdint.h> // uint8_t etc
 
-#define CALIBRATION_TYPE_NOT_SUPPORTED  0x00
-#define CALIBRATION_TYPE_COMPUTRAINER   0x01
-#define CALIBRATION_TYPE_ZERO_OFFSET    0x02
-#define CALIBRATION_TYPE_SPINDOWN       0x04
+#define CALIBRATION_TYPE_NOT_SUPPORTED      0x00
+#define CALIBRATION_TYPE_COMPUTRAINER       0x01
+#define CALIBRATION_TYPE_ZERO_OFFSET        0x02
+#define CALIBRATION_TYPE_ZERO_OFFSET_SRM    0x04
+#define CALIBRATION_TYPE_SPINDOWN           0x08
 
 #define CALIBRATION_STATE_IDLE          0x00
 #define CALIBRATION_STATE_PENDING       0x01
@@ -59,6 +60,9 @@ class CalibrationData
     double getTargetSpeed();
     void setTargetSpeed(double speed);
 
+    uint16_t getSlope();
+    void setSlope(uint16_t slope);
+
     void    resetCalibrationState(void);
     void    setRequested(uint8_t channel, bool required);
     void    setTimestamp(uint8_t channel, double time);
@@ -77,6 +81,7 @@ class CalibrationData
     uint8_t  state;
     uint16_t zerooffset;
     uint16_t spindowntime;
+    uint16_t slope;
     double   targetspeed;
 
 };

src/Train/RealtimeController.h

@@ -68,6 +68,7 @@ class RealtimeController : public QObject
     virtual void     setCalibrationState(uint8_t) {return; }
     virtual uint16_t getCalibrationSpindownTime() { return 0; }
     virtual uint16_t getCalibrationZeroOffset() { return 0; }
+    virtual uint16_t getCalibrationSlope() {return 0; }
     virtual void     resetCalibrationState() { return; }
 
     void   setCalibrationTimestamp();

src/Train/TrainSidebar.cpp

@@ -1148,7 +1148,7 @@ void TrainSidebar::Start()       // when start button is pressed
 
         //reset all calibration data
         calibrating = startCalibration = restartCalibration = finishCalibration = false;
-        calibrationSpindownTime = calibrationZeroOffset = calibrationTargetSpeed = 0;
+        calibrationSpindownTime = calibrationZeroOffset = calibrationSlope = calibrationTargetSpeed = 0;
         calibrationCadence = calibrationCurrentSpeed = calibrationTorque = 0;
         calibrationState = CALIBRATION_STATE_IDLE;
         calibrationType = CALIBRATION_TYPE_NOT_SUPPORTED;
@@ -1272,7 +1272,7 @@ void TrainSidebar::Stop(int deviceStatus)        // when stop button is pressed
 
     //reset all calibration data
     calibrating = startCalibration = restartCalibration = finishCalibration = false;
-    calibrationSpindownTime = calibrationZeroOffset = calibrationTargetSpeed = 0;
+    calibrationSpindownTime = calibrationZeroOffset = calibrationSlope = calibrationTargetSpeed = 0;
     calibrationCadence = calibrationCurrentSpeed = calibrationTorque = 0;
     calibrationState = CALIBRATION_STATE_IDLE;
     calibrationType = CALIBRATION_TYPE_NOT_SUPPORTED;
@@ -1485,6 +1485,7 @@ void TrainSidebar::guiUpdate()           // refreshes the telemetry
 
                     calibrationSpindownTime =  Devices[dev].controller->getCalibrationSpindownTime();
                     calibrationZeroOffset =  Devices[dev].controller->getCalibrationZeroOffset();
+                    calibrationSlope = Devices[dev].controller->getCalibrationSlope();
 
                     // if calibration is moving out of pending state..
                     if ((calibrationState == CALIBRATION_STATE_PENDING) && startCalibration) {
@@ -2069,6 +2070,59 @@ void TrainSidebar::updateCalibration()
 
             }
             break;
+
+        case CALIBRATION_TYPE_ZERO_OFFSET_SRM:
+
+            switch (calibrationState) {
+
+            case CALIBRATION_STATE_IDLE:
+                break;
+
+            case CALIBRATION_STATE_PENDING:
+                // Wait for cadence to be zero before requesting zero offset calibration
+                status = QString(tr("Unclip or stop pedalling to begin calibration.."));
+                if (calibrationCadence == 0)
+                    startCalibration = true;
+                break;
+
+            case CALIBRATION_STATE_REQUESTED:
+                status = QString(tr("Requesting calibration.."));
+                break;
+
+            case CALIBRATION_STATE_STARTING:
+                status = QString(tr("Requesting calibration.."));
+                break;
+
+            case CALIBRATION_STATE_STARTED:
+                break;
+
+            case CALIBRATION_STATE_POWER:
+                break;
+
+            case CALIBRATION_STATE_COAST:
+                status = QString(tr("Calibrating...\nUnclip or stop pedalling until process is completed..\nZero Offset %1")).arg(QString::number((int16_t)calibrationZeroOffset));
+                break;
+
+            case CALIBRATION_STATE_SUCCESS:
+                // yuk, zero offset for FE-C devices is unsigned, but for power meters is signed..
+                status = QString(tr("Calibration completed successfully!\nZero Offset %1\nSlope %2")).arg(QString::number((int16_t)calibrationZeroOffset), QString::number(calibrationSlope));;
+
+                // 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!"));
+
+                // No further ANT messages to set state, so must move ourselves on..
+                if ((stateCount % 25) == 0)
+                    finishCalibration = true;
+                break;
+
+            }
+            break;
+
         }
 
         lastState = calibrationState;

src/Train/TrainSidebar.h

@@ -265,7 +265,7 @@ class TrainSidebar : public GcWindow
         bool     startCalibration, restartCalibration, finishCalibration;
         int      calibrationDeviceIndex;
         uint8_t  calibrationType, calibrationState;
-        uint16_t calibrationSpindownTime, calibrationZeroOffset;
+        uint16_t calibrationSpindownTime, calibrationZeroOffset, calibrationSlope;
         double   calibrationTargetSpeed, calibrationCurrentSpeed;
         double   calibrationCadence, calibrationTorque;