From 701eb200ef7376fb6550e5a152d978c01d9bf4c2 Mon Sep 17 00:00:00 2001 From: Mark Liversedge Date: Sat, 5 Mar 2011 15:08:41 +0000 Subject: [PATCH] Native ANT+ Support (2 of 3) 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. --- src/ANT.cpp | 2526 +++++------------------------------- src/ANT.h | 395 ++---- src/ANTChannel.cpp | 703 ++++++++++ src/ANTChannel.h | 172 +++ src/ANTMessage.cpp | 516 ++++++++ src/ANTMessage.h | 102 ++ src/ANTlocalController.cpp | 1 + src/src.pro | 4 + 8 files changed, 1900 insertions(+), 2519 deletions(-) create mode 100755 src/ANTChannel.cpp create mode 100755 src/ANTChannel.h create mode 100755 src/ANTMessage.cpp create mode 100755 src/ANTMessage.h diff --git a/src/ANT.cpp b/src/ANT.cpp index fb0ae3d6e..168f1acd4 100644 --- a/src/ANT.cpp +++ b/src/ANT.cpp @@ -1,5 +1,4 @@ /* - * Copyright (c) 2009 Justin F. Knotzke (jknotzke@shampoo.ca) * Copyright (c) 2009 Mark Rages * Copyright (c) 2011 Mark Liversedge (liversedge@gmail.com) * @@ -25,6 +24,7 @@ //------------------------------------------------------------------------ #include "ANT.h" +#include "ANTMessage.h" #include #include #include @@ -41,16 +41,24 @@ const unsigned char ANT::key[8] = { 0xB9, 0xA5, 0x21, 0xFB, 0xBD, 0x72, 0xC3, 0x // supported sensor types const ant_sensor_type_t ANT::ant_sensor_types[] = { - { CHANNEL_TYPE_UNUSED, 0, 0, 0, 0, "Unused", '?' }, - { CHANNEL_TYPE_HR, ANT_SPORT_HR_PERIOD, ANT_SPORT_HR_TYPE, ANT_SPORT_FREQUENCY, ANT_SPORT_NETWORK_NUMBER, "Heartrate", 'h' }, - { CHANNEL_TYPE_POWER, ANT_SPORT_POWER_PERIOD, ANT_SPORT_POWER_TYPE, ANT_SPORT_FREQUENCY, ANT_SPORT_NETWORK_NUMBER, "Power", 'p' }, - { CHANNEL_TYPE_SPEED, ANT_SPORT_SPEED_PERIOD, ANT_SPORT_SPEED_TYPE, ANT_SPORT_FREQUENCY, ANT_SPORT_NETWORK_NUMBER, "Speed", 's' }, - { CHANNEL_TYPE_CADENCE, ANT_SPORT_CADENCE_PERIOD, ANT_SPORT_CADENCE_TYPE, ANT_SPORT_FREQUENCY, ANT_SPORT_NETWORK_NUMBER, "Cadence", 'c' }, - { CHANNEL_TYPE_SandC, ANT_SPORT_SandC_PERIOD, ANT_SPORT_SandC_TYPE, ANT_SPORT_FREQUENCY, ANT_SPORT_NETWORK_NUMBER, "Speed + Cadence", 'd' }, - { CHANNEL_TYPE_QUARQ, ANT_QUARQ_PERIOD, ANT_QUARQ_TYPE, ANT_QUARQ_FREQUENCY, DEFAULT_NETWORK_NUMBER, "Quarq Channel", 'Q' }, - { CHANNEL_TYPE_FAST_QUARQ, ANT_FAST_QUARQ_PERIOD, ANT_FAST_QUARQ_TYPE, ANT_FAST_QUARQ_FREQUENCY, DEFAULT_NETWORK_NUMBER, "Fast Quarq", 'q' }, - { CHANNEL_TYPE_FAST_QUARQ_NEW, ANT_FAST_QUARQ_PERIOD, ANT_FAST_QUARQ_TYPE_WAS, ANT_FAST_QUARQ_FREQUENCY, DEFAULT_NETWORK_NUMBER, "Fast Quarq New", 'n' }, - { CHANNEL_TYPE_GUARD, 0, 0, 0, 0, "", '\0' } + { ANTChannel::CHANNEL_TYPE_UNUSED, 0, 0, 0, 0, "Unused", '?' }, + { ANTChannel::CHANNEL_TYPE_HR, ANT_SPORT_HR_PERIOD, ANT_SPORT_HR_TYPE, + ANT_SPORT_FREQUENCY, ANT_SPORT_NETWORK_NUMBER, "Heartrate", 'h' }, + { ANTChannel::CHANNEL_TYPE_POWER, ANT_SPORT_POWER_PERIOD, ANT_SPORT_POWER_TYPE, + ANT_SPORT_FREQUENCY, ANT_SPORT_NETWORK_NUMBER, "Power", 'p' }, + { ANTChannel::CHANNEL_TYPE_SPEED, ANT_SPORT_SPEED_PERIOD, ANT_SPORT_SPEED_TYPE, + ANT_SPORT_FREQUENCY, ANT_SPORT_NETWORK_NUMBER, "Speed", 's' }, + { ANTChannel::CHANNEL_TYPE_CADENCE, ANT_SPORT_CADENCE_PERIOD, ANT_SPORT_CADENCE_TYPE, + ANT_SPORT_FREQUENCY, ANT_SPORT_NETWORK_NUMBER, "Cadence", 'c' }, + { ANTChannel::CHANNEL_TYPE_SandC, ANT_SPORT_SandC_PERIOD, ANT_SPORT_SandC_TYPE, + ANT_SPORT_FREQUENCY, ANT_SPORT_NETWORK_NUMBER, "Speed + Cadence", 'd' }, + { ANTChannel::CHANNEL_TYPE_QUARQ, ANT_QUARQ_PERIOD, ANT_QUARQ_TYPE, + ANT_QUARQ_FREQUENCY, DEFAULT_NETWORK_NUMBER, "Quarq Channel", 'Q' }, + { ANTChannel::CHANNEL_TYPE_FAST_QUARQ, ANT_FAST_QUARQ_PERIOD, ANT_FAST_QUARQ_TYPE, + ANT_FAST_QUARQ_FREQUENCY, DEFAULT_NETWORK_NUMBER, "Fast Quarq", 'q' }, + { ANTChannel::CHANNEL_TYPE_FAST_QUARQ_NEW, ANT_FAST_QUARQ_PERIOD, ANT_FAST_QUARQ_TYPE_WAS, + ANT_FAST_QUARQ_FREQUENCY, DEFAULT_NETWORK_NUMBER, "Fast Quarq New", 'n' }, + { ANTChannel::CHANNEL_TYPE_GUARD, 0, 0, 0, 0, "", '\0' } }; // @@ -71,13 +79,18 @@ ANT::ANT(QObject *parent, DeviceConfiguration *devConf) : QThread(parent) baud=115200; // state machine - state = ST_WAIT_FOR_SYNC; - length = bytes = 0; - checksum = ANT_SYNC_BYTE; + state = ST_WAIT_FOR_SYNC; + length = bytes = 0; + checksum = ANT_SYNC_BYTE; - // ant ids - antIDs = devConf->deviceProfile.split(","); - lastReadHR = lastReadWatts = lastReadCadence = lastReadSpeed = 0; + // ant ids - may not be configured of course + if (devConf->deviceProfile.length()) + antIDs = devConf->deviceProfile.split(","); + else + antIDs.clear(); + + // setup the channels + for (int i=0; i 0) ANT_ReceiveByte((unsigned char)byte); + if (rawRead(&byte, 1) > 0) receiveByte((unsigned char)byte); + //else msleep(300); //---------------------------------------------------------------------- // LISTEN TO CONTROLLER FOR COMMANDS @@ -211,7 +248,7 @@ ANT::stop() int ANT::quit(int code) { -qDebug()<<"Stopping ANT thread..."; +//qDebug()<<"Stopping ANT thread..."; // event code goes here! closePort(); exit(code); @@ -228,1351 +265,293 @@ ANT::getRealtimeData() * Channel management *====================================================================*/ -void -ANT::channel_manager_init(channel_manager_t *self) { - int i; - - for (i=0; ichannels+i, i, self); - -#if 0 // XXX need to understand this better, not used obviously! - if (0) { - channel_manager_remember_device(self, 17, CHANNEL_TYPE_POWER); - channel_manager_remember_device(self, 17, CHANNEL_TYPE_HR); - channel_manager_remember_device(self, 17, CHANNEL_TYPE_SandC); - channel_manager_remember_device(self, 17, CHANNEL_TYPE_SPEED); - - return; - } -#endif - -} - // returns 1 for success, 0 for fail. int -ANT::channel_manager_add_device(channel_manager_t *self, int device_number, int device_type, int channel_number) { - int i; - - // if we're given a device number, then use that one - if (channel_number>-1) { - ant_channel_close(self->channels+channel_number); - ant_channel_open(self->channels+channel_number, device_number, device_type); - return 1; - } - - // if we already have the device, then return. - for (i=0; ichannels[i].channel_type & 0xf ) == device_type) && - (self->channels[i].device_number == device_number)) { - // send the channel found... - ant_channel_channel_info(self->channels+i); - return 1; - } - - // look for an unused channel and use on that one - for (i=0; ichannels[i].channel_type == CHANNEL_TYPE_UNUSED) { - - fprintf(stderr, "opening channel #%d\n",i); - - ant_channel_open(self->channels+i, device_number, device_type); - return 1; +ANT::addDevice(int device_number, int device_type, int channel_number) +{ + // if we're given a device number, then use that one + if (channel_number>-1) { + antChannel[channel_number]->close(); + antChannel[channel_number]->open(device_number, device_type); + return 1; } - // there are no unused channels. fail. - return 0; + // if we already have the device, then return. + for (int i=0; ichannel_type & 0xf ) == device_type) && + (antChannel[i]->device_number == device_number)) { + // send the channel found... + antChannel[i]->channelInfo(); + return 1; + } + } + + // look for an unused channel and use on that one + for (int i=0; ichannel_type == ANTChannel::CHANNEL_TYPE_UNUSED) { + antChannel[i]->open(device_number, device_type); + return 1; + } + } + + // there are no unused channels. fail. + return 0; } // returns 1 for successfully removed, 0 for none found. int -ANT::channel_manager_remove_device(channel_manager_t *self, int device_number, int channel_type) { +ANT::removeDevice(int device_number, int channel_type) +{ + int i; - int i; + for (i=0; ichannels+i; + if ((ac->channel_type == channel_type) && (ac->device_number == device_number)) { - if ((ac->channel_type == channel_type) && - (ac->device_number == device_number)) { - - if ((ac->control_channel!=ac) && ac->control_channel) { - - channel_manager_remove_device(self, device_number, ac->control_channel->channel_type); - } - - ant_channel_close(ac); + if ((ac->control_channel!=ac) && ac->control_channel) + removeDevice(device_number, ac->control_channel->channel_type); - ac->channel_type=CHANNEL_TYPE_UNUSED; - ac->device_number=0; - ant_channel_set_id(ac); - return 1; - } + ac->close(); + ac->channel_type=ANTChannel::CHANNEL_TYPE_UNUSED; + ac->device_number=0; + ac->setId(); + return 1; + } } // device not found. return 0; } -ant_channel_t * -ANT::channel_manager_find_device(channel_manager_t *self, int device_number, int channel_type) { +ANTChannel * +ANT::findDevice(int device_number, int channel_type) +{ - int i; + int i; - for (i=0; ichannels[i].channel_type) == channel_type) && - (self->channels[i].device_number==device_number)) { - return self->channels+i; + for (i=0; ichannel_type) == channel_type) && + (antChannel[i]->device_number==device_number)) { + return antChannel[i]; + } } - // device not found. - return NULL; - + // device not found. + return NULL; } int -ANT::channel_manager_start_waiting_search(channel_manager_t *self) { - - int i; - // are any fast searches in progress? if so, then bail - for (i=0; ichannels[i].channel_type_flags & CHANNEL_TYPE_QUICK_SEARCH) { - return 0; - } - } +ANT::startWaitingSearch() +{ + int i; - // start the first slow search - for (i=0; ichannels[i].channel_type_flags & CHANNEL_TYPE_WAITING) { - self->channels[i].channel_type_flags &= ~CHANNEL_TYPE_WAITING; - ANT_UnassignChannel(i); - return 1; + // are any fast searches in progress? if so, then bail + for (i=0; ichannel_type_flags & CHANNEL_TYPE_QUICK_SEARCH) { + return 0; + } } - } - - return 0; + + // start the first slow search + for (i=0; ichannel_type_flags & CHANNEL_TYPE_WAITING) { + antChannel[i]->channel_type_flags &= ~CHANNEL_TYPE_WAITING; + sendMessage(ANTMessage::unassignChannel(i)); + return 1; + } + } + + return 0; } void -ANT::channel_manager_report(channel_manager_t *self) { - int i; - - for (i=0; ichannels+i); - +ANT::report() +{ + for (int i=0; ichannelInfo(); } void -ANT::channel_manager_associate_control_channels(channel_manager_t *self) { - int i; +ANT::associateControlChannels() { - // first, unassociate all control channels - for (i=0; ichannels[i].control_channel=NULL; + // first, unassociate all control channels + for (int i=0; icontrol_channel=NULL; - // then, associate cinqos: - // new cinqos get their own selves for control - // old cinqos, look for an open control channel - // if found and open, associate - // elif found and not open yet, nop - // elif not found, open one + // then, associate cinqos: + // new cinqos get their own selves for control + // old cinqos, look for an open control channel + // if found and open, associate + // elif found and not open yet, nop + // elif not found, open one + for (int i=0; ichannels+i; + switch (ac->channel_type) { + case ANTChannel::CHANNEL_TYPE_POWER: + if (ac->is_cinqo) { + if (ac->is_old_cinqo) { + ANTChannel *my_ant_channel; - switch (ac->channel_type) { - case CHANNEL_TYPE_POWER: - if (ac->is_cinqo) { - if (ac->is_old_cinqo) { - ant_channel_t * my_ant_channel; + my_ant_channel=findDevice(ac->device_number, ANTChannel::CHANNEL_TYPE_QUARQ); + if (!my_ant_channel) my_ant_channel=findDevice(ac->device_number, ANTChannel::CHANNEL_TYPE_FAST_QUARQ); + if (!my_ant_channel) my_ant_channel=findDevice(ac->device_number, ANTChannel::CHANNEL_TYPE_FAST_QUARQ_NEW); - my_ant_channel=channel_manager_find_device(self, - ac->device_number, - CHANNEL_TYPE_QUARQ); - if (!my_ant_channel) { - my_ant_channel=channel_manager_find_device(self, - ac->device_number, - CHANNEL_TYPE_FAST_QUARQ); - } - if (!my_ant_channel) { - my_ant_channel=channel_manager_find_device(self, - ac->device_number, - CHANNEL_TYPE_FAST_QUARQ_NEW); - } - - if (my_ant_channel) { - if (ant_channel_is_searching(my_ant_channel)) { - ; // searching, just wait around - } else { - ac->control_channel=my_ant_channel; - ant_channel_send_cinqo_success(ac); - } - } else { // no ant channel, let's start one - channel_manager_add_device(self, ac->device_number, - CHANNEL_TYPE_QUARQ, -1); - } - - } else { // new cinqo - ac->control_channel=ac; - ant_channel_send_cinqo_success(ac); - } - } // is_cinqo - break; - - case CHANNEL_TYPE_FAST_QUARQ: - case CHANNEL_TYPE_FAST_QUARQ_NEW: - case CHANNEL_TYPE_QUARQ: - ac->is_cinqo=1; - ac->control_channel=ac; - default: - ; - } // channel_type case - } // for-loop + if (my_ant_channel) { + if (my_ant_channel->isSearching()) { + // ignore if searching + } else { + ac->control_channel=my_ant_channel; + ac->sendCinqoSuccess(); + } + } else { // no ant channel, let's start one + addDevice(ac->device_number, ANTChannel::CHANNEL_TYPE_QUARQ, -1); + } + } else { // new cinqo + ac->control_channel=ac; + ac->sendCinqoSuccess(); + } + } // is_cinqo + break; + case ANTChannel::CHANNEL_TYPE_FAST_QUARQ: + case ANTChannel::CHANNEL_TYPE_FAST_QUARQ_NEW: + case ANTChannel::CHANNEL_TYPE_QUARQ: + ac->is_cinqo=1; + ac->control_channel=ac; + break; + default: + ; + } // channel_type case + } // for-loop } +// XXX device discovery for pairing to do... need to +// think about a cool way to do this. bool ANT::discover(DeviceConfiguration *, QProgressDialog *) { -#if 0 - QString strBuf; - QStringList strList; - sentDual = false; - sentSpeed = false; - sentHR = false; - sentCad = false; - sentPWR = false; - - openPort(); - - QByteArray strPwr("X-set-channel: 0p"); //Power - QByteArray strHR("X-set-channel: 0h"); //Heart Rate - QByteArray strSpeed("X-set-channel: 0s"); //Speed - QByteArray strCad("X-set-channel: 0c"); //Cadence - QByteArray strDual("X-set-channel: 0d"); //Dual (Speed/Cadence) - - // tell quarqd to start scanning.... - if (tcpSocket->isOpen()) { - tcpSocket->write(strPwr); //Power - - } - - QTime start; - start.start(); - progress->setMaximum(50000); - - while(start.elapsed() <= 50000) //Scan for 50 seconds. - { - if (progress->wasCanceled()) - { - tcpSocket->close(); - return false; - } - - progress->setValue(start.elapsed()); - - if(start.elapsed() >= 40000 && sentSpeed == false) - { - sentSpeed = true; - tcpSocket->write(strSpeed); //Speed - } else if(start.elapsed() >= 30000 && sentDual == false) - { - sentDual = true; - tcpSocket->write(strDual); //Dual - } else if(start.elapsed() >= 20000 && sentHR == false) - { - sentHR = true; - tcpSocket->write(strHR); //HR - } else if(start.elapsed() >= 10000 && sentCad == false) - { - sentCad = true; - tcpSocket->write(strCad); //Cadence - } else if(start.elapsed() >= 0 && sentPWR == false) - { - sentPWR = true; - tcpSocket->write(strPwr); - } - - if (tcpSocket->bytesAvailable() > 0) { - QByteArray array = tcpSocket->readAll(); - strBuf = array; - // qDebug() << strBuf; - QStringList qList = strBuf.split("\n"); - - //Loop for all the elements. - for(int i=0; isetValue(start.elapsed()); - QString str = qList.at(i); - // qDebug() << str; - if(str.contains("id")) - { - int start = str.indexOf("id"); - start += 4; - int end = str.indexOf("'", start); - QString id = str.mid(start, end - start); - if(!strList.contains(id)) - { - if(id != "0p" && id != "0h" && id != "0s" && id != "0c" && id != "0d") - strList.append(id); - } - } - } - } - } - progress->setValue(50000);//We are done. - //Now return a comma delimited string. - for(int i=0; i < strList.size(); i++) - { - config->deviceProfile.append(strList.at(i)); - if(i < strList.size() -1) - config->deviceProfile.append(','); - } - - return config; -#endif return false; } -void ANT::reinitChannel(QString /*_channel*/) -{ -#if 0 - if(!tcpSocket->isValid()) - return; - qDebug() << "Reinit: " << _channel; - - QByteArray channel("X-set-channel: "); - channel.append(_channel); - tcpSocket->write(channel); -#endif -} - - +/*---------------------------------------------------------------------- + * Message I/O + *--------------------------------------------------------------------*/ void -ANT::initChannel() -{ -#if 0 - qDebug() << "Init channel.."; - if(!tcpSocket->isValid()) - return; +ANT::sendMessage(ANTMessage m) { + static const unsigned char padding[5] = { '\0', '\0', '\0', '\0', '\0' }; - QByteArray setChannel("X-set-channel: "); - QByteArray channel; - for(int i=0; i < antIDs.size(); i++) - { - if(tcpSocket->isValid()) - { - channel.clear(); - channel = setChannel; - channel.append(antIDs.at(i)); - tcpSocket->write(channel); - tcpSocket->flush(); - qDebug() << channel; - sleep(2); - } - } -#endif -} + rawWrite((uint8_t*)m.data, m.length); - -/*====================================================================== - * ANT Channels - *====================================================================*/ - -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 - -void -ANT::ant_channel_set_id(ant_channel_t *self) { - if ((self->channel_type)==CHANNEL_TYPE_UNUSED) { - strcpy(self->id, "none"); - } else { - snprintf(self->id, 10, "%d%c", self->device_number, ant_sensor_types[self->channel_type].suffix); - } + // this padding is important, for some reason XXX find out why? + rawWrite((uint8_t*)padding, 5); } void -ANT::ant_channel_init(ant_channel_t *self, int number, channel_manager_t *parent) { - - self->parent=parent; - - self->channel_type=CHANNEL_TYPE_UNUSED; - self->channel_type_flags=0; - self->number=number; - - - self->is_cinqo=0; - self->is_old_cinqo=0; - self->control_channel=NULL; - self->manufacturer_id=0; - self->product_id=0; - self->product_version=0; - - self->device_number=0; - self->channel_assigned=0; - - ant_channel_set_id(self); - -// XXX fixme - channel event function setup... -#if 0 - ANT_AssignChannelEventFunction(self->number, - (void (*)(void *, unsigned char*)) ant_channel_receive_message, - (void *)self); -#endif - - self->state=ANT_UNASSIGN_CHANNEL; - self->blanked=1; - - self->messages_received=0; - self->messages_dropped=0; - - ant_channel_burst_init(self); - - INITIALIZE_MESSAGES_INITIALIZATION(self->mi); -} - -const char * -ANT::ant_channel_get_description(ant_channel_t *self) { - return ant_sensor_types[self->channel_type].descriptive_name; -} - -int -ANT::ant_channel_interpret_description(char *description) { - const ant_sensor_type_t *st=ant_sensor_types; - - do { - if (0==strcmp(st->descriptive_name,description)) - return st->type; - } while (++st, st->type != CHANNEL_TYPE_GUARD); - - return -1; -} - -int -ANT::ant_channel_interpret_suffix(char c) { - const ant_sensor_type_t *st=ant_sensor_types; - - do { - if (st->suffix==c) - return st->type; - } while (++st, st->type != CHANNEL_TYPE_GUARD); - - return -1; -} - -void -ANT::ant_channel_open(ant_channel_t *self, int device_number, int channel_type) { - self->channel_type=channel_type; - self->channel_type_flags = CHANNEL_TYPE_QUICK_SEARCH ; - self->device_number=device_number; - - ant_channel_set_id(self); - - if (self->channel_assigned) - ANT_UnassignChannel(self->number); - else - ant_channel_attempt_transition(self,ANT_UNASSIGN_CHANNEL); -} - - -void -ANT::ant_channel_close(ant_channel_t *self) { - ant_channel_lost_info(self); - - ANT_Close(self->number); - -} - -void -ANT::ant_channel_receive_message(ant_channel_t *self, unsigned char *ant_message) { - - unsigned char *message=ant_message+2; - -#ifdef DEBUG - int i; - if (quarqd_config.debug_level & DEBUG_LEVEL_ANT_MESSAGES) { - fprintf(stderr, "Got a message\n"); - for (i=0; i self->blanking_timestamp + timeout_blanking) { - if (!self->blanked) { - self->blanked=1; - ant_channel_stale_info(self); - } - } else self->blanked=0; -} - - -void -ANT::ant_channel_channel_event(ant_channel_t *self, unsigned char *ant_message) { - unsigned char *message=ant_message+2; - - if (MESSAGE_IS_RESPONSE_NO_ERROR(message)) { - ant_channel_attempt_transition(self,RESPONSE_NO_ERROR_MESSAGE_ID(message)); - } else if (MESSAGE_IS_EVENT_CHANNEL_CLOSED(message)) { - ANT_UnassignChannel(self->number); - } else if (MESSAGE_IS_EVENT_RX_SEARCH_TIMEOUT(message)) { - // timeouts are normal for search channel, so don't send xml for those - if (self->channel_type_flags & CHANNEL_TYPE_QUICK_SEARCH) { - DEBUG_ANT_CONNECTION("Got timeout on channel %d. Turning off search.\n", self->number); - self->channel_type_flags &= ~CHANNEL_TYPE_QUICK_SEARCH; - self->channel_type_flags |= CHANNEL_TYPE_WAITING; - - } else { - DEBUG_ANT_CONNECTION("Got timeout on channel %d. search is off.\n", self->number); - - ant_channel_lost_info(self); - - self->channel_type=CHANNEL_TYPE_UNUSED; - self->channel_type_flags=0; - self->device_number=0; - ant_channel_set_id(self); - - ANT_UnassignChannel(self->number); - } - - DEBUG_ANT_CONNECTION("Rx search timeout on %d\n",self->number); - channel_manager_start_waiting_search(self->parent); - - } else if (MESSAGE_IS_EVENT_RX_FAIL(message)) { - //ant_message_print_debug(message); - - self->messages_dropped++; - - double t=get_timestamp(); - - //fprintf(stderr, "timediff %f\n",self->last_message_timestamp-t); - - if (t > (self->last_message_timestamp + timeout_drop)) { - if (self->channel_type != CHANNEL_TYPE_UNUSED) - ant_channel_drop_info(self); - // this is a hacky way to prevent the drop message from sending multiple times - self->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 (self->tx_ack_disposition) { - self->tx_ack_disposition(self); - } - } else { - ant_message_print_debug(message); - ; // default - } -} - -void -ANT::ant_channel_send_cinqo_error(ant_channel_t * /*self*/) { - //XmlPrintf("\n", self->id); -} - -void -ANT::ant_channel_send_cinqo_success(ant_channel_t * /*self*/) { - //XmlPrintf("\n", self->id); -} - -void -ANT::ant_channel_check_cinqo(ant_channel_t *self) { - int version_hi, version_lo, swab_version; - - version_hi=(self->product_version >> 8) &0xff; - version_lo=(self->product_version & 0xff); - - swab_version=version_lo | (version_hi<<8); - - fprintf(stderr, "check cinqo\n"); - fprintf(stderr, "Product id %x\n",self->product_id); - fprintf(stderr, "Manu id %x\n",self->manufacturer_id); - - fprintf(stderr, "Product version %x\n",self->product_version); - fprintf(stderr, "Product version cvs rev#%d\n",swab_version); - fprintf(stderr, "Product version %d | %d\n",version_hi, version_lo); - - if (!(self->mi.first_time_manufacturer || self->mi.first_time_product)) { - if ((self->product_id == 1) && (self->manufacturer_id==7)) { - // we are a cinqo, were we aware of this? - self->is_cinqo=1; - - // are we an old-version or new-version cinqo? - self->is_old_cinqo = ((version_hi <= 17) && (version_lo==10)); - - fprintf(stderr, "I'm a cinqo %d!\n",self->is_old_cinqo); - - channel_manager_associate_control_channels(self->parent); - } // cinqo check - } -} - -void -ANT::ant_channel_broadcast_event(ant_channel_t *self, unsigned char *ant_message) { - - unsigned char *message=ant_message+2; - static char last_message[ANT_MAX_MESSAGE_SIZE]; - timestamp=get_timestamp(); - - self->messages_received++; - self->last_message_timestamp=timestamp; - - if (self->state != MESSAGE_RECEIVED) { - // first message! who are we talking to? - ANT_RequestMessage(self->number, ANT_CHANNEL_ID); - self->blanking_timestamp=get_timestamp(); - self->blanked=0; - return; // because we can't associate a channel id with the message yet - } - - if (0==memcmp(message, last_message, ANT_MAX_MESSAGE_SIZE)) { - //fprintf(stderr, "No change\n"); - return; // no change - } - - { - // for automatically opening quarq channel on early cinqo - if (MESSAGE_IS_PRODUCT(message)) { - self->mi.first_time_product=0; - self->product_version&=0x00ff; - self->product_version|=(PRODUCT_SW_REV(message))<<8; - ant_channel_check_cinqo(self); - } else if (MESSAGE_IS_MANUFACTURER(message)) { - self->mi.first_time_manufacturer=0; - self->product_version&=0xff00; - self->product_version|=MANUFACTURER_HW_REV(message); - self->manufacturer_id=MANUFACTURER_MANUFACTURER_ID(message); - self->product_id=MANUFACTURER_MODEL_NUMBER_ID(message); - ant_channel_check_cinqo(self); - } - } - -// XXX need to decide what to do with these messages -#if 0 - { - int matched=0; - - switch(self->channel_type) { - case CHANNEL_TYPE_HR: - ant_message_print_debug(message); - matched=xml_message_interpret_heartrate_broadcast(self, message); - break; - case CHANNEL_TYPE_POWER: - matched=xml_message_interpret_power_broadcast(self, message); - break; - case CHANNEL_TYPE_SPEED: - matched=xml_message_interpret_speed_broadcast(self, message); - break; - case CHANNEL_TYPE_CADENCE: - matched=xml_message_interpret_cadence_broadcast(self, message); - break; - case CHANNEL_TYPE_SandC: - matched=xml_message_interpret_speed_cadence_broadcast(self, message); - break; - case CHANNEL_TYPE_QUARQ: - case CHANNEL_TYPE_FAST_QUARQ: - case CHANNEL_TYPE_FAST_QUARQ_NEW: -#ifdef QUARQ_BUILD - matched=xml_message_interpret_quarq_broadcast(self, message); -#else - matched=xml_message_interpret_power_broadcast(self, message); -#endif - break; - default: - break; - } - - if ((!matched) && (quarqd_config.debug_level & DEBUG_LEVEL_ERRORS)) { - int i; - fprintf(stderr, "Unknown Message!\n"); - for (i=0; ichannel_type) { - case CHANNEL_TYPE_POWER: - matched=xml_message_interpret_power_broadcast(self, message); - break; - } - - if ((!matched) && (quarqd_config.debug_level & DEBUG_LEVEL_ERRORS)) { - int i; - fprintf(stderr, "Unknown Message!\n"); - for (i=0; idevice_number=CHANNEL_ID_DEVICE_NUMBER(message); - self->device_id=CHANNEL_ID_DEVICE_TYPE_ID(message); - - ant_channel_set_id(self); - - self->state=MESSAGE_RECEIVED; - - DEBUG_ANT_CONNECTION("%d: Connected to %s (0x%x) device number %d\n", - self->number, - ant_channel_get_description(self), - self->device_id, - self->device_number); - - ant_channel_channel_info(self); - - // if we were searching, - if (self->channel_type_flags & CHANNEL_TYPE_QUICK_SEARCH) { - ANT_SetChannelSearchTimeout(self->number, - (int)(timeout_lost/2.5)); - } - self->channel_type_flags &= ~CHANNEL_TYPE_QUICK_SEARCH; - - channel_manager_start_waiting_search(self->parent); - - // if we are quarq channel, hook up with the ant+ channel we are connected to - channel_manager_associate_control_channels(self->parent); - -} - -void -ANT::ant_channel_burst_init(ant_channel_t *self) { - self->rx_burst_data_index=0; - self->rx_burst_next_sequence=0; - self->rx_burst_disposition=NULL; -} - -int -ANT::ant_channel_is_searching(ant_channel_t *self) { - return ((self->channel_type_flags & (CHANNEL_TYPE_WAITING | CHANNEL_TYPE_QUICK_SEARCH)) || (self->state != MESSAGE_RECEIVED)); -} - - -void -ANT::ant_channel_burst_data(ant_channel_t *self, 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!=self->rx_burst_next_sequence) { - DEBUG_ERRORS("Bad sequence %d not %d\n",seq,self->rx_burst_next_sequence); - // burst problem! - } else { - int len=ant_message[ANT_OFFSET_LENGTH]-3; - - if ((self->rx_burst_data_index + len)>(RX_BURST_DATA_LEN)) { - len = RX_BURST_DATA_LEN-self->rx_burst_data_index; - } - - self->rx_burst_next_sequence=next_sequence[(int)seq]; - memcpy(self->rx_burst_data+self->rx_burst_data_index, message+2, len); - self->rx_burst_data_index+=len; - - //fprintf(stderr, "Copying %d bytes.\n",len); - } - - if (last) { - if (self->rx_burst_disposition) { - self->rx_burst_disposition(self); - } - ant_channel_burst_init(self); - } -} - -void -ANT::ant_channel_request_calibrate(ant_channel_t *self) { - ANT_RequestCalibrate(self->number); -} - -void -ANT::ant_channel_attempt_transition(ant_channel_t *self, int message_id) { - - const ant_sensor_type_t *st; - - int previous_state=self->state; - - st=ant_sensor_types+(self->channel_type); - - - // update state - self->state=message_id; - - // do transitions - switch (self->state) { - case ANT_CLOSE_CHANNEL: - // next step is unassign and start over - // but we must wait until event_channel_closed - // which is its own channel event - self->state=MESSAGE_RECEIVED; - break; - case ANT_UNASSIGN_CHANNEL: - self->channel_assigned=0; - if (st->type==CHANNEL_TYPE_UNUSED) { - // we're shutting the channel down - - } else { - - self->device_id=st->device_id; - - if (self->channel_type & CHANNEL_TYPE_PAIR) { - self->device_id |= 0x80; - } - - ant_channel_set_id(self); - - DEBUG_ANT_CONNECTION("Opening for %s\n",ant_channel_get_description(self)); - ANT_AssignChannel(self->number, 0, st->network); // recieve channel on network 1 - } - break; - case ANT_ASSIGN_CHANNEL: - self->channel_assigned=1; - ANT_SetChannelID(self->number, self->device_number, self->device_id, 0); - break; - case ANT_CHANNEL_ID: - if (self->channel_type & CHANNEL_TYPE_QUICK_SEARCH) { - DEBUG_ANT_CONNECTION("search\n"); - ANT_SetChannelSearchTimeout(self->number, - (int)(timeout_scan/2.5)); - } else { - DEBUG_ANT_CONNECTION("nosearch\n"); - ANT_SetChannelSearchTimeout(self->number, - (int)(timeout_lost/2.5)); - } - break; - case ANT_SEARCH_TIMEOUT: - if (previous_state==ANT_CHANNEL_ID) { - // continue down the intialization chain - ANT_SetChannelPeriod(self->number, st->period); - } else { - // we are setting the ant_search timeout after connected - // we'll just pretend this never happened - DEBUG_ANT_CONNECTION("resetting ant_search timeout.\n"); - self->state=previous_state; - } - break; - case ANT_CHANNEL_PERIOD: - ANT_SetChannelFreq(self->number, st->frequency); - break; - case ANT_CHANNEL_FREQUENCY: - ANT_Open(self->number); - INITIALIZE_MESSAGES_INITIALIZATION(self->mi); - break; - case ANT_OPEN_CHANNEL: - DEBUG_ANT_CONNECTION("Channel %d opened for %s\n",self->number, - ant_channel_get_description(self)); - break; - default: - DEBUG_ERRORS("unknown channel event 0x%x\n",message_id); - } -} - -#define BXmlPrintf(format, args...) \ - self->blanking_timestamp=timestamp; \ - XmlPrintf(format, ##args) - -#define RememberMessage(message_len, if_changed) \ - static unsigned char last_messages[ANT_CHANNEL_COUNT][message_len]; \ - unsigned char * last_message=last_messages[self->number]; \ - \ - if (!first_time) { \ - if (0!=memcmp(message,last_message,message_len)) { \ - if_changed; \ - } \ - } else { \ - first_time=0; \ - } \ - memcpy(last_message,message,message_len) - -#define RememberXmlPrintf(message_len, format, args...) \ - RememberMessage(message_len, XmlPrintf(format, ##args)) - -#define BlankingXmlPrintf(message_len, format, args...) \ - RememberMessage(message_len, BXmlPrintf(format, ##args)) - -void -ANT::ant_channel_channel_info(ant_channel_t * /*self*/) { -#if 0 - XmlPrintf("\n", - self->id, - self->device_number, - ant_channel_get_description(self), - ((self->state==MESSAGE_RECEIVED || - (ant_sensor_types[self->channel_type]).type==CHANNEL_TYPE_UNUSED)? - "":" (searching)"), - (self->channel_type_flags&CHANNEL_TYPE_PAIR)?" paired":"", - self->number, - self->messages_received, - self->messages_dropped, - self->messages_received? \ - 100*self->messages_dropped/self->messages_received : 0); -#endif -} - -void -ANT::ant_channel_drop_info(ant_channel_t * /*self*/) { -#if 0 - XmlPrintf("\n", - self->id, - self->device_number, - ant_channel_get_description(self), - timeout_drop, - self->number); -#endif -} - -void -ANT::ant_channel_lost_info(ant_channel_t * /*self*/) { -#if 0 - XmlPrintf("\n", - self->id, - self->device_number, - ant_channel_get_description(self), - timeout_lost, - self->number); -#endif -} - -void -ANT::ant_channel_stale_info(ant_channel_t * /*self*/) { -#if 0 - XmlPrintf("\n", - self->id, - self->device_number, - ant_channel_get_description(self), - timeout_blanking, - self->number); -#endif -} - -void -ANT::ant_channel_report_timeouts( void ) { -#if 0 - XmlPrintf("\n" - "\n" - "\n" - "\n", - timeout_blanking, - timeout_drop, - timeout_scan, - timeout_lost); -#endif -} - -int -ANT::ant_channel_set_timeout( char *type, float value, int /*connection*/) { - - if (0==strcmp(type,"blanking")) timeout_blanking=value; - else if (0==strcmp(type,"drop")) timeout_drop=value; - else if (0==strcmp(type,"scan")) timeout_scan=value; - else if (0==strcmp(type,"lost")) timeout_lost=value; - else { - ant_channel_report_timeouts(); - return 0; - } - ant_channel_report_timeouts(); - return 1; -} - -// this is in the wrong place here for the convenience of the -// XmlPrintf macro -void -ANT::ant_channel_search_complete( void ) { -#if 0 - XmlPrintf("\n"); -#endif -} - -// XXX decide how to manage this -#if 0 -float -ANT::get_srm_offset(int device_id) { - float ret=0.0; - read_offset_log(device_id, &ret); - return ret; -} - -void -ANT::set_srm_offset(int device_id, float value) { - update_offset_log(device_id, value); -} -#endif - -/*====================================================================== - * ANT Messaging - *====================================================================*/ - -void -ANT::ANT_ResetSystem(void){ - - txMessage[ANT_OFFSET_LENGTH] = 1; - txMessage[ANT_OFFSET_ID] = ANT_SYSTEM_RESET; - ANT_SendMessage(); -} - -void -ANT::ANT_AssignChannel(const unsigned char channel, const unsigned char type, const unsigned char network) { - - unsigned char *data = txMessage + ANT_OFFSET_DATA; - - txMessage[ANT_OFFSET_LENGTH] = 3; - txMessage[ANT_OFFSET_ID] = ANT_ASSIGN_CHANNEL; - *data++ = channel; - *data++ = type; - *data++ = network; - - ANT_SendMessage(); - -} - -void -ANT::ANT_UnassignChannel(const unsigned char channel) { - - unsigned char *data = txMessage + ANT_OFFSET_DATA; - - txMessage[ANT_OFFSET_LENGTH] = 1; - txMessage[ANT_OFFSET_ID] = ANT_UNASSIGN_CHANNEL; - *data = channel; - - ANT_SendMessage(); -} - -void -ANT::ANT_SetChannelSearchTimeout(const unsigned char channel, const unsigned char search_timeout) { - - unsigned char *data = txMessage + ANT_OFFSET_DATA; - - txMessage[ANT_OFFSET_LENGTH] = 2; - txMessage[ANT_OFFSET_ID] = ANT_SEARCH_TIMEOUT; - *data++ = channel; - *data++ = search_timeout; - - ANT_SendMessage(); -} - -void -ANT::ANT_RequestMessage(const unsigned char channel, const unsigned char request) { - - unsigned char *data = txMessage + ANT_OFFSET_DATA; - - txMessage[ANT_OFFSET_LENGTH] = 2; - txMessage[ANT_OFFSET_ID] = ANT_REQ_MESSAGE; - *data++ = channel; - *data++ = request; - - ANT_SendMessage(); -} - -void -ANT::ANT_SetChannelID(const unsigned char channel, const unsigned short device, - const unsigned char deviceType, const unsigned char txType) { - - unsigned char *data = txMessage + ANT_OFFSET_DATA; - - txMessage[ANT_OFFSET_LENGTH] = 5; - txMessage[ANT_OFFSET_ID] = ANT_CHANNEL_ID; - *data++ = channel; - *data++ = device & 0xff; - *data++ = (device >> 8) & 0xff; - *data++ = deviceType; - *data++ = txType; - - ANT_SendMessage(); -} - -void -ANT::ANT_SetChannelPeriod(const unsigned char channel, const unsigned short period) { - - unsigned char *data = txMessage + ANT_OFFSET_DATA; - - txMessage[ANT_OFFSET_LENGTH] = 3; - txMessage[ANT_OFFSET_ID] = ANT_CHANNEL_PERIOD; - *data++ = channel; - *data++ = period & 0xff; - *data++ = (period >> 8) & 0xff; - - ANT_SendMessage(); -} - -void -ANT::ANT_SetChannelFreq(const unsigned char channel, const unsigned char frequency) { - - unsigned char *data = txMessage + ANT_OFFSET_DATA; - - txMessage[ANT_OFFSET_LENGTH] = 2; - txMessage[ANT_OFFSET_ID] = ANT_CHANNEL_FREQUENCY; - *data++ = channel; - *data++ = frequency; - - ANT_SendMessage(); -} - -void -ANT::ANT_SetNetworkKey(const unsigned char net, const unsigned char *key) { - - unsigned char *data = txMessage + ANT_OFFSET_DATA; - int i; - - txMessage[ANT_OFFSET_LENGTH] = 9; - txMessage[ANT_OFFSET_ID] = ANT_SET_NETWORK; - *data++ = net; - for (i = 0; i < ANT_KEY_LENGTH; i++) - *data++ = key[i]; - - ANT_SendMessage(); - -} - -void -ANT::ANT_SendAckMessage( void ) { - - memcpy(txMessage, txAckMessage, ANT_MAX_MESSAGE_SIZE); - ANT_SendMessage(); - -} - -void -ANT::ANT_SetAutoCalibrate(const unsigned char channel, const int autozero) { - unsigned char *data = txAckMessage + ANT_OFFSET_DATA; - - txAckMessage[ANT_OFFSET_LENGTH] = 4; - txAckMessage[ANT_OFFSET_ID] = ANT_ACK_DATA; - *data++ = channel; - *data++ = ANT_SPORT_CALIBRATION_MESSAGE; - *data++ = ANT_SPORT_CALIBRATION_REQUEST_AUTOZERO_CONFIG; - *data++ = autozero ? ANT_SPORT_AUTOZERO_ON : ANT_SPORT_AUTOZERO_OFF; - - ANT_SendAckMessage(); -} - -void -ANT::ANT_RequestCalibrate(const unsigned char channel) { - unsigned char *data = txAckMessage + ANT_OFFSET_DATA; - - txAckMessage[ANT_OFFSET_LENGTH] = 4; - txAckMessage[ANT_OFFSET_ID] = ANT_ACK_DATA; - *data++ = channel; - *data++ = ANT_SPORT_CALIBRATION_MESSAGE; - *data++ = ANT_SPORT_CALIBRATION_REQUEST_MANUALZERO; - *data++ = 0; - - ANT_SendAckMessage(); - -} - -void -ANT::ANT_Open(const unsigned char channel) { - unsigned char *data = txMessage + ANT_OFFSET_DATA; - - txMessage[ANT_OFFSET_LENGTH] = 1; - txMessage[ANT_OFFSET_ID] = ANT_OPEN_CHANNEL; - *data++ = channel; - - ANT_SendMessage(); -} - -void -ANT::ANT_Close(const unsigned char channel) { - - unsigned char *data = txMessage + ANT_OFFSET_DATA; - - txMessage[ANT_OFFSET_LENGTH] = 1; - txMessage[ANT_OFFSET_ID] = ANT_CLOSE_CHANNEL; - *data++ = channel; - - ANT_SendMessage(); -} - -void -ANT::ANT_SendMessage(void) { - int i; - const int length = txMessage[ANT_OFFSET_LENGTH] + ANT_OFFSET_DATA; - unsigned char checksum = ANT_SYNC_BYTE; - - txMessage[ANT_OFFSET_SYNC] = ANT_SYNC_BYTE; - - for (i = ANT_OFFSET_LENGTH; i < length; i++) - checksum ^= txMessage[i]; - - txMessage[i++] = checksum; - - txMessage[i++] = 0x0; - txMessage[i++] = 0x0; - txMessage[i++] = 0x0; - txMessage[i++] = 0x0; - txMessage[i++] = 0x0; - - ANT_Transmit(txMessage, i); -} - -void -ANT::ANT_Transmit(const unsigned char *msg, int length) { - if (rawWrite((uint8_t*)msg, length) < 0) { - //XXX error somewhere; +ANT::receiveByte(unsigned char byte) { + + switch (state) { + case ST_WAIT_FOR_SYNC: + if (byte == ANT_SYNC_BYTE) { + state = ST_GET_LENGTH; + checksum = ANT_SYNC_BYTE; + } + break; + + case ST_GET_LENGTH: + if ((byte == 0) || (byte > ANT_MAX_LENGTH)) { + state = ST_WAIT_FOR_SYNC; + } + else { + rxMessage[ANT_OFFSET_LENGTH] = byte; + checksum ^= byte; + length = byte; + bytes = 0; + state = ST_GET_MESSAGE_ID; + } + break; + + case ST_GET_MESSAGE_ID: + rxMessage[ANT_OFFSET_ID] = byte; + checksum ^= byte; + state = ST_GET_DATA; + break; + + case ST_GET_DATA: + rxMessage[ANT_OFFSET_DATA + bytes] = byte; + checksum ^= byte; + if (++bytes >= length){ + state = ST_VALIDATE_PACKET; + } + break; + + case ST_VALIDATE_PACKET: + if (checksum == byte){ + processMessage(); + } + state = ST_WAIT_FOR_SYNC; + break; } } + +// +// Pass inbound message to channel for handling +// void -ANT::ANT_ReceiveByte(unsigned char byte) { - - switch (state) { - case ST_WAIT_FOR_SYNC: - if (byte == ANT_SYNC_BYTE) { - state = ST_GET_LENGTH; - checksum = ANT_SYNC_BYTE; - } - break; - - case ST_GET_LENGTH: - if ((byte == 0) || (byte > ANT_MAX_LENGTH)) { - state = ST_WAIT_FOR_SYNC; - } - else { - rxMessage[ANT_OFFSET_LENGTH] = byte; - checksum ^= byte; - length = byte; - bytes = 0; - state = ST_GET_MESSAGE_ID; - } - break; - - case ST_GET_MESSAGE_ID: - rxMessage[ANT_OFFSET_ID] = byte; - checksum ^= byte; - state = ST_GET_DATA; - break; - - case ST_GET_DATA: - rxMessage[ANT_OFFSET_DATA + bytes] = byte; - checksum ^= byte; - if (++bytes >= length){ - state = ST_VALIDATE_PACKET; - } - break; - - case ST_VALIDATE_PACKET: - if (checksum == byte){ - ANT_ProcessMessage(); - } - state = ST_WAIT_FOR_SYNC; - break; - } -} - - -void -ANT::ANT_HandleChannelEvent(void) { - int channel = rxMessage[ANT_OFFSET_DATA] & 0x7; - if(channel >= 0 && channel < 4) { +ANT::handleChannelEvent(void) { + int channel = rxMessage[ANT_OFFSET_DATA] & 0x7; + if(channel >= 0 && channel < 4) { // handle a channel event here! -qDebug()<<"channel event on channel:"<receiveMessage(rxMessage); + } } void -ANT::ANT_ProcessMessage(void) { +ANT::processMessage(void) { -qDebug()<<"processing ant message"<suffix==c) return st->type; + } while (++st, st->type != ANTChannel::CHANNEL_TYPE_GUARD); -void -ANT::ant_message_print_debug(unsigned char * message) { -// assign_channel -if (( message[0]==0x42 )) { - unsigned char channel = message[0]; - unsigned char channel_type = message[0]; - unsigned char network_number = message[0]; - - fprintf(stderr,"assign_channel:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tchannel_type: %d (0x%02X)\n",channel_type,channel_type); - fprintf(stderr,"\tnetwork_number: %d (0x%02X)\n",network_number,network_number); -} else -// unassign_channel -if (( message[0]==0x41 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"unassign_channel:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// open_channel -if (( message[0]==0x4b )) { - unsigned char channel = message[0]; - - fprintf(stderr,"open_channel:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// channel_id -if (( message[0]==0x51 )) { - unsigned char channel = message[0]; - unsigned short device_number = (message[1]<<8)+(message[0]); - unsigned char device_type_id = message[0]; - unsigned char transmission_type = message[0]; - - fprintf(stderr,"channel_id:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tdevice_number: %d\n",device_number); - fprintf(stderr,"\tdevice_type_id: %d (0x%02X)\n",device_type_id,device_type_id); - fprintf(stderr,"\ttransmission_type: %d (0x%02X)\n",transmission_type,transmission_type); -} else -// burst_message -if (( message[0]==0x50 )) { - unsigned char chan_seq = message[0]; - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - unsigned char data3 = message[0]; - unsigned char data4 = message[0]; - unsigned char data5 = message[0]; - unsigned char data6 = message[0]; - unsigned char data7 = message[0]; - - fprintf(stderr,"burst_message:\n"); - fprintf(stderr,"\tchan_seq: %d (0x%02X)\n",chan_seq,chan_seq); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); - fprintf(stderr,"\tdata3: %d (0x%02X)\n",data3,data3); - fprintf(stderr,"\tdata4: %d (0x%02X)\n",data4,data4); - fprintf(stderr,"\tdata5: %d (0x%02X)\n",data5,data5); - fprintf(stderr,"\tdata6: %d (0x%02X)\n",data6,data6); - fprintf(stderr,"\tdata7: %d (0x%02X)\n",data7,data7); -} else -// burst_message7 -if (( message[0]==0x50 )) { - unsigned char chan_seq = message[0]; - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - unsigned char data3 = message[0]; - unsigned char data4 = message[0]; - unsigned char data5 = message[0]; - unsigned char data6 = message[0]; - - fprintf(stderr,"burst_message7:\n"); - fprintf(stderr,"\tchan_seq: %d (0x%02X)\n",chan_seq,chan_seq); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); - fprintf(stderr,"\tdata3: %d (0x%02X)\n",data3,data3); - fprintf(stderr,"\tdata4: %d (0x%02X)\n",data4,data4); - fprintf(stderr,"\tdata5: %d (0x%02X)\n",data5,data5); - fprintf(stderr,"\tdata6: %d (0x%02X)\n",data6,data6); -} else -// burst_message6 -if (( message[0]==0x50 )) { - unsigned char chan_seq = message[0]; - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - unsigned char data3 = message[0]; - unsigned char data4 = message[0]; - unsigned char data5 = message[0]; - - fprintf(stderr,"burst_message6:\n"); - fprintf(stderr,"\tchan_seq: %d (0x%02X)\n",chan_seq,chan_seq); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); - fprintf(stderr,"\tdata3: %d (0x%02X)\n",data3,data3); - fprintf(stderr,"\tdata4: %d (0x%02X)\n",data4,data4); - fprintf(stderr,"\tdata5: %d (0x%02X)\n",data5,data5); -} else -// burst_message5 -if (( message[0]==0x50 )) { - unsigned char chan_seq = message[0]; - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - unsigned char data3 = message[0]; - unsigned char data4 = message[0]; - - fprintf(stderr,"burst_message5:\n"); - fprintf(stderr,"\tchan_seq: %d (0x%02X)\n",chan_seq,chan_seq); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); - fprintf(stderr,"\tdata3: %d (0x%02X)\n",data3,data3); - fprintf(stderr,"\tdata4: %d (0x%02X)\n",data4,data4); -} else -// burst_message4 -if (( message[0]==0x50 )) { - unsigned char chan_seq = message[0]; - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - unsigned char data3 = message[0]; - - fprintf(stderr,"burst_message4:\n"); - fprintf(stderr,"\tchan_seq: %d (0x%02X)\n",chan_seq,chan_seq); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); - fprintf(stderr,"\tdata3: %d (0x%02X)\n",data3,data3); -} else -// burst_message3 -if (( message[0]==0x50 )) { - unsigned char chan_seq = message[0]; - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - - fprintf(stderr,"burst_message3:\n"); - fprintf(stderr,"\tchan_seq: %d (0x%02X)\n",chan_seq,chan_seq); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); -} else -// burst_message2 -if (( message[0]==0x50 )) { - unsigned char chan_seq = message[0]; - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - - fprintf(stderr,"burst_message2:\n"); - fprintf(stderr,"\tchan_seq: %d (0x%02X)\n",chan_seq,chan_seq); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); -} else -// burst_message1 -if (( message[0]==0x50 )) { - unsigned char chan_seq = message[0]; - unsigned char data0 = message[0]; - - fprintf(stderr,"burst_message1:\n"); - fprintf(stderr,"\tchan_seq: %d (0x%02X)\n",chan_seq,chan_seq); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); -} else -// burst_message0 -if (( message[0]==0x50 )) { - unsigned char chan_seq = message[0]; - - fprintf(stderr,"burst_message0:\n"); - fprintf(stderr,"\tchan_seq: %d (0x%02X)\n",chan_seq,chan_seq); -} else -// channel_period -if (( message[0]==0x43 )) { - unsigned char channel = message[0]; - unsigned short period = (message[1]<<8)+(message[0]); - - fprintf(stderr,"channel_period:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tperiod: %d\n",period); -} else -// search_timeout -if (( message[0]==0x44 )) { - unsigned char channel = message[0]; - unsigned char search_timeout = message[0]; - - fprintf(stderr,"search_timeout:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tsearch_timeout: %d (0x%02X)\n",search_timeout,search_timeout); -} else -// channel_frequency -if (( message[0]==0x45 )) { - unsigned char channel = message[0]; - unsigned char rf_frequency = message[0]; - - fprintf(stderr,"channel_frequency:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\trf_frequency: %d (0x%02X)\n",rf_frequency,rf_frequency); -} else -// set_network -if (( message[0]==0x46 )) { - unsigned char channel = message[0]; - unsigned char key0 = message[0]; - unsigned char key1 = message[0]; - unsigned char key2 = message[0]; - unsigned char key3 = message[0]; - unsigned char key4 = message[0]; - unsigned char key5 = message[0]; - unsigned char key6 = message[0]; - unsigned char key7 = message[0]; - - fprintf(stderr,"set_network:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tkey0: %d (0x%02X)\n",key0,key0); - fprintf(stderr,"\tkey1: %d (0x%02X)\n",key1,key1); - fprintf(stderr,"\tkey2: %d (0x%02X)\n",key2,key2); - fprintf(stderr,"\tkey3: %d (0x%02X)\n",key3,key3); - fprintf(stderr,"\tkey4: %d (0x%02X)\n",key4,key4); - fprintf(stderr,"\tkey5: %d (0x%02X)\n",key5,key5); - fprintf(stderr,"\tkey6: %d (0x%02X)\n",key6,key6); - fprintf(stderr,"\tkey7: %d (0x%02X)\n",key7,key7); -} else -// transmit_power -if (( message[0]==0x47 ) && ( message[0]==0x00 )) { - unsigned char tx_power = message[0]; - - fprintf(stderr,"transmit_power:\n"); - fprintf(stderr,"\ttx_power: %d (0x%02X)\n",tx_power,tx_power); -} else -// reset_system -if (( message[0]==0x4a )) { - - - fprintf(stderr,"reset_system:\n"); -} else -// request_message -if (( message[0]==0x4d )) { - unsigned char channel = message[0]; - unsigned char message_requested = message[0]; - - fprintf(stderr,"request_message:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_requested: %d (0x%02X)\n",message_requested,message_requested); -} else -// close_channel -if (( message[0]==0x4c )) { - unsigned char channel = message[0]; - - fprintf(stderr,"close_channel:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// response_no_error -if (( message[0]==0x40 ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"response_no_error:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// response_assign_channel_ok -if (( message[0]==0x40 ) && ( message[0]==0x42 ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"response_assign_channel_ok:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// response_channel_unassign_ok -if (( message[0]==0x40 ) && ( message[0]==0x41 ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"response_channel_unassign_ok:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// response_open_channel_ok -if (( message[0]==0x40 ) && ( message[0]==0x4b ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"response_open_channel_ok:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// response_channel_id_ok -if (( message[0]==0x40 ) && ( message[0]==0x51 ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"response_channel_id_ok:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// response_channel_period_ok -if (( message[0]==0x40 ) && ( message[0]==0x43 ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"response_channel_period_ok:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// response_channel_frequency_ok -if (( message[0]==0x40 ) && ( message[0]==0x45 ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"response_channel_frequency_ok:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// response_set_network_ok -if (( message[0]==0x40 ) && ( message[0]==0x46 ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"response_set_network_ok:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// response_transmit_power_ok -if (( message[0]==0x40 ) && ( message[0]==0x47 ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"response_transmit_power_ok:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// response_close_channel_ok -if (( message[0]==0x40 ) && ( message[0]==0x4c ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"response_close_channel_ok:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// response_search_timeout_ok -if (( message[0]==0x40 ) && ( message[0]==0x44 ) && ( message[0]==0x00 )) { - unsigned char channel = message[0]; - - fprintf(stderr,"response_search_timeout_ok:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// event_rx_search_timeout -if (( message[0]==0x40 ) && ( message[0]==0x01 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_rx_search_timeout:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_rx_fail -if (( message[0]==0x40 ) && ( message[0]==0x02 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_rx_fail:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_tx -if (( message[0]==0x40 ) && ( message[0]==0x03 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_tx:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_transfer_rx_failed -if (( message[0]==0x40 ) && ( message[0]==0x04 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_transfer_rx_failed:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_transfer_tx_completed -if (( message[0]==0x40 ) && ( message[0]==0x05 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_transfer_tx_completed:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_transfer_tx_failed -if (( message[0]==0x40 ) && ( message[0]==0x06 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_transfer_tx_failed:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_channel_closed -if (( message[0]==0x40 ) && ( message[0]==0x07 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_channel_closed:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_rx_fail_go_to_search -if (( message[0]==0x40 ) && ( message[0]==0x08 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_rx_fail_go_to_search:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_channel_collision -if (( message[0]==0x40 ) && ( message[0]==0x09 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_channel_collision:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_transfer_tx_start -if (( message[0]==0x40 ) && ( message[0]==0x0a )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_transfer_tx_start:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_rx_acknowledged -if (( message[0]==0x40 ) && ( message[0]==0x0b )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_rx_acknowledged:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// event_rx_burst_packet -if (( message[0]==0x40 ) && ( message[0]==0x0c )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"event_rx_burst_packet:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// channel_in_wrong_state -if (( message[0]==0x40 ) && ( message[0]==0x15 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"channel_in_wrong_state:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// channel_not_opened -if (( message[0]==0x40 ) && ( message[0]==0x16 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"channel_not_opened:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// channel_id_not_set -if (( message[0]==0x40 ) && ( message[0]==0x18 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"channel_id_not_set:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// transfer_in_progress -if (( message[0]==0x40 ) && ( message[0]==0x1f )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"transfer_in_progress:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// channel_status -if (( message[0]==0x52 )) { - unsigned char channel = message[0]; - unsigned char status = message[0]; - - fprintf(stderr,"channel_status:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tstatus: %d (0x%02X)\n",status,status); -} else -// cw_init -if (( message[0]==0x53 )) { - - - fprintf(stderr,"cw_init:\n"); -} else -// cw_test -if (( message[0]==0x48 )) { - unsigned char power = message[0]; - unsigned char freq = message[0]; - - fprintf(stderr,"cw_test:\n"); - fprintf(stderr,"\tpower: %d (0x%02X)\n",power,power); - fprintf(stderr,"\tfreq: %d (0x%02X)\n",freq,freq); -} else -// capabilities -if (( message[0]==0x54 )) { - unsigned char max_channels = message[0]; - unsigned char max_networks = message[0]; - unsigned char standard_options = message[0]; - unsigned char advanced_options = message[0]; - - fprintf(stderr,"capabilities:\n"); - fprintf(stderr,"\tmax_channels: %d (0x%02X)\n",max_channels,max_channels); - fprintf(stderr,"\tmax_networks: %d (0x%02X)\n",max_networks,max_networks); - fprintf(stderr,"\tstandard_options: %d (0x%02X)\n",standard_options,standard_options); - fprintf(stderr,"\tadvanced_options: %d (0x%02X)\n",advanced_options,advanced_options); -} else -// capabilities_extended -if (( message[0]==0x54 )) { - unsigned char max_channels = message[0]; - unsigned char max_networks = message[0]; - unsigned char standard_options = message[0]; - unsigned char advanced_options = message[0]; - unsigned char advanced_options_2 = message[0]; - unsigned char max_data_channels = message[0]; - - fprintf(stderr,"capabilities_extended:\n"); - fprintf(stderr,"\tmax_channels: %d (0x%02X)\n",max_channels,max_channels); - fprintf(stderr,"\tmax_networks: %d (0x%02X)\n",max_networks,max_networks); - fprintf(stderr,"\tstandard_options: %d (0x%02X)\n",standard_options,standard_options); - fprintf(stderr,"\tadvanced_options: %d (0x%02X)\n",advanced_options,advanced_options); - fprintf(stderr,"\tadvanced_options_2: %d (0x%02X)\n",advanced_options_2,advanced_options_2); - fprintf(stderr,"\tmax_data_channels: %d (0x%02X)\n",max_data_channels,max_data_channels); -} else -// ant_version -if (( message[0]==0x3e )) { - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - unsigned char data3 = message[0]; - unsigned char data4 = message[0]; - unsigned char data5 = message[0]; - unsigned char data6 = message[0]; - unsigned char data7 = message[0]; - unsigned char data8 = message[0]; - - fprintf(stderr,"ant_version:\n"); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); - fprintf(stderr,"\tdata3: %d (0x%02X)\n",data3,data3); - fprintf(stderr,"\tdata4: %d (0x%02X)\n",data4,data4); - fprintf(stderr,"\tdata5: %d (0x%02X)\n",data5,data5); - fprintf(stderr,"\tdata6: %d (0x%02X)\n",data6,data6); - fprintf(stderr,"\tdata7: %d (0x%02X)\n",data7,data7); - fprintf(stderr,"\tdata8: %d (0x%02X)\n",data8,data8); -} else -// ant_version_long -if (( message[0]==0x3e )) { - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - unsigned char data3 = message[0]; - unsigned char data4 = message[0]; - unsigned char data5 = message[0]; - unsigned char data6 = message[0]; - unsigned char data7 = message[0]; - unsigned char data8 = message[0]; - unsigned char data9 = message[0]; - unsigned char data10 = message[0]; - unsigned char data11 = message[0]; - unsigned char data12 = message[0]; - - fprintf(stderr,"ant_version_long:\n"); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); - fprintf(stderr,"\tdata3: %d (0x%02X)\n",data3,data3); - fprintf(stderr,"\tdata4: %d (0x%02X)\n",data4,data4); - fprintf(stderr,"\tdata5: %d (0x%02X)\n",data5,data5); - fprintf(stderr,"\tdata6: %d (0x%02X)\n",data6,data6); - fprintf(stderr,"\tdata7: %d (0x%02X)\n",data7,data7); - fprintf(stderr,"\tdata8: %d (0x%02X)\n",data8,data8); - fprintf(stderr,"\tdata9: %d (0x%02X)\n",data9,data9); - fprintf(stderr,"\tdata10: %d (0x%02X)\n",data10,data10); - fprintf(stderr,"\tdata11: %d (0x%02X)\n",data11,data11); - fprintf(stderr,"\tdata12: %d (0x%02X)\n",data12,data12); -} else -// transfer_seq_number_error -if (( message[0]==0x40 ) && ( message[0]==0x20 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"transfer_seq_number_error:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// invalid_message -if (( message[0]==0x40 ) && ( message[0]==0x28 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"invalid_message:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// invalid_network_number -if (( message[0]==0x40 ) && ( message[0]==0x29 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - - fprintf(stderr,"invalid_network_number:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); -} else -// channel_response -if (( message[0]==0x40 )) { - unsigned char channel = message[0]; - unsigned char message_id = message[0]; - unsigned char message_code = message[0]; - - fprintf(stderr,"channel_response:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmessage_id: %d (0x%02X)\n",message_id,message_id); - fprintf(stderr,"\tmessage_code: %d (0x%02X)\n",message_code,message_code); -} else -// extended_broadcast_data -if (( message[0]==0x5d )) { - unsigned char channel = message[0]; - unsigned short device_number = (message[1]<<8)+(message[0]); - unsigned char device_type = message[0]; - unsigned char transmission_type = message[0]; - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - unsigned char data3 = message[0]; - unsigned char data4 = message[0]; - unsigned char data5 = message[0]; - unsigned char data6 = message[0]; - unsigned char data7 = message[0]; - - fprintf(stderr,"extended_broadcast_data:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tdevice_number: %d\n",device_number); - fprintf(stderr,"\tdevice_type: %d (0x%02X)\n",device_type,device_type); - fprintf(stderr,"\ttransmission_type: %d (0x%02X)\n",transmission_type,transmission_type); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); - fprintf(stderr,"\tdata3: %d (0x%02X)\n",data3,data3); - fprintf(stderr,"\tdata4: %d (0x%02X)\n",data4,data4); - fprintf(stderr,"\tdata5: %d (0x%02X)\n",data5,data5); - fprintf(stderr,"\tdata6: %d (0x%02X)\n",data6,data6); - fprintf(stderr,"\tdata7: %d (0x%02X)\n",data7,data7); -} else -// extended_ack_data -if (( message[0]==0x5e )) { - unsigned char channel = message[0]; - unsigned short device_number = (message[1]<<8)+(message[0]); - unsigned char device_type = message[0]; - unsigned char transmission_type = message[0]; - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - unsigned char data3 = message[0]; - unsigned char data4 = message[0]; - unsigned char data5 = message[0]; - unsigned char data6 = message[0]; - unsigned char data7 = message[0]; - - fprintf(stderr,"extended_ack_data:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tdevice_number: %d\n",device_number); - fprintf(stderr,"\tdevice_type: %d (0x%02X)\n",device_type,device_type); - fprintf(stderr,"\ttransmission_type: %d (0x%02X)\n",transmission_type,transmission_type); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); - fprintf(stderr,"\tdata3: %d (0x%02X)\n",data3,data3); - fprintf(stderr,"\tdata4: %d (0x%02X)\n",data4,data4); - fprintf(stderr,"\tdata5: %d (0x%02X)\n",data5,data5); - fprintf(stderr,"\tdata6: %d (0x%02X)\n",data6,data6); - fprintf(stderr,"\tdata7: %d (0x%02X)\n",data7,data7); -} else -// extended_burst_data -if (( message[0]==0x5f )) { - unsigned char channel = message[0]; - unsigned short device_number = (message[1]<<8)+(message[0]); - unsigned char device_type = message[0]; - unsigned char transmission_type = message[0]; - unsigned char data0 = message[0]; - unsigned char data1 = message[0]; - unsigned char data2 = message[0]; - unsigned char data3 = message[0]; - unsigned char data4 = message[0]; - unsigned char data5 = message[0]; - unsigned char data6 = message[0]; - unsigned char data7 = message[0]; - - fprintf(stderr,"extended_burst_data:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tdevice_number: %d\n",device_number); - fprintf(stderr,"\tdevice_type: %d (0x%02X)\n",device_type,device_type); - fprintf(stderr,"\ttransmission_type: %d (0x%02X)\n",transmission_type,transmission_type); - fprintf(stderr,"\tdata0: %d (0x%02X)\n",data0,data0); - fprintf(stderr,"\tdata1: %d (0x%02X)\n",data1,data1); - fprintf(stderr,"\tdata2: %d (0x%02X)\n",data2,data2); - fprintf(stderr,"\tdata3: %d (0x%02X)\n",data3,data3); - fprintf(stderr,"\tdata4: %d (0x%02X)\n",data4,data4); - fprintf(stderr,"\tdata5: %d (0x%02X)\n",data5,data5); - fprintf(stderr,"\tdata6: %d (0x%02X)\n",data6,data6); - fprintf(stderr,"\tdata7: %d (0x%02X)\n",data7,data7); -} else -// startup_message -if (( message[0]==0x6f )) { - unsigned char start_message = message[0]; - - fprintf(stderr,"startup_message:\n"); - fprintf(stderr,"\tstart_message: %d (0x%02X)\n",start_message,start_message); -} else -// calibration_request -if (( message[0]==0x01 ) && ( message[0]==0xaa )) { - unsigned char channel = message[0]; - - fprintf(stderr,"calibration_request:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); -} else -// srm_zero_response -if (( message[0]==0x01 ) && ( message[0]==0x10 ) && ( message[0]==0x01 )) { - unsigned char channel = message[0]; - unsigned short offset = (message[0]<<8)+(message[1]); - - fprintf(stderr,"srm_zero_response:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\toffset: %d\n",offset); -} else -// calibration_pass -if (( message[0]==0x01 ) && ( message[0]==0xac )) { - unsigned char channel = message[0]; - unsigned char autozero_status = message[0]; - unsigned short calibration_data = (message[1]<<8)+(message[0]); - - fprintf(stderr,"calibration_pass:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tautozero_status: %d (0x%02X)\n",autozero_status,autozero_status); - fprintf(stderr,"\tcalibration_data: %d\n",calibration_data); -} else -// calibration_fail -if (( message[0]==0x01 ) && ( message[0]==0xaf )) { - unsigned char channel = message[0]; - unsigned char autozero_status = message[0]; - unsigned short calibration_data = (message[1]<<8)+(message[0]); - - fprintf(stderr,"calibration_fail:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tautozero_status: %d (0x%02X)\n",autozero_status,autozero_status); - fprintf(stderr,"\tcalibration_data: %d\n",calibration_data); -} else -// torque_support -if (( message[0]==0x01 ) && ( message[0]==0x12 )) { - unsigned char channel = message[0]; - unsigned char sensor_configuration = message[0]; - signed short raw_torque = (message[1]<<8)+(message[0]); - signed short offset_torque = (message[1]<<8)+(message[0]); - - fprintf(stderr,"torque_support:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tsensor_configuration: %d (0x%02X)\n",sensor_configuration,sensor_configuration); - fprintf(stderr,"\traw_torque: %d\n",raw_torque); - fprintf(stderr,"\toffset_torque: %d\n",offset_torque); -} else -// standard_power -if (( message[0]==0x4e ) && ( message[0]==0x10 )) { - unsigned char channel = message[0]; - unsigned char event_count = message[0]; - unsigned char instant_cadence = message[0]; - unsigned short sum_power = (message[1]<<8)+(message[0]); - unsigned short instant_power = (message[1]<<8)+(message[0]); - - fprintf(stderr,"standard_power:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tevent_count: %d (0x%02X)\n",event_count,event_count); - fprintf(stderr,"\tinstant_cadence: %d (0x%02X)\n",instant_cadence,instant_cadence); - fprintf(stderr,"\tsum_power: %d\n",sum_power); - fprintf(stderr,"\tinstant_power: %d\n",instant_power); -} else -// wheel_torque -if (( message[0]==0x4e ) && ( message[0]==0x11 )) { - unsigned char channel = message[0]; - unsigned char event_count = message[0]; - unsigned char wheel_rev = message[0]; - unsigned char instant_cadence = message[0]; - unsigned short wheel_period = (message[1]<<8)+(message[0]); - unsigned short torque = (message[1]<<8)+(message[0]); - - fprintf(stderr,"wheel_torque:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tevent_count: %d (0x%02X)\n",event_count,event_count); - fprintf(stderr,"\twheel_rev: %d (0x%02X)\n",wheel_rev,wheel_rev); - fprintf(stderr,"\tinstant_cadence: %d (0x%02X)\n",instant_cadence,instant_cadence); - fprintf(stderr,"\twheel_period: %d\n",wheel_period); - fprintf(stderr,"\ttorque: %d\n",torque); -} else -// crank_torque -if (( message[0]==0x4e ) && ( message[0]==0x12 )) { - unsigned char channel = message[0]; - unsigned char event_count = message[0]; - unsigned char crank_rev = message[0]; - unsigned char instant_cadence = message[0]; - unsigned short crank_period = (message[1]<<8)+(message[0]); - unsigned short torque = (message[1]<<8)+(message[0]); - - fprintf(stderr,"crank_torque:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tevent_count: %d (0x%02X)\n",event_count,event_count); - fprintf(stderr,"\tcrank_rev: %d (0x%02X)\n",crank_rev,crank_rev); - fprintf(stderr,"\tinstant_cadence: %d (0x%02X)\n",instant_cadence,instant_cadence); - fprintf(stderr,"\tcrank_period: %d\n",crank_period); - fprintf(stderr,"\ttorque: %d\n",torque); -} else -// crank_SRM -if (( message[0]==0x4e ) && ( message[0]==0x20 )) { - unsigned char channel = message[0]; - unsigned char event_count = message[0]; - unsigned short slope = (message[0]<<8)+(message[1]); - unsigned short crank_period = (message[0]<<8)+(message[1]); - unsigned short torque = (message[0]<<8)+(message[1]); - - fprintf(stderr,"crank_SRM:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tevent_count: %d (0x%02X)\n",event_count,event_count); - fprintf(stderr,"\tslope: %d\n",slope); - fprintf(stderr,"\tcrank_period: %d\n",crank_period); - fprintf(stderr,"\ttorque: %d\n",torque); -} else -// manufacturer -if (( message[0]==0x4e ) && ( message[0]==0x50 )) { - unsigned char channel = message[0]; - unsigned char hw_rev = message[0]; - unsigned short manufacturer_id = (message[1]<<8)+(message[0]); - unsigned short model_number_id = (message[1]<<8)+(message[0]); - - fprintf(stderr,"manufacturer:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\thw_rev: %d (0x%02X)\n",hw_rev,hw_rev); - fprintf(stderr,"\tmanufacturer_id: %d\n",manufacturer_id); - fprintf(stderr,"\tmodel_number_id: %d\n",model_number_id); -} else -// product -if (( message[0]==0x4e ) && ( message[0]==0x51 )) { - unsigned char channel = message[0]; - unsigned char sw_rev = message[0]; - unsigned short serial_number_qpod = (message[1]<<8)+(message[0]); - unsigned short serial_number_spider = (message[1]<<8)+(message[0]); - - fprintf(stderr,"product:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tsw_rev: %d (0x%02X)\n",sw_rev,sw_rev); - fprintf(stderr,"\tserial_number_qpod: %d\n",serial_number_qpod); - fprintf(stderr,"\tserial_number_spider: %d\n",serial_number_spider); -} else -// battery_voltage -if (( message[0]==0x4e ) && ( message[0]==0x52 )) { - unsigned char channel = message[0]; - unsigned char operating_time_lsb = message[0]; - unsigned char operating_time_1sb = message[0]; - unsigned char operating_time_msb = message[0]; - unsigned char voltage_lsb = message[0]; - unsigned char descriptive_bits = message[0]; - - fprintf(stderr,"battery_voltage:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\toperating_time_lsb: %d (0x%02X)\n",operating_time_lsb,operating_time_lsb); - fprintf(stderr,"\toperating_time_1sb: %d (0x%02X)\n",operating_time_1sb,operating_time_1sb); - fprintf(stderr,"\toperating_time_msb: %d (0x%02X)\n",operating_time_msb,operating_time_msb); - fprintf(stderr,"\tvoltage_lsb: %d (0x%02X)\n",voltage_lsb,voltage_lsb); - fprintf(stderr,"\tdescriptive_bits: %d (0x%02X)\n",descriptive_bits,descriptive_bits); -} else -// heart_rate -if (( message[0]==0x4e )) { - unsigned char channel = message[0]; - unsigned short measurement_time = (message[1]<<8)+(message[0]); - unsigned char beats = message[0]; - unsigned char instant_heart_rate = message[0]; - - fprintf(stderr,"heart_rate:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmeasurement_time: %d\n",measurement_time); - fprintf(stderr,"\tbeats: %d (0x%02X)\n",beats,beats); - fprintf(stderr,"\tinstant_heart_rate: %d (0x%02X)\n",instant_heart_rate,instant_heart_rate); -} else -// speed -if (( message[0]==0x4e )) { - unsigned char channel = message[0]; - unsigned short measurement_time = (message[1]<<8)+(message[0]); - unsigned short wheel_revs = (message[1]<<8)+(message[0]); - - fprintf(stderr,"speed:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmeasurement_time: %d\n",measurement_time); - fprintf(stderr,"\twheel_revs: %d\n",wheel_revs); -} else -// cadence -if (( message[0]==0x4e )) { - unsigned char channel = message[0]; - unsigned short measurement_time = (message[1]<<8)+(message[0]); - unsigned short crank_revs = (message[1]<<8)+(message[0]); - - fprintf(stderr,"cadence:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tmeasurement_time: %d\n",measurement_time); - fprintf(stderr,"\tcrank_revs: %d\n",crank_revs); -} else -// speed_cadence -if (( message[0]==0x4e )) { - unsigned char channel = message[0]; - unsigned short cadence_measurement_time = (message[1]<<8)+(message[0]); - unsigned short crank_revs = (message[1]<<8)+(message[0]); - unsigned short speed_measurement_time = (message[1]<<8)+(message[0]); - unsigned short wheel_revs = (message[1]<<8)+(message[0]); - - fprintf(stderr,"speed_cadence:\n"); - fprintf(stderr,"\tchannel: %d (0x%02X)\n",channel,channel); - fprintf(stderr,"\tcadence_measurement_time: %d\n",cadence_measurement_time); - fprintf(stderr,"\tcrank_revs: %d\n",crank_revs); - fprintf(stderr,"\tspeed_measurement_time: %d\n",speed_measurement_time); - fprintf(stderr,"\twheel_revs: %d\n",wheel_revs); -} else - { ; } -} + return -1; +} diff --git a/src/ANT.h b/src/ANT.h index 5cc4b36b1..d4262e84b 100644 --- a/src/ANT.h +++ b/src/ANT.h @@ -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 diff --git a/src/ANTChannel.cpp b/src/ANTChannel.cpp new file mode 100755 index 000000000..4fa6d99f4 --- /dev/null +++ b/src/ANTChannel.cpp @@ -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 + +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"<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)); +} diff --git a/src/ANTChannel.h b/src/ANTChannel.h new file mode 100755 index 000000000..5a9317aab --- /dev/null +++ b/src/ANTChannel.h @@ -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 diff --git a/src/ANTMessage.cpp b/src/ANTMessage.cpp new file mode 100755 index 000000000..a1273a553 --- /dev/null +++ b/src/ANTMessage.cpp @@ -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 + +// +// 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); +} diff --git a/src/ANTMessage.h b/src/ANTMessage.h new file mode 100755 index 000000000..0ad4c23c0 --- /dev/null +++ b/src/ANTMessage.h @@ -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 diff --git a/src/ANTlocalController.cpp b/src/ANTlocalController.cpp index aa6604afe..1e6d1704b 100644 --- a/src/ANTlocalController.cpp +++ b/src/ANTlocalController.cpp @@ -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 diff --git a/src/src.pro b/src/src.pro index 8abb79c4b..b14e6a06d 100644 --- a/src/src.pro +++ b/src/src.pro @@ -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 \