From c17c5488bbec9eb3f5bda75eefba70b697360451 Mon Sep 17 00:00:00 2001
From: Mark Liversedge <liversedge@gmail.com>
Date: Sun, 6 Mar 2011 21:16:04 +0000
Subject: [PATCH] Native ANT+ Part 2 - tweaks

Couple of little tweaks to the Native ANT support. Firstly,
data channels such as watts, hr and cadence are now rounded
to an integer (lots of grid lines in ride plot looked nasty).

Secondly, basic support for calibration messages; this has been
limited to SRM messages, but will add support for other devices
as we start to debug them.

Part 3 of this work is still pending, this patch only contains
adjustments to part 2. I suspect there will be further adjustments
as we squash bugs for Quarq and PT users.
---
 src/ANT.h            |  1 +
 src/ANTChannel.cpp   | 50 +++++++++++++++++++++++++++++++++++++++++++-
 src/ANTMessage.cpp   | 46 +++++++++++++++++++++++++++++++++++-----
 src/ANTMessage.h     |  3 ++-
 src/RealtimeData.cpp |  6 +++---
 5 files changed, 96 insertions(+), 10 deletions(-)

diff --git a/src/ANT.h b/src/ANT.h
index d4262e84b..07e282f61 100644
--- a/src/ANT.h
+++ b/src/ANT.h
@@ -199,6 +199,7 @@ static inline double get_timestamp( void ) {
 #define ANT_QUARQ_FREQUENCY 61
 
 #define ANT_SPORT_CALIBRATION_MESSAGE                 0x01
+#define ANT_SPORT_SRM_CALIBRATIONID                   0x10
 #define ANT_SPORT_AUTOZERO_OFF                        0x00
 #define ANT_SPORT_AUTOZERO_ON                         0x01
 #define ANT_SPORT_CALIBRATION_REQUEST_MANUALZERO      0xAA
diff --git a/src/ANTChannel.cpp b/src/ANTChannel.cpp
index 691611495..6d49be640 100755
--- a/src/ANTChannel.cpp
+++ b/src/ANTChannel.cpp
@@ -285,7 +285,53 @@ void ANTChannel::broadcastEvent(unsigned char *ant_message)
            case CHANNEL_TYPE_FAST_QUARQ_NEW:
 
                 // what kind of power device is this?
-                switch(antMessage.power_type) {
+                switch(antMessage.data_page) {
+
+                case ANT_SPORT_CALIBRATION_MESSAGE:
+                {
+                    // Always ack calibs unless they are acks too!
+                    if (antMessage.data[6] != 0xAC) {
+                        antMessage.data[6] = 0xAC;
+                        qDebug()<<"sending calibration ack...";
+                        parent->sendMessage(antMessage);
+                    }
+
+                    // each device sends a different type
+                    // of calibration message...
+                    switch (antMessage.calibrationID) {
+
+                    case ANT_SPORT_SRM_CALIBRATIONID:
+
+                        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;
+                                parent->setWatts(0);
+                                parent->setCadence(0);
+qDebug()<<"got new offset!"<<srm_offset;
+                                break;
+
+                            case 0x02: // slope
+                                break;
+
+                            case 0x03: // serial number
+                                break;
+
+                            default:
+                                break;
+
+                            }
+                            break;
+
+                        default: //XXX need to support Powertap/Quarq too!!
+                            break;
+                    }
+
+                } // ANT_SPORT_CALIBRATION
+                break;
 
                 //
                 // SRM - crank torque frequency
@@ -313,6 +359,8 @@ void ANTChannel::broadcastEvent(unsigned char *ant_message)
                     } else {
 
                         nullCount++;
+                        antMessage.type = 0; // we need a new data pair
+
                         if (nullCount >= 4) { // 4 messages on an SRM
                             parent->setWatts(0);
                             parent->setCadence(0);
diff --git a/src/ANTMessage.cpp b/src/ANTMessage.cpp
index 984f19d15..e6b59eb38 100755
--- a/src/ANTMessage.cpp
+++ b/src/ANTMessage.cpp
@@ -372,6 +372,8 @@ qDebug()<<"request message";
 
 qDebug()<<"broadcast data, channel="<<message[3]<<"type="<<message[4]<<"calid?"<<message[5];
 
+            data_page = message[4];
+
             // we need to handle ant sport messages here
             switch(parent->antChannel[message[3]]->channel_type) {
 
@@ -398,11 +400,11 @@ qDebug()<<"broadcast data, channel="<<message[3]<<"type="<<message[4]<<"calid?"<
             case ANTChannel::CHANNEL_TYPE_FAST_QUARQ_NEW:
 
                 channel = message[3];
-                power_type = message[4];
 
-                switch (power_type) {
+                switch (data_page) {
 
                 case ANT_STANDARD_POWER: // 0x10 - standard power
+
                     eventCount = message[5];
                     instantCadence = message[6];
                     sumPower = message[7] + (message[8]<<8);
@@ -432,9 +434,41 @@ qDebug()<<"broadcast data, channel="<<message[3]<<"type="<<message[4]<<"calid?"<
                     torque = message[11] + (message[10]<<8); // yes it is bigendian
                     break;
 
-                default: // UNKNOWN
+                case ANT_SPORT_CALIBRATION_MESSAGE:
+
+                    calibrationID = message[5];
+                    ctfID = message[6];
+
+                    switch (calibrationID) {
+
+                        case ANT_SPORT_SRM_CALIBRATIONID:
+
+                            switch(ctfID) { // different types of calibration for SRMs
+
+                            case 0x01 : // srm_offset
+                                srmOffset = message[11] + (message[10]<<8); // yes it is bigendian
+                                break;
+
+                            case 0x02 : // slope
+                                srmSlope = message[11] + (message[10]<<8); // yes it is bigendian
+                                break;
+
+                            case 0x03 : //serial number
+                                srmSerial = message[11] + (message[10]<<8); // yes it is bigendian
+                                break;
+
+                            default:
+                            case 0xAC : // ack
+                                break;
+                            }
+                            break;
+
+                        default: // XXX calib support for Quarq/PT
+                            break;
+
+                    }
                     break;
-                }
+                } // data_page
                 break;
 
             case ANTChannel::CHANNEL_TYPE_SPEED:
@@ -535,7 +569,7 @@ ANTMessage::ANTMessage(const unsigned char len,
 void ANTMessage::init()
 {
     timestamp = get_timestamp();
-    power_type = frequency = deviceType = transmitPower = searchTimeout = 0;
+    data_page = frequency = deviceType = transmitPower = searchTimeout = 0;
     transmissionType = networkNumber = channelType = channel = 0;
     channelPeriod = deviceNumber = 0;
     wheelMeasurementTime = crankMeasurementTime = measurementTime = 0;
@@ -544,6 +578,8 @@ void ANTMessage::init()
     wheelRevolutions = crankRevolutions = 0;
     slope = period = torque = 0;
     sync = length = type = 0;
+    srmOffset = srmSlope = srmSerial = 0;
+    calibrationID = ctfID = 0;
 }
 
 ANTMessage ANTMessage::resetSystem()
diff --git a/src/ANTMessage.h b/src/ANTMessage.h
index 0ad4c23c0..aa1ca756a 100755
--- a/src/ANTMessage.h
+++ b/src/ANTMessage.h
@@ -87,7 +87,8 @@ class ANTMessage {
         uint8_t key[8];
 
         // ANT Sport values
-        uint8_t power_type;
+        uint8_t data_page, calibrationID, ctfID;
+        uint16_t srmOffset, srmSlope, srmSerial;
         uint8_t eventCount;
         uint16_t measurementTime, wheelMeasurementTime, crankMeasurementTime;
         uint8_t heartrateBeats, instantHeartrate; // heartrate
diff --git a/src/RealtimeData.cpp b/src/RealtimeData.cpp
index fead83fa3..c57ee9703 100644
--- a/src/RealtimeData.cpp
+++ b/src/RealtimeData.cpp
@@ -32,11 +32,11 @@ void RealtimeData::setName(char *name)
 }
 void RealtimeData::setWatts(double watts)
 {
-    this->watts = watts;
+    this->watts = (int)watts;
 }
 void RealtimeData::setHr(double hr)
 {
-    this->hr = hr;
+    this->hr = (int)hr;
 }
 void RealtimeData::setTime(long time)
 {
@@ -52,7 +52,7 @@ void RealtimeData::setWheelRpm(double wheelRpm)
 }
 void RealtimeData::setCadence(double aCadence)
 {
-    cadence = aCadence;
+    cadence = (int)aCadence;
 }
 void RealtimeData::setLoad(double load)
 {