Files
GoldenCheetah/src/Fortius.cpp
Mark Liversedge 43b1c61d6b Add Device Wizard
Introduce a wizard for adding realtime devices since it
is prone to user error, is relatively complicated and is
something most users will only ever do once or twice.

There are several logical updates within this patch:

First; Fix intermittent ANT+ not working
* LibUsb     - check bufRemaining is > 0 not non-zero
* LibUsb     - Always reset the USB stick on open
* ANT.cpp    - Support > 4 channels on USB2
* ANTChannel -  Do not use pairing, but always
*               unassign, assign and set channel id.

Second; Fix device discovery
* Find and discover support in realtime controllers
* Extend Serial.cpp to cover more Serial devices
* Support for 4 or 8 ANT channels for USB1/USB2

Third; Introduce Add Device Wizard with
* General and Specific wizard pages for each device type
* Device pairing with telemetry display
* fixed compile time warnings

Fourth; Update Device Config Page
* use wizard to add new device
* remove edit fields, replaced by wizard
* remove pair, firmware buttons replaced by wizard

Fifth; Deprecate/Remove Device Types
* Null Device - used by developers only
* Quarqd client - replaced by Native ANT+
* GC Server - not implemented yet

I have also introduced a device specific wheel size
and flags for controlling the default selection and
sources for sensor data when in multi-device mode. These
are not yet supported in the code.

Fixes #611.
Fixes #497.
2012-01-21 20:28:33 +00:00

529 lines
15 KiB
C++

/*
* 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 "Fortius.h"
//
// Outbound control message has the format:
// Byte Value / Meaning
// 0 0x01 CONSTANT
// 1 0x08 CONSTANT
// 2 0x01 CONSTANT
// 3 0x00 CONSTANT
// 4 Brake Value - Lo Byte
// 5 Brake Value - Hi Byte
// 6 Echo cadence sensor
// 7 0x00 -- UNKNOWN
// 8 0x02 -- 0 - idle 2 - Active
// 9 0x52 -- Mode (?) 52 = idle, 0a = ergo, 48 = slope.
// 10 0x10 -- UNKNOWN - CONSTANT?
// 11 0x04 -- UNKNOWN - CONSTANT?
const static uint8_t ergo_command[12] = {
// 0 1 2 3 4 5 6 7 8 9 10 11
0x01, 0x08, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x0a, 0x10, 0x04
};
const static uint8_t slope_command[12] = {
// 0 1 2 3 4 5 6 7 8 9 10 11
0x01, 0x08, 0x01, 0x00, 0x6c, 0x01, 0x00, 0x00, 0x02, 0x48, 0x10, 0x04
};
/* ----------------------------------------------------------------------
* CONSTRUCTOR/DESRTUCTOR
* ---------------------------------------------------------------------- */
Fortius::Fortius(QObject *parent) : QThread(parent)
{
devicePower = deviceHeartRate = deviceCadence = deviceSpeed = 0.00;
mode = FT_ERGOMODE;
load = DEFAULT_LOAD;
gradient = DEFAULT_GRADIENT;
deviceStatus=0;
this->parent = parent;
/* 56 byte control sequence, composed of 8 command packets
* where the last packet sets the load. The first byte
* is a CRC for the value being issued (e.g. Load in WATTS)
*
* these members are modified as load / gradient are set
*/
memcpy(ERGO_Command, ergo_command, 12);
memcpy(SLOPE_Command, slope_command, 12);
// for interacting over the USB port
usb2 = new LibUsb(TYPE_FORTIUS);
}
Fortius::~Fortius()
{
}
/* ----------------------------------------------------------------------
* SET
* ---------------------------------------------------------------------- */
void Fortius::setMode(int mode, double load, double gradient)
{
pvars.lock();
this->mode = mode;
this->load = load;
this->gradient = gradient;
pvars.unlock();
}
void Fortius::setLoad(double load)
{
pvars.lock();
// we can only do 50-800w on a Fortius
if (load > 1000) load = 1000;
if (load < 50) load = 50;
this->load = load;
pvars.unlock();
}
void Fortius::setGradient(double gradient)
{
pvars.lock();
this->gradient = gradient;
pvars.unlock();
}
/* ----------------------------------------------------------------------
* GET
* ---------------------------------------------------------------------- */
void Fortius::getTelemetry(double &power, double &heartrate, double &cadence, double &speed, int &buttons, int &status)
{
pvars.lock();
power = devicePower;
heartrate = deviceHeartRate;
cadence = deviceCadence;
speed = deviceSpeed;
buttons = deviceButtons;
status = deviceStatus;
pvars.unlock();
}
int Fortius::getMode()
{
int tmp;
pvars.lock();
tmp = mode;
pvars.unlock();
return tmp;
}
double Fortius::getLoad()
{
double tmp;
pvars.lock();
tmp = load;
pvars.unlock();
return tmp;
}
double Fortius::getGradient()
{
double tmp;
pvars.lock();
tmp = gradient;
pvars.unlock();
return tmp;
}
int
Fortius::start()
{
QThread::start();
return 0;
}
void Fortius::prepareCommand(int mode, double value)
{
// prepare the control message according to the current mode and gradient/load
// we do not put the brake on for speeds less than 10kph to ensure it does
// not burn out
int16_t encoded;
switch (mode) {
case FT_ERGOMODE :
if (deviceSpeed < 10) encoded =0;
else encoded = 10 * value;
qToLittleEndian<int16_t>(encoded, &ERGO_Command[4]); // little endian
break;
case FT_SSMODE :
if (deviceSpeed < 10) encoded =0;
else encoded = 650 * value;
qToLittleEndian<int16_t>(encoded, &SLOPE_Command[4]); // little endian
break;
}
}
/* ----------------------------------------------------------------------
* EXECUTIVE FUNCTIONS
*
* start() - start/re-start reading telemetry in a thread
* stop() - stop reading telemetry and terminates thread
* pause() - discards inbound telemetry (ignores it)
*
*
* THE MEAT OF THE CODE IS IN RUN() IT IS A WHILE LOOP CONSTANTLY
* READING TELEMETRY AND ISSUING CONTROL COMMANDS WHILST UPDATING
* MEMBER VARIABLES AS TELEMETRY CHANGES ARE FOUND.
*
* run() - bg thread continuosly reading/writing the device port
* it is kicked off by start and then examines status to check
* when it is time to pause or stop altogether.
* ---------------------------------------------------------------------- */
int Fortius::restart()
{
int status;
// get current status
pvars.lock();
status = this->deviceStatus;
pvars.unlock();
// what state are we in anyway?
if (status&FT_RUNNING && status&FT_PAUSED) {
status &= ~FT_PAUSED;
pvars.lock();
this->deviceStatus = status;
pvars.unlock();
return 0; // ok its running again!
}
return 2;
}
int Fortius::stop()
{
// what state are we in anyway?
pvars.lock();
deviceStatus = 0; // Terminate it!
pvars.unlock();
return 0;
}
int Fortius::pause()
{
int status;
// get current status
pvars.lock();
status = this->deviceStatus;
pvars.unlock();
if (status&FT_PAUSED) return 2; // already paused you muppet!
else if (!(status&FT_RUNNING)) return 4; // not running anyway, fool!
else {
// ok we're running and not paused so lets pause
status |= FT_PAUSED;
pvars.lock();
this->deviceStatus = status;
pvars.unlock();
return 0;
}
}
// used by thread to set variables and emit event if needed
// on unexpected exit
int Fortius::quit(int code)
{
// event code goes here!
exit(code);
return 0; // never gets here obviously but shuts up the compiler!
}
/*----------------------------------------------------------------------
* THREADED CODE - READS TELEMETRY AND SENDS COMMANDS TO KEEP FORTIUS ALIVE
*----------------------------------------------------------------------*/
void Fortius::run()
{
// newly read values - compared against cached values
double newload, newgradient;
bool isDeviceOpen = false;
// Cached current values
// when new values are received from the device
// if they differ from current values we update
// otherwise do nothing
int curmode, curstatus;
double curload, curgradient;
double curPower; // current output power in Watts
double curHeartRate; // current heartrate in BPM
double curCadence; // current cadence in RPM
double curSpeed; // current speef in KPH
int curButtons; // Button status
// we need to average out power for the last second
// since we get updates every 16ms (60hz)
int powerhist[16]; // last 60s values received
int powertot=0; // running total
int powerindex=0; // index into the powerhist array
for (int i=0; i<16; i++) powerhist[i]=0;
// initialise local cache & main vars
pvars.lock();
this->deviceStatus = FT_RUNNING;
curmode = this->mode;
curload = this->load;
curgradient = this->gradient;
curPower = this->devicePower = 0;
curHeartRate = this->deviceHeartRate = 0;
curCadence = this->deviceCadence = 0;
curSpeed = this->deviceSpeed = 0;
curButtons = this->deviceButtons;
curButtons = 0;
this->deviceButtons = 0;
pvars.unlock();
// open the device
if (openPort()) {
quit(2);
return; // open failed!
} else {
isDeviceOpen = true;
}
// send first command
prepareCommand(curmode, curmode == FT_ERGOMODE ? curload : curgradient);
if (sendCommand(curmode) == -1) {
// send failed - ouch!
closePort(); // need to release that file handle!!
quit(4);
return; // couldn't write to the device
}
QTime timer;
timer.start();
while(1) {
if (isDeviceOpen == true) {
if (readMessage() > 0) {
msleep(60); //slow down - need to wait for previous interrupt to clear
// before reading. Not sure why, but solves issues
// when working on OSX, and possibly Windows
//----------------------------------------------------------------
// UPDATE BASIC TELEMETRY (HR, CAD, SPD et al)
// The data structure is very simple, no bit twiddling needed here
//----------------------------------------------------------------
// buttons
curButtons = buf[13];
// brake status status&0x04 == stopping wheel
// status&0x01 == brake on
// buf[42];
// cadence
curCadence = buf[44];
// speed
curSpeed = (double)(qFromLittleEndian<quint16>(&buf[32])) / 360.00f;
// power
int power = qFromLittleEndian<qint16>(&buf[38])/10;
powertot += power;
powertot -= powerhist[powerindex];
powerhist[powerindex] = power;
curPower = powertot / 16;
powerindex = (powerindex == 15) ? 0 : powerindex+1;
// heartrate
curHeartRate = buf[12];
#if 0
// debug
fprintf(stderr,"%08d:", timer.elapsed());
for(int i=0; i<48; i++) fprintf(stderr, "%02x ", buf[i]);
fprintf(stderr, "\n");
#endif
} else {
// no data available!? It should block in libusb.. but lets sleep anyway
msleep (50);
}
}
//----------------------------------------------------------------
// LISTEN TO GUI CONTROL COMMANDS
//----------------------------------------------------------------
pvars.lock();
curstatus = this->deviceStatus;
load = curload = newload = this->load;
gradient = curgradient = newgradient = this->gradient;
// whilst we are here lets update the values
deviceButtons = curButtons;
deviceSpeed = curSpeed;
deviceCadence = curCadence;
deviceHeartRate = curHeartRate;
devicePower = curPower;
pvars.unlock();
/* time to shut up shop */
if (!(curstatus&FT_RUNNING)) {
// time to stop!
closePort(); // need to release that file handle!!
quit(0);
return;
}
if ((curstatus&FT_PAUSED) && isDeviceOpen == true) {
closePort();
isDeviceOpen = false;
} else if (!(curstatus&FT_PAUSED) && (curstatus&FT_RUNNING) && isDeviceOpen == false) {
if (openPort()) {
quit(2);
return; // open failed!
}
isDeviceOpen = true;
timer.restart();
// reset smoothing.
powertot = 0;
powerindex = 0;
for (int i=0; i<16; i++) powerhist[i]=0;
// send first command to get fortius ready
prepareCommand(curmode, curmode == FT_ERGOMODE ? curload : curgradient);
if (sendCommand(curmode) == -1) {
// send failed - ouch!
closePort(); // need to release that file handle!!
quit(4);
return; // couldn't write to the device
}
}
//----------------------------------------------------------------
// KEEP THE FORTIUS CONNECTION ALIVE
//----------------------------------------------------------------
if (isDeviceOpen == true) {
prepareCommand(curmode, curmode == FT_ERGOMODE ? curload : curgradient);
if (sendCommand(curmode) == -1) {
// send failed - ouch!
closePort(); // need to release that file handle!!
quit(4);
return; // couldn't write to the device
}
}
}
}
/* ----------------------------------------------------------------------
* LOW LEVEL DEVICE IO ROUTINES - PORT TO QIODEVICE REQUIRED BEFORE COMMIT
*
*
* HIGH LEVEL IO
* int sendCommand() - writes a command to the device
* int readMessage() - reads an inbound message
*
* LOW LEVEL IO
* openPort() - opens serial device and configures it
* closePort() - closes serial device and releases resources
* rawRead() - non-blocking read of inbound data
* rawWrite() - non-blocking write of outbound data
* discover() - check if a ct is attached to the port specified
* ---------------------------------------------------------------------- */
int Fortius::sendCommand(int mode) // writes a command to the device
{
switch (mode) {
case FT_ERGOMODE :
return rawWrite(ERGO_Command, 12);
break;
case FT_SSMODE :
return rawWrite(SLOPE_Command, 12);
break;
default :
return -1;
break;
}
}
int Fortius::readMessage()
{
int rc;
rc = rawRead(buf, 48);
return rc;
}
int Fortius::closePort()
{
usb2->close();
return 0;
}
bool Fortius::find()
{
return usb2->find();
}
int Fortius::openPort()
{
// on windows we try on USB2 then on USB1 then fail...
return usb2->open();
}
int Fortius::rawWrite(uint8_t *bytes, int size) // unix!!
{
int rc=0;
rc = usb2->write((char *)bytes, size);
if (!rc) rc = -1; // return -1 if nothing written
return rc;
}
int Fortius::rawRead(uint8_t bytes[], int size)
{
return usb2->read((char *)bytes, size);
}
// check to see of there is a port at the device specified
// returns true if the device exists and false if not
bool Fortius::discover(QString)
{
return true;
}