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

Native ANT+ Support (2 of 3)

Browse Source 
Second stage of development, refactored the quarqd sources and introduced
an ANTChannel and ANTMessage class.

This is a functional patch and should work with known ANT+ devices, but
has only been tested with a Garmin HR strap, GSC-10 dual speed/cadence
and SRM wireless cranks. It has only been tested with a first generation
Garmin ANT+ USB stick.

It *should* work with other devices (e.g. Powertap, Quarq) but this has
not been tested.

The configuration pane has not been fixed yet, so you can either add a
Native ANT+ device and leave the profile blank (it will autodiscover
whatever it can when you run) or you can copy the profile from a Quarqd
device and use that.

There are lots of bugs;
* Calibration is not supported, uses a static srm_offset
* Wheel circumference is fixed at 2100mm for speed calculations
* Timeouts are hit and miss and need to be completed
* Sensor loss / timeouts are not managed yet
* Burst data and Acks are not handled
* Device descriptions, versions and battery messages are not handled

Aside from the bugs above part 3 wil also need to include;
* Configuration screen fixups and device pairing
* Add a calibration button to the realtime window

Lastly, the refactoring of the quarqd code is incomplete, there is still
a need to use ANTMessage across the code, especially within the ANTChannel
code which still does a bit of decoding locally.
This commit is contained in:
Mark Liversedge
2011-03-05 15:08:41 +00:00
parent dd4f5ff85c
commit 701eb200ef
8 changed files with 1900 additions and 2519 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2526
src/ANT.cpp
View File

File diff suppressed because it is too large Load Diff

395
src/ANT.h
View File

@@ -1,5 +1,6 @@
/*
* Copyright (c) 2009 Justin F. Knotzke (jknotzke@shampoo.ca)
* Copyright (c) 2009 Mark Rages
* Copyright (c) 2011 Mark Liversedge (liversedge@gmail.com)
*
* This program is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
@@ -65,38 +66,8 @@
#define ANT_READTIMEOUT 1000
#define ANT_WRITETIMEOUT 2000
//======================================================================
// Debug output, set DEBUGLEVEL using Quarqd settings
//======================================================================
#define DEBUGLEVEL 0
#define DEBUG_PRINTF(mask, format, args...) \
if (DEBUGLEVEL & mask) \
fprintf(stderr, format, ##args)
#define DEBUG_ERRORS(format, args...) \
DEBUG_PRINTF(DEBUG_LEVEL_ERRORS, format, ##args)
#define DEBUG_ANT_CONNECTION(format, args...) \
DEBUG_PRINTF(DEBUG_LEVEL_ANT_CONNECTION, format, ##args)
#define DEBUG_ANT_MESSAGES(format, args...) \
DEBUG_PRINTF(DEBUG_LEVEL_ANT_MESSAGES, format, ##args)
#define DEBUG_CONFIG_PARSE(format, args...) \
DEBUG_PRINTF(DEBUG_LEVEL_CONFIG_PARSE, format, ##args)
//======================================================================
// ANT channels
//======================================================================
#define CHANNEL_TYPE_QUICK_SEARCH 0x10 // or'ed with current channel type
/* after fast search, wait for slow search. Otherwise, starting slow
search might postpone the fast search on another channel. */
#define CHANNEL_TYPE_WAITING 0x20
#define CHANNEL_TYPE_PAIR 0x40 // to do an Ant pair
#define MESSAGE_RECEIVED -1
class ANTMessage;
class ANTChannel;
typedef struct ant_sensor_type {
int type;
@@ -106,167 +77,84 @@ typedef struct ant_sensor_type {
int network;
const char *descriptive_name;
char suffix;
} ant_sensor_type_t;
#define DEFAULT_NETWORK_NUMBER 0
#define ANT_SPORT_NETWORK_NUMBER 1
#define RX_BURST_DATA_LEN 128
// note: this struct is from quarqd_dist/quarqd/src/generated-headers.h
typedef struct {
int first_time_crank_torque;
int first_time_crank_SRM;
int first_time_wheel_torque;
int first_time_standard_power;
int first_time_torque_support;
int first_time_calibration_pass;
int first_time_calibration_fail;
int first_time_heart_rate;
int first_time_speed;
int first_time_cadence;
int first_time_speed_cadence;
int first_time_manufacturer;
int first_time_product;
int first_time_battery_voltage;
} messages_initialization_t;
#define INITIALIZE_MESSAGES_INITIALIZATION(x) \
x.first_time_crank_torque=1; \
x.first_time_crank_SRM=1; \
x.first_time_wheel_torque=1; \
x.first_time_standard_power=1; \
x.first_time_torque_support=1; \
x.first_time_calibration_pass=1; \
x.first_time_calibration_fail=1; \
x.first_time_heart_rate=1; \
x.first_time_speed=1; \
x.first_time_cadence=1; \
x.first_time_speed_cadence=1; \
x.first_time_manufacturer=1; \
x.first_time_product=1; \
x.first_time_battery_voltage=1; \
typedef struct ant_channel {
int number; // Channel number within Ant chip
int state;
int channel_type;
int channel_type_flags;
int device_number;
int device_id;
int search_type;
struct channel_manager *parent;
double last_message_timestamp;
double blanking_timestamp;
int blanked;
char id[10]; // short identifier
int channel_assigned;
messages_initialization_t mi;
int messages_received; // for signal strength metric
int messages_dropped;
unsigned char rx_burst_data[RX_BURST_DATA_LEN];
int rx_burst_data_index;
unsigned char rx_burst_next_sequence;
void (*rx_burst_disposition)(struct ant_channel *);
void (*tx_ack_disposition)(struct ant_channel *);
int is_cinqo; // bool
int is_old_cinqo; // bool, set for cinqo needing separate control channel
struct ant_channel *control_channel;
/* cinqo-specific, which channel should control
messages be sent to? Open a channel for older
cinqos */
int manufacturer_id;
int product_id;
int product_version;
} ant_channel_t;
static inline double get_timestamp( void ) {
struct timeval tv;
gettimeofday(&tv, NULL);
gettimeofday(&tv, NULL);
return tv.tv_sec * 1.0 + tv.tv_usec * 1.0e-6;
}
//======================================================================
// ANT Channel Management
//======================================================================
#define ANT_CHANNEL_COUNT 4
typedef struct channel_manager {
struct ant_channel channels[ANT_CHANNEL_COUNT];
} channel_manager_t;
//======================================================================
// ANT Messaging
// ANT Constants
//======================================================================
#include "ANTMessages.h"
// ANT constants
#define POWER_MSG_ID 0x10
#define TORQUE_AT_WHEEL_MSG_ID 0x11
#define TORQUE_AT_CRANK_MSG_ID 0x12
#define ANT_MAX_DATA_MESSAGE_SIZE 8
#define ANT_MAX_DATA_MESSAGE_SIZE 8
// ANT Sport Power Broadcast message types
#define ANT_STANDARD_POWER 0x10
#define ANT_WHEELTORQUE_POWER 0x11
#define ANT_CRANKTORQUE_POWER 0x12
#define ANT_CRANKSRM_POWER 0x20
// ANT messages
#define ANT_UNASSIGN_CHANNEL 0x41
#define ANT_ASSIGN_CHANNEL 0x42
#define ANT_CHANNEL_ID 0x51
#define ANT_CHANNEL_PERIOD 0x43
#define ANT_SEARCH_TIMEOUT 0x44
#define ANT_CHANNEL_FREQUENCY 0x45
#define ANT_SET_NETWORK 0x46
#define ANT_TX_POWER 0x47
#define ANT_ID_LIST_ADD 0x59
#define ANT_ID_LIST_CONFIG 0x5A
#define ANT_CHANNEL_TX_POWER 0x60
#define ANT_LP_SEARCH_TIMEOUT 0x63
#define ANT_SET_SERIAL_NUMBER 0x65
#define ANT_ENABLE_EXT_MSGS 0x66
#define ANT_ENABLE_LED 0x68
#define ANT_SYSTEM_RESET 0x4A
#define ANT_OPEN_CHANNEL 0x4B
#define ANT_CLOSE_CHANNEL 0x4C
#define ANT_OPEN_RX_SCAN_CH 0x5B
#define ANT_REQ_MESSAGE 0x4D
#define ANT_BROADCAST_DATA 0x4E
#define ANT_ACK_DATA 0x4F
#define ANT_BURST_DATA 0x50
#define ANT_CHANNEL_EVENT 0x40
#define ANT_CHANNEL_STATUS 0x52
#define ANT_CHANNEL_ID 0x51
#define ANT_VERSION 0x3E
#define ANT_CAPABILITIES 0x54
#define ANT_SERIAL_NUMBER 0x61
#define ANT_CW_INIT 0x53
#define ANT_CW_TEST 0x48
#define ANT_UNASSIGN_CHANNEL 0x41
#define ANT_ASSIGN_CHANNEL 0x42
#define ANT_CHANNEL_ID 0x51
#define ANT_CHANNEL_PERIOD 0x43
#define ANT_SEARCH_TIMEOUT 0x44
#define ANT_CHANNEL_FREQUENCY 0x45
#define ANT_SET_NETWORK 0x46
#define ANT_TX_POWER 0x47
#define ANT_ID_LIST_ADD 0x59
#define ANT_ID_LIST_CONFIG 0x5A
#define ANT_CHANNEL_TX_POWER 0x60
#define ANT_LP_SEARCH_TIMEOUT 0x63
#define ANT_SET_SERIAL_NUMBER 0x65
#define ANT_ENABLE_EXT_MSGS 0x66
#define ANT_ENABLE_LED 0x68
#define ANT_SYSTEM_RESET 0x4A
#define ANT_OPEN_CHANNEL 0x4B
#define ANT_CLOSE_CHANNEL 0x4C
#define ANT_OPEN_RX_SCAN_CH 0x5B
#define ANT_REQ_MESSAGE 0x4D
#define ANT_BROADCAST_DATA 0x4E
#define ANT_ACK_DATA 0x4F
#define ANT_BURST_DATA 0x50
#define ANT_CHANNEL_EVENT 0x40
#define ANT_CHANNEL_STATUS 0x52
#define ANT_CHANNEL_ID 0x51
#define ANT_VERSION 0x3E
#define ANT_CAPABILITIES 0x54
#define ANT_SERIAL_NUMBER 0x61
#define ANT_CW_INIT 0x53
#define ANT_CW_TEST 0x48
// ANT message structure.
#define ANT_OFFSET_SYNC 0
#define ANT_OFFSET_LENGTH 1
#define ANT_OFFSET_ID 2
#define ANT_OFFSET_DATA 3
#define ANT_OFFSET_CHANNEL_NUMBER 3
#define ANT_OFFSET_MESSAGE_ID 4
#define ANT_OFFSET_MESSAGE_CODE 5
#define ANT_OFFSET_SYNC 0
#define ANT_OFFSET_LENGTH 1
#define ANT_OFFSET_ID 2
#define ANT_OFFSET_DATA 3
#define ANT_OFFSET_CHANNEL_NUMBER 3
#define ANT_OFFSET_MESSAGE_ID 4
#define ANT_OFFSET_MESSAGE_CODE 5
// other ANT stuff
#define ANT_SYNC_BYTE 0xA4
#define ANT_MAX_LENGTH 9
#define ANT_KEY_LENGTH 8
#define ANT_MAX_BURST_DATA 8
#define ANT_MAX_MESSAGE_SIZE 12
#define ANT_MAX_CHANNELS 4
#define ANT_SYNC_BYTE 0xA4
#define ANT_MAX_LENGTH 9
#define ANT_KEY_LENGTH 8
#define ANT_MAX_BURST_DATA 8
#define ANT_MAX_MESSAGE_SIZE 12
#define ANT_MAX_CHANNELS 4
// Channel messages
#define RESPONSE_NO_ERROR 0
@@ -303,12 +191,12 @@ typedef struct channel_manager {
#define ANT_SPORT_CADENCE_TYPE 0x7A
#define ANT_SPORT_SandC_TYPE 0x79
#define ANT_FAST_QUARQ_TYPE_WAS 11 // before release 1.8
#define ANT_FAST_QUARQ_TYPE 0x60
#define ANT_QUARQ_TYPE 0x60
#define ANT_FAST_QUARQ_TYPE 0x60
#define ANT_QUARQ_TYPE 0x60
#define ANT_SPORT_FREQUENCY 57
#define ANT_FAST_QUARQ_FREQUENCY 61
#define ANT_QUARQ_FREQUENCY 61
#define ANT_QUARQ_FREQUENCY 61
#define ANT_SPORT_CALIBRATION_MESSAGE 0x01
#define ANT_SPORT_AUTOZERO_OFF 0x00
@@ -340,85 +228,37 @@ public slots:
int quit(int error); // called by thread before exiting
// channel management
void initChannel();
bool discover(DeviceConfiguration *, QProgressDialog *); // confirm Server available at portSpec
void reinitChannel(QString _channel);
// get telemetry
RealtimeData getRealtimeData(); // return current realtime data
public:
// ANT Channel Management
void channel_manager_init(channel_manager_t *self);
int channel_manager_add_device(channel_manager_t *self, int device_number, int device_type, int channel_number);
int channel_manager_remove_device(channel_manager_t *self, int device_number, int channel_type);
ant_channel_t *channel_manager_find_device(channel_manager_t *self, int device_number, int channel_type);
int channel_manager_start_waiting_search(channel_manager_t *self);
void channel_manager_report(channel_manager_t *self);
void channel_manager_associate_control_channels(channel_manager_t *self);
// debug enums
enum { DEBUG_LEVEL_ERRORS=1,
DEBUG_LEVEL_ANT_CONNECTION=2,
DEBUG_LEVEL_ANT_MESSAGES=4,
DEBUG_LEVEL_CONFIG_PARSE=8
};
// ANT channel functions
void ant_channel_init(ant_channel_t *self, int number, struct channel_manager *parent);
const char *ant_channel_get_description(ant_channel_t *self);
int ant_channel_interpret_description(char *description);
int ant_channel_interpret_suffix(char c);
void ant_channel_open(ant_channel_t *self, int device_number, int channel_type);
void ant_channel_close(ant_channel_t *self);
static const unsigned char key[8];
static const ant_sensor_type_t ant_sensor_types[];
ANTChannel *antChannel[ANT_MAX_CHANNELS];
void ant_channel_receive_message(ant_channel_t *self, unsigned char *message);
void ant_channel_channel_event(ant_channel_t *self, unsigned char *message);
void ant_channel_burst_data(ant_channel_t *self, unsigned char *message);
void ant_channel_broadcast_event(ant_channel_t *self, unsigned char *message);
void ant_channel_ack_event(ant_channel_t *self, unsigned char *message);
void ant_channel_channel_id(ant_channel_t *self, unsigned char *message);
void ant_channel_request_calibrate(ant_channel_t *self);
void ant_channel_attempt_transition(ant_channel_t *self, int message_code);
void ant_channel_channel_info(ant_channel_t *self);
void ant_channel_drop_info(ant_channel_t *self);
void ant_channel_lost_info(ant_channel_t *self);
void ant_channel_stale_info(ant_channel_t *self);
void ant_channel_report_timeouts( void );
int ant_channel_set_timeout( char *type, float value, int connection );
void ant_channel_search_complete(void);
//void ant_channel_send_error( char * message ); // XXX missing in quarqd src (unused)
void ant_channel_set_id(ant_channel_t *self);
int ant_channel_is_searching(ant_channel_t *self);
void ant_channel_burst_init(ant_channel_t *self);
void ant_channel_send_cinqo_error(ant_channel_t *self);
void ant_channel_send_cinqo_success(ant_channel_t *self);
void ant_message_print_debug(unsigned char *);
// ANT Devices and Channels
int addDevice(int device_number, int device_type, int channel_number);
int removeDevice(int device_number, int channel_type);
ANTChannel *findDevice(int device_number, int channel_type);
int startWaitingSearch();
void report();
void associateControlChannels();
// device specific
void ant_channel_check_cinqo(ant_channel_t *self);
// XXX decide how to manage this
#if 0
float get_srm_offset(int);
void set_srm_offset(int, float);
#endif
// low-level ANT functions (from quarqd_dist/src/ant.c)
void ANT_ResetSystem(void);
void ANT_AssignChannel(const unsigned char channel, const unsigned char type, const unsigned char network);
void ANT_UnassignChannel(const unsigned char channel);
void ANT_SetChannelSearchTimeout(const unsigned char channel, const unsigned char search_timeout);
void ANT_RequestMessage(const unsigned char channel, const unsigned char request);
void ANT_SetChannelID(const unsigned char channel, const unsigned short device,
const unsigned char deviceType, const unsigned char txType);
void ANT_SetChannelPeriod(const unsigned char channel, const unsigned short period);
void ANT_SetChannelFreq(const unsigned char channel, const unsigned char frequency);
void ANT_SetNetworkKey(const unsigned char net, const unsigned char *key);
void ANT_SendAckMessage( void );
void ANT_SetAutoCalibrate(const unsigned char channel, const int autozero);
void ANT_RequestCalibrate(const unsigned char channel);
void ANT_Open(const unsigned char channel);
void ANT_Close(const unsigned char channel);
void ANT_SendMessage(void);
void ANT_Transmit(const unsigned char *msg, int length);
void ANT_ReceiveByte(unsigned char byte);
void ANT_HandleChannelEvent(void);
void ANT_ProcessMessage(void);
// transmission
void sendMessage(ANTMessage);
void receiveByte(unsigned char byte);
void handleChannelEvent(void);
void processMessage(void);
// serial i/o lifted from Computrainer.cpp
@@ -429,16 +269,30 @@ public:
int rawRead(uint8_t bytes[], int size);
int rawWrite(uint8_t *bytes, int size);
private:
RealtimeData telemetry;
double timestamp;
// channels update our telemetry
void setBPM(float x) {
telemetry.setHr(x);
}
void setCadence(float x) {
telemetry.setCadence(x);
}
void setWheelRpm(float x) {
telemetry.setWheelRpm(x);
telemetry.setSpeed(x * 2.1 * 60 / 1000); // XXX fixed wheel size needs fixing
}
void setWatts(float x) {
telemetry.setWatts(x);
}
private:
void run();
static int interpretSuffix(char c); // utility to convert e.g. 'c' to CHANNEL_TYPE_CADENCE
RealtimeData telemetry;
QMutex pvars; // lock/unlock access to telemetry data between thread and controller
int Status; // what status is the client in?
// thread process
void run();
// access to device file
QString deviceFilename;
int baud;
@@ -452,48 +306,19 @@ private:
// telemetry and state
QStringList antIDs;
long lastReadHR;
long lastReadWatts;
long lastReadCadence;
long lastReadWheelRpm;
long lastReadSpeed;
#if 0
QTime elapsedTime;
bool sentDual, sentSpeed, sentHR, sentCad, sentPWR;
#endif
// ANT low-level routines state and buffers
// from quarqd_dist/src/ant.c
unsigned char rxMessage[ANT_MAX_MESSAGE_SIZE];
unsigned char txMessage[ANT_MAX_MESSAGE_SIZE+10];
unsigned char txAckMessage[ANT_MAX_MESSAGE_SIZE];
static const unsigned char key[8];
static const ant_sensor_type_t ant_sensor_types[];
unsigned char rxMessage[ANT_MAX_MESSAGE_SIZE];
// state machine whilst receiving bytes
enum States {ST_WAIT_FOR_SYNC, ST_GET_LENGTH, ST_GET_MESSAGE_ID, ST_GET_DATA, ST_VALIDATE_PACKET};
enum States state;
int length;
int bytes;
int checksum;
enum States {ST_WAIT_FOR_SYNC, ST_GET_LENGTH, ST_GET_MESSAGE_ID, ST_GET_DATA, ST_VALIDATE_PACKET};
enum States state;
int length;
int bytes;
int checksum;
// debug enums
enum { DEBUG_LEVEL_ERRORS=1,
DEBUG_LEVEL_ANT_CONNECTION=2,
DEBUG_LEVEL_ANT_MESSAGES=4,
DEBUG_LEVEL_CONFIG_PARSE=8
};
enum {
CHANNEL_TYPE_UNUSED,
CHANNEL_TYPE_HR,
CHANNEL_TYPE_POWER,
CHANNEL_TYPE_SPEED,
CHANNEL_TYPE_CADENCE,
CHANNEL_TYPE_SandC,
CHANNEL_TYPE_QUARQ,
CHANNEL_TYPE_FAST_QUARQ,
CHANNEL_TYPE_FAST_QUARQ_NEW,
CHANNEL_TYPE_GUARD
};
};
#endif

703
src/ANTChannel.cpp Executable file
View File

@@ -0,0 +1,703 @@
/*
* Copyright (c) 2009 Mark Rages
* Copyright (c) 2011 Mark Liversedge (liversedge@gmail.com)
*
* This program is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
* Software Foundation; either version 2 of the License, or (at your option)
* any later version.
*
* This program is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc., 51
* Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "ANTChannel.h"
#include <QDebug>
static float timeout_blanking=2.0; // time before reporting stale data, seconds
static float timeout_drop=2.0; // time before reporting dropped message
static float timeout_scan=10.0; // time to do initial scan
static float timeout_lost=30.0; // time to do more thorough scan
ANTChannel::ANTChannel(int number, ANT *parent) : number(number), parent(parent)
{
channel_type=CHANNEL_TYPE_UNUSED;
channel_type_flags=0;
is_cinqo=0;
is_old_cinqo=0;
control_channel=NULL;
manufacturer_id=0;
product_id=0;
product_version=0;
device_number=0;
channel_assigned=0;
state=ANT_UNASSIGN_CHANNEL;
blanked=1;
messages_received=0;
messages_dropped=0;
setId();
srm_offset=0;
burstInit();
}
//
// channel id is in the form nnnnx where nnnn is the device number
// and x is the channel type (p)ower, (c) adence etc the full list
// can be found in ANT.cpp when initialising ant_sensor_types
//
void ANTChannel::setId()
{
if (channel_type==CHANNEL_TYPE_UNUSED) {
strcpy(id, "none");
} else {
snprintf(id, 10, "%d%c", device_number, parent->ant_sensor_types[channel_type].suffix);
}
}
// The description of the device
const char * ANTChannel::getDescription()
{
return parent->ant_sensor_types[channel_type].descriptive_name;
}
// Get device type
int ANTChannel::interpretDescription(char *description)
{
const ant_sensor_type_t *st=parent->ant_sensor_types;
do {
if (0==strcmp(st->descriptive_name,description))
return st->type;
} while (++st, st->type != CHANNEL_TYPE_GUARD);
return -1;
}
// Open an ant channel assignment.
void ANTChannel::open(int device, int chan_type)
{
channel_type=chan_type;
channel_type_flags = CHANNEL_TYPE_QUICK_SEARCH ;
device_number=device;
setId();
if (channel_assigned) {
parent->sendMessage(ANTMessage::unassignChannel(number));
} else {
attemptTransition(ANT_UNASSIGN_CHANNEL);
}
}
// close an ant channel assignment
void ANTChannel::close()
{
lostInfo();
lastMessage = ANTMessage();
parent->sendMessage(ANTMessage::close(number));
}
//
// The main read loop is in ANT.cpp, it will pass us
// the inbound message received for our channel.
// XXX fix this up to re-use ANTMessage for decoding
// all the inbound messages
//
void ANTChannel::receiveMessage(unsigned char *ant_message)
{
switch (ant_message[2]) {
case ANT_CHANNEL_EVENT:
channelEvent(ant_message);
break;
case ANT_BROADCAST_DATA:
broadcastEvent(ant_message);
break;
case ANT_ACK_DATA:
ackEvent(ant_message);
break;
case ANT_CHANNEL_ID:
channelId(ant_message);
break;
case ANT_BURST_DATA:
burstData(ant_message);
break;
default:
break; //XXX should trap error here, but silently ignored for now
}
if (get_timestamp() > blanking_timestamp + timeout_blanking) {
if (!blanked) {
blanked=1;
staleInfo(); //XXX does nothing for now....
}
} else blanked=0;
}
// static helper to conver message codes to an english string
// when outputing diagnostics for received messages
static const char *errormessage(unsigned char c)
{
switch (c) {
case 0 : return "No error";
case 1 : return "Search timeout";
case 2 : return "Message RX fail";
case 3 : return "Event TX";
case 4 : return "Receive TX fail";
case 5 : return "Ack or Burst completed";
case 6 : return "Event transfer TX failed";
case 7 : return "Channel closed success";
case 8 : return "dropped to search after missing too many messages.";
case 9 : return "Channel collision";
case 10 : return "Burst starts";
case 21 : return "Channel in wrong state";
case 22 : return "Channel not opened";
case 24 : return "Open without valid id";
case 25 : return "OpenRXScan when other channels open";
case 31 : return "Transmit whilst transfer in progress";
case 32 : return "Sequence number out of order";
case 33 : return "Burst message past sequence number not transmitted";
case 40 : return "INVALID PARAMETERS";
case 41 : return "INVALID NETWORK";
case 48 : return "ID out of bounds";
case 49 : return "Transmit during scan mode";
case 64 : return "NVM for SensRciore mode is full";
case 65 : return "NVM write failed";
default: return "UNKNOWN MESSAGE CODE";
}
}
// process a channel event message
// XXX should re-use ANTMessage rather than
// raw message data
void ANTChannel::channelEvent(unsigned char *ant_message) {
unsigned char *message=ant_message+2;
if (MESSAGE_IS_RESPONSE_NO_ERROR(message)) {
attemptTransition(RESPONSE_NO_ERROR_MESSAGE_ID(message));
} else if (MESSAGE_IS_EVENT_CHANNEL_CLOSED(message)) {
parent->sendMessage(ANTMessage::unassignChannel(number));
} else if (MESSAGE_IS_EVENT_RX_SEARCH_TIMEOUT(message)) {
// timeouts are normal for search channel
if (channel_type_flags & CHANNEL_TYPE_QUICK_SEARCH) {
channel_type_flags &= ~CHANNEL_TYPE_QUICK_SEARCH;
channel_type_flags |= CHANNEL_TYPE_WAITING;
} else {
lostInfo(); //XXX does nothing for now
channel_type=CHANNEL_TYPE_UNUSED;
channel_type_flags=0;
device_number=0;
setId();
parent->sendMessage(ANTMessage::unassignChannel(number));
}
//XXX channel_manager_start_waiting_search(self->parent);
} else if (MESSAGE_IS_EVENT_RX_FAIL(message)) {
messages_dropped++;
double t=get_timestamp();
if (t > (last_message_timestamp + timeout_drop)) {
if (channel_type != CHANNEL_TYPE_UNUSED) dropInfo();
// this is a hacky way to prevent the drop message from sending multiple times
last_message_timestamp+=2*timeout_drop;
}
} else if (MESSAGE_IS_EVENT_RX_ACKNOWLEDGED(message)) {
exit(-10);
} else if (MESSAGE_IS_EVENT_TRANSFER_TX_COMPLETED(message)) {
if (tx_ack_disposition) {} //XXX tx_ack_disposition();
} else {
// Not very friendly but useful for now!
fprintf(stderr, "WARNING type=%x channel=%x id=%x code=%s\n", *message, *(message+1), *(message+2), errormessage(*(message+3)));
}
}
// if this is a quarq cinqo then record that fact
// was probably interesting to quarqd, but less so for us!
void ANTChannel::checkCinqo()
{
int version_hi, version_lo, swab_version;
version_hi=(product_version >> 8) &0xff;
version_lo=(product_version & 0xff);
swab_version=version_lo | (version_hi<<8);
if (!(mi.first_time_manufacturer || mi.first_time_product)) {
if ((product_id == 1) && (manufacturer_id==7)) {
// we are a cinqo, were we aware of this?
is_cinqo=1;
// are we an old-version or new-version cinqo?
is_old_cinqo = ((version_hi <= 17) && (version_lo==10));
//XXX channel_manager_associate_control_channels(self->parent);
}
}
}
// notify we have a cinqo, does nothing
void ANTChannel::sendCinqoSuccess() {}
//
// We got a broadcast event -- this is where inbound
// telemetry gets processed, and for many message types
// we need to remember previous messages to look at the
// deltas during the period XXX this needs fixing!
//
void ANTChannel::broadcastEvent(unsigned char *ant_message)
{
unsigned char *message=ant_message+2;
double timestamp=get_timestamp();
messages_received++;
last_message_timestamp=timestamp;
if (state != MESSAGE_RECEIVED) {
// first message! who are we talking to?
parent->sendMessage(ANTMessage::requestMessage(number, ANT_CHANNEL_ID));
blanking_timestamp=get_timestamp();
blanked=0;
return; // because we can't associate a channel id with the message yet
}
// for automatically opening quarq channel on early cinqo
if (MESSAGE_IS_PRODUCT(message)) {
mi.first_time_product= false;
product_version&=0x00ff;
product_version|=(PRODUCT_SW_REV(message))<<8;
checkCinqo();
} else if (MESSAGE_IS_MANUFACTURER(message)) {
mi.first_time_manufacturer= false;
product_version&=0xff00;
product_version|=MANUFACTURER_HW_REV(message);
manufacturer_id=MANUFACTURER_MANUFACTURER_ID(message);
product_id=MANUFACTURER_MODEL_NUMBER_ID(message);
checkCinqo();
} else {
//
// We got some telemetry on this channel
//
ANTMessage antMessage(parent, ant_message);
if (lastMessage.type != 0) {
switch (channel_type) {
// Power
case CHANNEL_TYPE_POWER:
case CHANNEL_TYPE_QUARQ:
case CHANNEL_TYPE_FAST_QUARQ:
case CHANNEL_TYPE_FAST_QUARQ_NEW:
// what kind of power device is this?
switch(antMessage.power_type) {
//
// SRM - crank torque frequency
//
case ANT_CRANKSRM_POWER: // 0x20 - crank torque (SRM)
{
int period = antMessage.period - lastMessage.period;
int torque = antMessage.torque - lastMessage.torque;
float time = (float)period / (float)2000.00;
if (time && antMessage.slope && period) {
nullCount = 0;
float torque_freq = torque / time - 428/*srm_offset*/; //XXX need to support calibration
float nm_torque = 10.0 * torque_freq / antMessage.slope;
float cadence = 2000.0 * 60 * (antMessage.eventCount - lastMessage.eventCount) / period;
float power = 3.14159 * nm_torque * cadence / 30;
// ignore the occassional spikes XXX is this a boundary error on event count ?
if (power > 0 && power < 2501 && cadence >0 && cadence < 256) {
parent->setWatts(power);
parent->setCadence(cadence);
}
} else {
nullCount++;
if (nullCount >= 4) { // 4 messages on an SRM
parent->setWatts(0);
parent->setCadence(0);
}
}
}
break;
//
// Powertap - wheel torque
//
case ANT_WHEELTORQUE_POWER: // 0x11 - wheel torque (Powertap)
{
int events = antMessage.eventCount - lastMessage.eventCount;
int period = antMessage.period - lastMessage.period;
int torque = antMessage.torque - lastMessage.torque;
if (events && period) {
nullCount = 0;
float nm_torque = torque / (32.0 * events);
float wheelRPM = 2048.0 * 60.0 * events / period;
float power = 3.14159 * nm_torque * wheelRPM / 30;
parent->setWheelRpm(wheelRPM);
parent->setWatts(power);
} else {
nullCount++;
if (nullCount >= 4) { // 4 messages on Powertap ? XXX validate this
parent->setWheelRpm(0);
parent->setWatts(0);
}
}
}
break;
//
// Standard Power
//
case ANT_STANDARD_POWER: // 0x10 - standard power
{
int events = antMessage.eventCount - lastMessage.eventCount;
if (events) {
nullCount =0;
parent->setWatts(antMessage.instantPower);
} else {
nullCount++;
if (nullCount >= 4) { // 4 messages on Standard Power ? XXX validate this
parent->setWatts(0);
}
}
}
break;
//
// Quarq - Crank torque
//
case ANT_CRANKTORQUE_POWER: // 0x12 - crank torque (Quarq)
{
int events = antMessage.eventCount - lastMessage.eventCount;
int period = antMessage.period - lastMessage.period;
int torque = antMessage.torque - lastMessage.torque;
if (events && period) {
nullCount = 0;
float nm_torque = torque / (32.0 * events);
float cadence = 2048.0 * 60.0 * events / period;
float power = 3.14159 * nm_torque * cadence / 30;
parent->setCadence(cadence);
parent->setWatts(power);
} else {
nullCount++;
if (nullCount >= 4) { //XXX 4 on a quarq??? validate this
parent->setCadence(0);
parent->setWatts(0);
}
}
}
break;
default: // UNKNOWN POWER DEVICE? XXX Garmin (Metrigear) Vector????
break;
}
break;
// HR
case CHANNEL_TYPE_HR:
{
// cadence first...
int time = antMessage.measurementTime - lastMessage.measurementTime;
if (time) {
nullCount = 0;
parent->setBPM(antMessage.instantHeartrate);
} else {
nullCount++;
if (nullCount >= 12) parent->setBPM(0); // 12 according to the docs
}
}
break;
// Cadence
case CHANNEL_TYPE_CADENCE:
{
int time = antMessage.crankMeasurementTime - lastMessage.crankMeasurementTime;
int revs = antMessage.crankRevolutions - lastMessage.crankRevolutions;
if (time) {
float cadence = 1024*60*revs / time;
parent->setCadence(cadence);
}
}
break;
// Speed and Cadence
case CHANNEL_TYPE_SandC:
{
// cadence first...
int time = antMessage.crankMeasurementTime - lastMessage.crankMeasurementTime;
int revs = antMessage.crankRevolutions - lastMessage.crankRevolutions;
if (time) {
nullCount = 0;
float cadence = 1024*60*revs / time;
parent->setCadence(cadence);
} else {
nullCount++;
if (nullCount >= 12) parent->setCadence(0);
}
// now speed ...
time = antMessage.wheelMeasurementTime - lastMessage.wheelMeasurementTime;
revs = antMessage.wheelRevolutions - lastMessage.wheelRevolutions;
if (time) {
dualNullCount = 0;
float rpm = 1024*60*revs / time;
parent->setWheelRpm(rpm);
} else {
dualNullCount++;
if (dualNullCount >= 12) parent->setWheelRpm(0);
}
}
break;
// Speed
case CHANNEL_TYPE_SPEED:
{
int time = antMessage.wheelMeasurementTime - lastMessage.wheelMeasurementTime;
int revs = antMessage.wheelRevolutions - lastMessage.wheelRevolutions;
if (time) {
nullCount=0;
float rpm = 1024*60*revs / time;
parent->setWheelRpm(rpm);
} else {
nullCount++;
if (nullCount >= 12) parent->setWheelRpm(0);
}
}
break;
default:
break; // unknown?
}
} else {
// reset nullCount if receiving first telemetry update
dualNullCount = nullCount = 0;
}
lastMessage = antMessage;
}
}
// we got an acknowledgement, so lets process it
// does nothing for now
void ANTChannel::ackEvent(unsigned char * /*ant_message*/) { }
// we got a channel ID notification
void ANTChannel::channelId(unsigned char *ant_message) {
unsigned char *message=ant_message+2;
device_number=CHANNEL_ID_DEVICE_NUMBER(message);
device_id=CHANNEL_ID_DEVICE_TYPE_ID(message);
state=MESSAGE_RECEIVED;
setId();
channelInfo();
// if we were searching,
if (channel_type_flags & CHANNEL_TYPE_QUICK_SEARCH) {
parent->sendMessage(ANTMessage::setSearchTimeout(number, (int)(timeout_lost/2.5)));
}
channel_type_flags &= ~CHANNEL_TYPE_QUICK_SEARCH;
//XXX channel_manager_start_waiting_search(self->parent);
// if we are quarq channel, hook up with the ant+ channel we are connected to
//XXX channel_manager_associate_control_channels(self->parent);
}
// get ready to burst
void ANTChannel::burstInit() {
rx_burst_data_index=0;
rx_burst_next_sequence=0;
rx_burst_disposition=NULL;
}
// are we in the middle of a search?
int ANTChannel::isSearching() {
return ((channel_type_flags & (CHANNEL_TYPE_WAITING | CHANNEL_TYPE_QUICK_SEARCH)) || (state != MESSAGE_RECEIVED));
}
// receive burst data
void ANTChannel::burstData(unsigned char *ant_message) {
unsigned char *message=ant_message+2;
char seq=(message[1]>>5)&0x3;
char last=(message[1]>>7)&0x1;
const unsigned char next_sequence[4]={1,2,3,1};
if (seq!=rx_burst_next_sequence) {
// XXX handle errors
} else {
int len=ant_message[ANT_OFFSET_LENGTH]-3;
if ((rx_burst_data_index + len)>(RX_BURST_DATA_LEN)) {
len = RX_BURST_DATA_LEN-rx_burst_data_index;
}
rx_burst_next_sequence=next_sequence[(int)seq];
memcpy(rx_burst_data+rx_burst_data_index, message+2, len);
rx_burst_data_index+=len;
}
if (last) {
if (rx_burst_disposition) {
//XXX what does this do? rx_burst_disposition();
}
burstInit();
}
}
void ANTChannel::attemptTransition(int message_id)
{
const ant_sensor_type_t *st;
int previous_state=state;
st=&(parent->ant_sensor_types[channel_type]);
// update state
state=message_id;
// do transitions
switch (state) {
case ANT_CLOSE_CHANNEL:
//qDebug()<<"transition ... close channel...";
// next step is unassign and start over
// but we must wait until event_channel_closed
// which is its own channel event
state=MESSAGE_RECEIVED;
break;
case ANT_UNASSIGN_CHANNEL:
//qDebug()<<"transition ... unassign channel...";
channel_assigned=0;
if (st->type==CHANNEL_TYPE_UNUSED) {
// we're shutting the channel down
} else {
device_id=st->device_id;
if (channel_type & CHANNEL_TYPE_PAIR) {
device_id |= 0x80;
}
setId();
//qDebug()<<"st network is"<<st->network;
parent->sendMessage(ANTMessage::assignChannel(number, 0, st->network)); // recieve channel on network 1
}
break;
case ANT_ASSIGN_CHANNEL:
//qDebug()<<"transition ... assign channel...";
channel_assigned=1;
parent->sendMessage(ANTMessage::setChannelID(number, device_number, device_id, 0));
break;
case ANT_CHANNEL_ID:
//qDebug()<<"transition ... channel id...";
if (channel_type & CHANNEL_TYPE_QUICK_SEARCH) {
parent->sendMessage(ANTMessage::setSearchTimeout(number, (int)(timeout_scan/2.5)));
} else {
parent->sendMessage(ANTMessage::setSearchTimeout(number, (int)(timeout_lost/2.5)));
}
break;
case ANT_SEARCH_TIMEOUT:
//qDebug()<<"transition ... search timeout...";
if (previous_state==ANT_CHANNEL_ID) {
// continue down the intialization chain
parent->sendMessage(ANTMessage::setChannelPeriod(number, st->period));
} else {
// we are setting the ant_search timeout after connected
// we'll just pretend this never happened
state=previous_state;
}
break;
case ANT_CHANNEL_PERIOD:
//qDebug()<<"transition ... channel period...";
parent->sendMessage(ANTMessage::setChannelFreq(number, st->frequency));
break;
case ANT_CHANNEL_FREQUENCY:
//qDebug()<<"transition ... channel frequency...";
parent->sendMessage(ANTMessage::open(number));
mi.initialise();
break;
case ANT_OPEN_CHANNEL:
//qDebug()<<"transition ... open channel...";
break;
default:
break;
}
}
// refactored out XXX fix this
int ANTChannel::setTimeout(char * /*type*/, float /*value*/, int /*connection*/) { return 0; }
// These should emit signals to notify the channel manager
// but for now we just ignore XXX fix this
void ANTChannel::searchComplete() {}
void ANTChannel::reportTimeouts() {}
void ANTChannel::staleInfo() {}
void ANTChannel::lostInfo() {}
void ANTChannel::dropInfo() {}
void ANTChannel::channelInfo() {}
//
// Calibrate... XXX not used at present
//
// request the device on this channel calibrates itselt
void ANTChannel::requestCalibrate() {
parent->sendMessage(ANTMessage::requestCalibrate(number));
}

172
src/ANTChannel.h Executable file
View File

@@ -0,0 +1,172 @@
/*
* Copyright (c) 2009 Mark Rages
* Copyright (c) 2011 Mark Liversedge (liversedge@gmail.com)
*
* This program is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
* Software Foundation; either version 2 of the License, or (at your option)
* any later version.
*
* This program is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc., 51
* Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
#ifndef gc_ANTChannel_h
#define gc_ANTChannel_h
#include "ANT.h"
#include "ANTMessage.h"
#define CHANNEL_TYPE_QUICK_SEARCH 0x10 // or'ed with current channel type
/* after fast search, wait for slow search. Otherwise, starting slow
search might postpone the fast search on another channel. */
#define CHANNEL_TYPE_WAITING 0x20
#define CHANNEL_TYPE_PAIR 0x40 // to do an Ant pair
#define MESSAGE_RECEIVED -1
// note: this struct is from quarqd_dist/quarqd/src/generated-headers.h
class ANTChannelInitialisation {
public:
ANTChannelInitialisation() {
initialise();
}
void initialise() {
first_time_crank_torque=
first_time_crank_SRM=
first_time_wheel_torque=
first_time_standard_power=
first_time_torque_support=
first_time_calibration_pass=
first_time_calibration_fail=
first_time_heart_rate=
first_time_speed=
first_time_cadence=
first_time_speed_cadence=
first_time_manufacturer=
first_time_product=
first_time_battery_voltage= true;
}
bool first_time_crank_torque;
bool first_time_crank_SRM;
bool first_time_wheel_torque;
bool first_time_standard_power;
bool first_time_torque_support;
bool first_time_calibration_pass;
bool first_time_calibration_fail;
bool first_time_heart_rate;
bool first_time_speed;
bool first_time_cadence;
bool first_time_speed_cadence;
bool first_time_manufacturer;
bool first_time_product;
bool first_time_battery_voltage;
};
class ANTChannel {
public:
enum channeltype {
CHANNEL_TYPE_UNUSED,
CHANNEL_TYPE_HR,
CHANNEL_TYPE_POWER,
CHANNEL_TYPE_SPEED,
CHANNEL_TYPE_CADENCE,
CHANNEL_TYPE_SandC,
CHANNEL_TYPE_QUARQ,
CHANNEL_TYPE_FAST_QUARQ,
CHANNEL_TYPE_FAST_QUARQ_NEW,
CHANNEL_TYPE_GUARD
};
typedef enum channeltype ChannelType;
// Channel Information - to save tedious set/getters made public
int number; // Channel number within Ant chip
int state;
int channel_type;
int device_number;
int channel_type_flags;
int device_id;
bool is_cinqo; // bool
bool is_old_cinqo; // bool, set for cinqo needing separate control channel
int search_type;
int srm_offset;
ANTChannel *control_channel;
ANTChannel(int number, ANT *parent);
// What kind of channel
const char *getDescription();
int interpretDescription(char *description);
// channel open/close
void open(int device_number, int channel_type);
void close();
// handle inbound data
void receiveMessage(unsigned char *message);
void channelEvent(unsigned char *message);
void burstInit();
void burstData(unsigned char *message);
void broadcastEvent(unsigned char *message);
void ackEvent(unsigned char *message);
void channelId(unsigned char *message);
void setId();
void requestCalibrate();
void attemptTransition(int message_code);
int setTimeout(char *type, float value, int connection);
// Should be signals ? XXX
void channelInfo();
void dropInfo();
void lostInfo();
void staleInfo();
void reportTimeouts();
void searchComplete();
// search
int isSearching();
// Cinqo support
void sendCinqoError();
void sendCinqoSuccess();
void checkCinqo();
private:
ANT *parent;
ANTMessage lastMessage;
int dualNullCount, nullCount;
double last_message_timestamp;
double blanking_timestamp;
int blanked;
char id[10]; // short identifier
bool channel_assigned;
ANTChannelInitialisation mi;
int messages_received; // for signal strength metric
int messages_dropped;
unsigned char rx_burst_data[RX_BURST_DATA_LEN];
int rx_burst_data_index;
unsigned char rx_burst_next_sequence;
void (*rx_burst_disposition)(struct ant_channel *);
void (*tx_ack_disposition)(struct ant_channel *);
// what we got
int manufacturer_id;
int product_id;
int product_version;
};
#endif

516
src/ANTMessage.cpp Executable file
View File

@@ -0,0 +1,516 @@
/*
* Copyright (c) 2011 Mark Liversedge (liversedge@gmail.com)
* Copyright (c) 2009 Mark Rages (Quarq)
*
* This program is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
* Software Foundation; either version 2 of the License, or (at your option)
* any later version.
*
* This program is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc., 51
* Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "ANTMessage.h"
#include <QDebug>
//
// This ANTMessage class is to decode and Encode ANT Messages
//
// DECODING
// To decode a message a pointer to unsigned char passes the message
// data and the contents are decoded into the class variables.
//
//
// ENCODING
// To encode a message 8 bytes of message data are passed and a checksum
// is calculataed.
//
// Since many of the message types do not require all data to be encoded
// there are a number of static convenience methods that take the message
// variables and call the underlying constructor to construct the message.
//
// MESSAGE STRUCTURE
// An ANT message is very simple, it is a maximum of 13 bytes of data in
// the following structure:
//
// Byte 0 : Sync byte (always of value 0x44)
// Byte 1 : Message length (between 1 and 9)
// Byte 2 : Message ID (Type) (between 1 and 255, 0 is invalid)
// Byte 3-10 : Data (variable length)
// Last Byte : Checksum byte (XOR of message bytes including sync)
//
// Depending upon the message id (type) the payload in bytes 3-7 vary. The
// different message types and formats can be found at thisisant.com and are
// also available here: http://www.sparkfun.com/datasheets/Wireless/Nordic/ANT-UserGuide.pdf
//
// The majority of the decode/encode routines have been developed from the quarqd sources.
//
// It is important to note that the ANT+ sports protocol is built on top of the ANT
// message format and the ANT+ message formats can be obtained from thisisant.com
// by registering as an ANT adoptor see: http://www.thisisant.com/pages/ant/ant-adopter-zone
//
//======================================================================
// ANT MESSAGE FORMATS
//======================================================================
/*
* Message Type Message Id ...
* ------------ ---------------------------------------------------------------------
* assign_channel 0x42,channel,channel_type,network_number
* unassign_channel 0x41,channel
* open_channel 0x4b,channel
* channel_id 0x51,channel,uint16_le:device_number,device_type_id,transmission_type
* burst_message 0x50,chan_seq,data0,data1,data2,data3,data4,data5,data6,data7
* burst_message7 0x50,chan_seq,data0,data1,data2,data3,data4,data5,data6
* burst_message6 0x50,chan_seq,data0,data1,data2,data3,data4,data5
* burst_message5 0x50,chan_seq,data0,data1,data2,data3,data4
* burst_message4 0x50,chan_seq,data0,data1,data2,data3
* burst_message3 0x50,chan_seq,data0,data1,data2
* burst_message2 0x50,chan_seq,data0,data1
* burst_message1 0x50,chan_seq,data0
* burst_message0 0x50,chan_seq
* channel_period 0x43,channel,uint16_le:period
* search_timeout 0x44,channel,search_timeout
* channel_frequency 0x45,channel,rf_frequency
* set_network 0x46,channel,key0,key1,key2,key3,key4,key5,key6,key7
* transmit_power 0x47,0,tx_power
* reset_system 0x4a,None
* request_message 0x4d,channel,message_requested
* close_channel 0x4c,channel
* response_no_error 0x40,channel,message_id,0x00
* response_assign_channel_ok 0x40,channel,0x42,0x00
* response_channel_unassign_ok 0x40,channel,0x41,0x00
* response_open_channel_ok 0x40,channel,0x4b,0x00
* response_channel_id_ok 0x40,channel,0x51,0x00
* response_channel_period_ok 0x40,channel,0x43,0x00
* response_channel_frequency_ok 0x40,channel,0x45,0x00
* response_set_network_ok 0x40,channel,0x46,0x00
* response_transmit_power_ok 0x40,channel,0x47,0x00
* response_close_channel_ok 0x40,channel,0x4c,0x00
* response_search_timeout_ok 0x40,channel,0x44,0x00
* event_rx_search_timeout 0x40,channel,message_id,0x01
* event_rx_fail 0x40,channel,message_id,0x02
* event_tx 0x40,channel,message_id,0x03
* event_transfer_rx_failed 0x40,channel,message_id,0x04
* event_transfer_tx_completed 0x40,channel,message_id,0x05
* event_transfer_tx_failed 0x40,channel,message_id,0x06
* event_channel_closed 0x40,channel,message_id,0x07
* event_rx_fail_go_to_search 0x40,channel,message_id,0x08
* event_channel_collision 0x40,channel,message_id,0x09
* event_transfer_tx_start 0x40,channel,message_id,0x0A
# event_rx_broadcast 0x40,channel,message_id,0x0A
* event_rx_acknowledged 0x40,channel,message_id,0x0B
* event_rx_burst_packet 0x40,channel,message_id,0x0C
* channel_in_wrong_state 0x40,channel,message_id,0x15
* channel_not_opened 0x40,channel,message_id,0x16
* channel_id_not_set 0x40,channel,message_id,0x18
* transfer_in_progress 0x40,channel,message_id,31
* channel_status 0x52,channel,status
* cw_init 0x53,None
* cw_test 0x48,None,power,freq
* capabilities 0x54,max_channels,max_networks,standard_options,advanced_options
* capabilities_extended 0x54,max_channels,max_networks,standard_options,advanced_options,
* advanced_options_2,max_data_channels
* ant_version 0x3e,data0,data1,data2,data3,data4,data5,data6,data7,data8
* ant_version_long 0x3e,data0,data1,data2,data3,data4,data5,data6,data7,data8,data9,
* data10,data11,data12
* transfer_seq_number_error 0x40,channel,message_id,0x20
* invalid_message 0x40,channel,message_id,0x28
* invalid_network_number 0x40,channel,message_id,0x29
* channel_response 0x40,channel,message_id,message_code
* extended_broadcast_data 0x5d,channel,uint16le:device_number,device_type,transmission_type,
* data0,data1,data2,data3,data4,data5,data6,data7
* extended_ack_data 0x5e,channel,uint16le:device_number,device_type,transmission_type,
* data0,data1,data2,data3,data4,data5,data6,data7
* extended_burst_data 0x5f,channel,uint16le:device_number,device_type,transmission_type,
* data0,data1,data2,data3,data4,data5,data6,data7
* startup_message 0x6f,start_message
*/
//======================================================================
// ANT SPORT MESSAGE FORMATS
//======================================================================
/* The channel type the message was received upon generally indicates which kind of broadcast message
* we are dealing with (4e is an ANT broadcast message). So in the ANTMessage constructor we also
* pass the channel type to help decide how to decode. See interpret_xxx_broadcast() class members
*
* Channel Message
* type Type Message ID ...
* --------- ----------- --------------------------------------------------------------------------
* heartrate heart_rate 0x4e,channel,None,None,None,None,uint16_le_diff:measurement_time,
* uint8_diff:beats,uint8:instant_heart_rate
*
* speed speed 0x4e,channel,None,None,None,None,uint16_le_diff:measurement_time,
* uint16_le_diff:wheel_revs
*
* cadence cadence 0x4e,channel,None,None,None,None,uint16_le_diff:measurement_time,
* uint16_le_diff:crank_revs
*
* speed_cadence speed_cadence 0x4e,channel,uint16_le_diff:cadence_measurement_time,uint16_le_diff:crank_revs,
* uint16_le_diff:speed_measurement_time,uint16_le_diff:wheel_revs
*
* power calibration_request None,channel,0x01,0xAA,None,None,None,None,None,None
* power srm_zero_response None,channel,0x01,0x10,0x01,None,None,None,uint16_be:offset
* power calibration_pass None,channel,0x01,0xAC,uint8:autozero_status,None,None,None,uint16_le:calibration_data
* power calibration_fail None,channel,0x01,0xAF,uint8:autozero_status,None,None,None,uint16_le:calibration_data
* power torque_support None,channel,0x01,0x12,uint8:sensor_configuration,sint16_le:raw_torque,
* sint16_le:offset_torque,None
* power standard_power 0x4e,channel,0x10,uint8_diff:event_count,None,uint8:instant_cadence,
* uint16_le_diff:sum_power,uint16_le:instant_power
* power wheel_torque 0x4e,channel,0x11,uint8_diff:event_count,uint8:wheel_rev,uint8:instant_cadence,
* uint16_le_diff:wheel_period,uint16_le_diff:torque
* power crank_torque 0x4e,channel,0x12,uint8_diff:event_count,uint8:crank_rev,uint8:instant_cadence,
* uint16_le_diff:crank_period,uint16_le_diff:torque
* power crank_SRM 0x4e,channel,0x20,uint8_diff:event_count,uint16_be:slope,uint16_be_diff:crank_period,
* uint16_be_diff:torque
*
* any manufacturer 0x4e,channel,0x50,None,None,hw_rev,uint16_le:manufacturer_id,uint16_le:model_number_id
* any product 0x4e,channel,0x51,None,None,sw_rev,uint16_le:serial_number_qpod,
* uint16_le:serial_number_spider
* any battery_voltage 0x4e,channel,0x52,None,None,operating_time_lsb,operating_time_1sb,
* operating_time_msb,voltage_lsb,descriptive_bits
*/
// construct a null message
ANTMessage::ANTMessage(void) {
init();
}
// construct an ant message based upon a message structure
// the message structure must include the sync byte
ANTMessage::ANTMessage(ANT *parent, const unsigned char *message) {
// Initialise all fields to invalid
init();
// standard fields
sync = message[0];
length = message[1];
type = message[2];
// snaffle away the data
memcpy(data, message, ANT_MAX_MESSAGE_SIZE);
// now lets decode!
switch(type) {
case ANT_UNASSIGN_CHANNEL:
channel = message[3];
break;
case ANT_ASSIGN_CHANNEL:
channel = message[3];
channelType = message[4];
networkNumber = message[5];
break;
case ANT_CHANNEL_ID:
channel = message[3];
deviceNumber = message[4] + (message[5]<<8);
deviceType = message[6];
transmissionType = message[7];
break;
case ANT_CHANNEL_PERIOD:
channel = message[3];
channelPeriod = message[4] + (message[5]<<8);
break;
case ANT_SEARCH_TIMEOUT:
channel = message[3];
searchTimeout = message[4];
break;
case ANT_CHANNEL_FREQUENCY:
channel = message[3];
frequency = message[4];
break;
case ANT_SET_NETWORK:
channel = message[3];
key[0] = message[4];
key[1] = message[5];
key[2] = message[6];
key[3] = message[7];
key[4] = message[8];
key[5] = message[9];
key[6] = message[10];
key[7] = message[11];
break;
case ANT_TX_POWER:
transmitPower = message[4];
break;
case ANT_ID_LIST_ADD:
break;
case ANT_ID_LIST_CONFIG:
break;
case ANT_CHANNEL_TX_POWER:
break;
case ANT_LP_SEARCH_TIMEOUT:
break;
case ANT_SET_SERIAL_NUMBER:
break;
case ANT_ENABLE_EXT_MSGS:
break;
case ANT_ENABLE_LED:
break;
case ANT_SYSTEM_RESET:
break; // nothing to do, this is ok
case ANT_OPEN_CHANNEL:
channel = message[3];
break;
case ANT_CLOSE_CHANNEL:
break;
case ANT_OPEN_RX_SCAN_CH:
break;
case ANT_REQ_MESSAGE:
break;
//
// Telemetry received from device, this is channel type
// dependant so we examine based upon the channel configuration
//
case ANT_BROADCAST_DATA:
// we need to handle ant sport messages here
switch(parent->antChannel[message[3]]->channel_type) {
// Heartrate is fairly simple
case ANTChannel::CHANNEL_TYPE_HR:
channel = message[3];
measurementTime = message[8] + (message[9]<<8);
heartrateBeats = message[10];
instantHeartrate = message[11];
break;
/*
* these are not supported at present! XXX
* power calibration_request None,channel,0x01,0xAA,None,None,None,None,None,None
* power srm_zero_response None,channel,0x01,0x10,0x01,None,None,None,uint16_be:offset
* power calibration_pass None,channel,0x01,0xAC,uint8:autozero_status,None,None,None,uint16_le:calibration_data
* power calibration_fail None,channel,0x01,0xAF,uint8:autozero_status,None,None,None,uint16_le:calibration_data
* power torque_support None,channel,0x01,0x12,uint8:sensor_configuration,sint16_le:raw_torque,
* sint16_le:offset_torque,None
*/
case ANTChannel::CHANNEL_TYPE_POWER:
case ANTChannel::CHANNEL_TYPE_QUARQ:
case ANTChannel::CHANNEL_TYPE_FAST_QUARQ:
case ANTChannel::CHANNEL_TYPE_FAST_QUARQ_NEW:
channel = message[3];
power_type = message[4];
switch (power_type) {
case ANT_STANDARD_POWER: // 0x10 - standard power
eventCount = message[5];
instantCadence = message[6];
sumPower = message[7] + (message[8]<<8);
instantPower = message[9] + (message[10]<<8);
break;
case ANT_WHEELTORQUE_POWER: // 0x11 - wheel torque (Powertap)
eventCount = message[5];
wheelRevolutions = message[6];
instantCadence = message[7];
period = message[8] + (message[9]<<8);
torque = message[10] + (message[11]<<8);
break;
case ANT_CRANKTORQUE_POWER: // 0x12 - crank torque (Quarq)
eventCount = message[5];
crankRevolutions = message[6];
instantCadence = message[7];
period = message[8] + (message[9]<<8);
torque = message[10] + (message[11]<<8);
break;
case ANT_CRANKSRM_POWER: // 0x20 - crank torque (SRM)
eventCount = message[5];
slope = message[7] + (message[6]<<8); // yes it is bigendian
period = message[9] + (message[8]<<8); // yes it is bigendian
torque = message[11] + (message[10]<<8); // yes it is bigendian
break;
default: // UNKNOWN
break;
}
break;
case ANTChannel::CHANNEL_TYPE_SPEED:
channel = message[3];
wheelMeasurementTime = message[8] + (message[9]<<8);
wheelRevolutions = message[10] + (message[11]<<8);
break;
case ANTChannel::CHANNEL_TYPE_CADENCE:
channel = message[3];
crankMeasurementTime = message[8] + (message[9]<<8);
crankRevolutions = message[10] + (message[11]<<8);
break;
case ANTChannel::CHANNEL_TYPE_SandC:
channel = message[3];
crankMeasurementTime = message[4] + (message[5]<<8);
crankRevolutions = message[6] + (message[7]<<8);
wheelMeasurementTime = message[8] + (message[9]<<8);
wheelRevolutions = message[10] + (message[11]<<8);
break;
default:
break;
}
break;
case ANT_ACK_DATA:
break;
case ANT_BURST_DATA:
break;
case ANT_CHANNEL_EVENT:
break;
case ANT_CHANNEL_STATUS:
break;
case ANT_VERSION:
break;
case ANT_CAPABILITIES:
break;
case ANT_SERIAL_NUMBER:
break;
case ANT_CW_INIT:
break;
case ANT_CW_TEST:
break;
default:
break; // shouldn't get here!
}
}
// construct a message with all data passed (except sync and checksum)
ANTMessage::ANTMessage(const unsigned char len,
const unsigned char type,
const unsigned char b3,
const unsigned char b4,
const unsigned char b5,
const unsigned char b6,
const unsigned char b7,
const unsigned char b8,
const unsigned char b9,
const unsigned char b10,
const unsigned char b11)
{
timestamp = get_timestamp();
// encode the message
data[0] = ANT_SYNC_BYTE;
data[1] = len; // message payload length
data[2] = type; // message type
data[3] = b3;
data[4] = b4;
data[5] = b5;
data[6] = b6;
data[7] = b7;
data[8] = b8;
data[9] = b9;
data[10] = b10;
data[11] = b11;
// compute the checksum and place after data
unsigned char crc = 0;
int i=0;
for (; i< (len+3); i++) crc ^= data[i];
data[i] = crc;
length = i+1;
}
void ANTMessage::init()
{
timestamp = get_timestamp();
power_type = frequency = deviceType = transmitPower = searchTimeout = 0;
transmissionType = networkNumber = channelType = channel = 0;
channelPeriod = deviceNumber = 0;
wheelMeasurementTime = crankMeasurementTime = measurementTime = 0;
instantHeartrate = heartrateBeats = 0;
eventCount = 0;
wheelRevolutions = crankRevolutions = 0;
slope = period = torque = 0;
sync = length = type = 0;
}
ANTMessage ANTMessage::resetSystem()
{
return ANTMessage(1, ANT_SYSTEM_RESET);
}
ANTMessage ANTMessage::assignChannel(const unsigned char channel,
const unsigned char type,
const unsigned char network)
{
return ANTMessage(3, ANT_ASSIGN_CHANNEL, channel, type, network);
}
ANTMessage ANTMessage::unassignChannel(const unsigned char channel)
{
return ANTMessage(1, ANT_UNASSIGN_CHANNEL, channel);
}
ANTMessage ANTMessage::setSearchTimeout(const unsigned char channel,
const unsigned char timeout)
{
return ANTMessage(2, ANT_SEARCH_TIMEOUT, channel, timeout);
}
ANTMessage ANTMessage::requestMessage(const unsigned char channel,
const unsigned char request)
{
return ANTMessage(2, ANT_REQ_MESSAGE, channel, request);
}
ANTMessage ANTMessage::setChannelID(const unsigned char channel,
const unsigned short device,
const unsigned char devicetype,
const unsigned char txtype)
{
return ANTMessage(5, ANT_CHANNEL_ID, channel, device&0xff, (device>>8)&0xff, devicetype, txtype);
}
ANTMessage ANTMessage::setChannelPeriod(const unsigned char channel,
const unsigned short period)
{
return ANTMessage(3, ANT_CHANNEL_PERIOD, channel, period&0xff, (period>>8)&0xff);
}
ANTMessage ANTMessage::setChannelFreq(const unsigned char channel,
const unsigned char frequency)
{
return ANTMessage(2, ANT_CHANNEL_FREQUENCY, channel, frequency);
}
ANTMessage ANTMessage::setNetworkKey(const unsigned char net,
const unsigned char *key)
{
return ANTMessage(9, ANT_SET_NETWORK, net, key[0], key[1], key[2], key[3], key[4], key[5], key[6], key[7]);
}
ANTMessage ANTMessage::setAutoCalibrate(const unsigned char channel,
bool autozero)
{
return ANTMessage(4, ANT_ACK_DATA, channel, ANT_SPORT_CALIBRATION_MESSAGE,
ANT_SPORT_CALIBRATION_REQUEST_AUTOZERO_CONFIG,
autozero ? ANT_SPORT_AUTOZERO_ON : ANT_SPORT_AUTOZERO_OFF);
}
ANTMessage ANTMessage::requestCalibrate(const unsigned char channel)
{
return ANTMessage(4, ANT_ACK_DATA, channel, ANT_SPORT_CALIBRATION_MESSAGE,
ANT_SPORT_CALIBRATION_REQUEST_MANUALZERO);
}
ANTMessage ANTMessage::open(const unsigned char channel)
{
return ANTMessage(1, ANT_OPEN_CHANNEL, channel);
}
ANTMessage ANTMessage::close(const unsigned char channel)
{
return ANTMessage(1, ANT_CLOSE_CHANNEL, channel);
}

102
src/ANTMessage.h Executable file
View File

@@ -0,0 +1,102 @@
/*
* Copyright (c) 2009 Mark Rages
* Copyright (c) 2011 Mark Liversedge (liversedge@gmail.com)
*
* This program is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
* Software Foundation; either version 2 of the License, or (at your option)
* any later version.
*
* This program is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc., 51
* Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "ANT.h"
#include "ANTChannel.h"
#ifndef gc_ANTMessage_h
#define gc_ANTMessage_h
class ANTMessage {
public:
ANTMessage(); // null message
ANTMessage(ANT *parent, const unsigned char *data); // decode from parameters
ANTMessage(unsigned char b1,
unsigned char b2 = '\0',
unsigned char b3 = '\0',
unsigned char b4 = '\0',
unsigned char b5 = '\0',
unsigned char b6 = '\0',
unsigned char b7 = '\0',
unsigned char b8 = '\0',
unsigned char b9 = '\0',
unsigned char b10 = '\0',
unsigned char b11 = '\0'); // encode with values (at least one value must be passed though)
// convenience functions for encoding messages
static ANTMessage resetSystem();
static ANTMessage assignChannel(const unsigned char channel,
const unsigned char type,
const unsigned char network);
static ANTMessage unassignChannel(const unsigned char channel);
static ANTMessage setSearchTimeout(const unsigned char channel,
const unsigned char timeout);
static ANTMessage requestMessage(const unsigned char channel,
const unsigned char request);
static ANTMessage setChannelID(const unsigned char channel,
const unsigned short device,
const unsigned char devicetype,
const unsigned char txtype);
static ANTMessage setChannelPeriod(const unsigned char channel,
const unsigned short period);
static ANTMessage setChannelFreq(const unsigned char channel,
const unsigned char frequency);
static ANTMessage setNetworkKey(const unsigned char net,
const unsigned char *key);
static ANTMessage setAutoCalibrate(const unsigned char channel,
bool autozero);
static ANTMessage requestCalibrate(const unsigned char channel);
static ANTMessage open(const unsigned char channel);
static ANTMessage close(const unsigned char channel);
// to avoid a myriad of tedious set/getters the data fields
// are plain public members. This is unlikely to change in the
// future since the whole purpose of this class is to encode
// and decode ANT messages into the member variables below.
// BEAR IN MIND THAT ONLY A HANDFUL OF THE VARS WILL BE SET
// DURING DECODING, SO YOU *MUST* EXAMINE THE MESSAGE TYPE
// TO DECIDE WHICH FIELDS TO REFERENCE
// Standard ANT values
int length;
unsigned char data[ANT_MAX_MESSAGE_SIZE+1]; // include sync byte at front
double timestamp;
uint8_t sync, type;
uint8_t transmitPower, searchTimeout, transmissionType, networkNumber,
channelType, channel, deviceType, frequency;
uint16_t channelPeriod, deviceNumber;
uint8_t key[8];
// ANT Sport values
uint8_t power_type;
uint8_t eventCount;
uint16_t measurementTime, wheelMeasurementTime, crankMeasurementTime;
uint8_t heartrateBeats, instantHeartrate; // heartrate
uint16_t slope, period, torque; // power
uint16_t sumPower, instantPower; // power
uint16_t wheelRevolutions, crankRevolutions; // speed and power and cadence
uint8_t instantCadence; // cadence
private:
void init();
};
#endif

1
src/ANTlocalController.cpp
View File

@@ -1,4 +1,5 @@
/*
* Copyright (c) 2009 Mark Rages
* Copyright (c) 2009 Mark Liversedge (liversedge@gmail.com)
*
* This program is free software; you can redistribute it and/or modify it

4
src/src.pro
View File

@@ -126,6 +126,8 @@ HEADERS += \
AllPlot.h \
AllPlotWindow.h \
ANT.h \
ANTChannel.h \
ANTMessage.h \
ANTMessages.h \
ANTlocalController.h \
ANTplusController.h \
@@ -274,6 +276,8 @@ SOURCES += \
AllPlotWindow.cpp \
AthleteTool.cpp \
ANT.cpp \
ANTChannel.cpp \
ANTMessage.cpp \
ANTlocalController.cpp \
ANTplusController.cpp \
BasicRideMetrics.cpp \