From ecc3e04b8c58d1ee896610e1aff42633b8cd046a Mon Sep 17 00:00:00 2001 From: Jon Escombe Date: Fri, 28 Apr 2017 12:24:26 +0100 Subject: [PATCH] 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 | 10 ++++ src/ANT/ANTChannel.cpp | 94 +++++++++++++++++++++++++++++----- src/ANT/ANTChannel.h | 8 ++- src/ANT/ANTlocalController.cpp | 6 +++ src/ANT/ANTlocalController.h | 1 + src/Train/CalibrationData.cpp | 15 +++++- src/Train/CalibrationData.h | 13 +++-- src/Train/RealtimeController.h | 1 + src/Train/TrainSidebar.cpp | 58 ++++++++++++++++++++- src/Train/TrainSidebar.h | 2 +- 10 files changed, 186 insertions(+), 22 deletions(-) diff --git a/src/ANT/ANT.h b/src/ANT/ANT.h index 87f863959d..3246913246 100644 --- a/src/ANT/ANT.h +++ b/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); diff --git a/src/ANT/ANTChannel.cpp b/src/ANT/ANTChannel.cpp index 9b0d234e84..df21ad2c10 100644 --- a/src/ANT/ANTChannel.cpp +++ b/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; diff --git a/src/ANT/ANTChannel.h b/src/ANT/ANTChannel.h index 75724fef47..8463aa3650 100644 --- a/src/ANT/ANTChannel.h +++ b/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; } diff --git a/src/ANT/ANTlocalController.cpp b/src/ANT/ANTlocalController.cpp index e366e69cd5..3a50c42020 100644 --- a/src/ANT/ANTlocalController.cpp +++ b/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) { diff --git a/src/ANT/ANTlocalController.h b/src/ANT/ANTlocalController.h index f729c21fbc..d552a48bbd 100644 --- a/src/ANT/ANTlocalController.h +++ b/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: diff --git a/src/Train/CalibrationData.cpp b/src/Train/CalibrationData.cpp index 81026c6940..ea6d812b11 100644 --- a/src/Train/CalibrationData.cpp +++ b/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; diff --git a/src/Train/CalibrationData.h b/src/Train/CalibrationData.h index 199faa0907..5b6144d122 100644 --- a/src/Train/CalibrationData.h +++ b/src/Train/CalibrationData.h @@ -21,10 +21,11 @@ #include // 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; }; diff --git a/src/Train/RealtimeController.h b/src/Train/RealtimeController.h index 96379f36df..1624c2b673 100644 --- a/src/Train/RealtimeController.h +++ b/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(); diff --git a/src/Train/TrainSidebar.cpp b/src/Train/TrainSidebar.cpp index e323261e2b..80a33c78b8 100644 --- a/src/Train/TrainSidebar.cpp +++ b/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; diff --git a/src/Train/TrainSidebar.h b/src/Train/TrainSidebar.h index d71a0bf7d1..77fcdecd1a 100644 --- a/src/Train/TrainSidebar.h +++ b/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;