visus
/
teensy-usbhost
Watch 1
Star 0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
Browse Source

Move AntPlus parsing state from static to object scope

main
PaulStoffregen 7 years ago
parent
d8c4d55dad
commit
dc1030f379
3 changed files with 184 additions and 254 deletions
Split View Show Diff Stats
  1. +89
    -6
      USBHost_t36.h
  2. +90
    -245
      antplus.cpp
  3. +5
    -3
      antplusdefs.h

+ 89
- 6
USBHost_t36.h View File

@@ -936,13 +936,12 @@ private:
uint8_t channelStatusOld;
} flags;
} TDCONFIG;
typedef struct {
struct {
uint8_t initOnce;
uint8_t key; // key index
int iDevice; // index to the antplus we're interested in, if > one found
TDCONFIG dcfg[PROFILE_TOTAL]; // channel config, we're using one channel per device
} TLIBANTPLUS;
TLIBANTPLUS ant;
} ant;
int (*callbackFunc)(uint32_t msg, intptr_t *value1, uint32_t value2);
void (*user_onStatusChange)(int channel, int status);
void (*user_onDeviceID)(int channel, int devId, int devType, int transType);
@@ -986,21 +985,105 @@ private:
const int devType, const int TranType, const uint8_t *data);
int SendExtBurstTransfer(const int channel, const int devNum, const int devType,
const int tranType, const uint8_t *data, const int nunPackets);

static void profileSetup_HRM(TDCONFIG *cfg, const uint32_t deviceId);
static void profileSetup_SPDCAD(TDCONFIG *cfg, const uint32_t deviceId);
static void profileSetup_POWER(TDCONFIG *cfg, const uint32_t deviceId);
static void profileSetup_STRIDE(TDCONFIG *cfg, const uint32_t deviceId);
static void profileSetup_SPEED(TDCONFIG *cfg, const uint32_t deviceId);
static void profileSetup_CADENCE(TDCONFIG *cfg, const uint32_t deviceId);

struct {
struct {
uint16_t time;
uint16_t interval;
uint8_t bpm; // heart rate in beats per minute
uint8_t sequence;
} current;
struct {
uint8_t bpm;
uint8_t sequence;
uint16_t time;
uint16_t interval;
} previous;
} hrm;
void payload_HRM(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength);
struct {
struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
} current;
struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
} previous;
uint16_t wheelCircumference; // default is WHEEL_CIRCUMFERENCE (2122cm)
} spdcad;
void payload_SPDCAD(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength);
/* struct {
struct {
uint8_t sequence;
uint16_t pedalPowerContribution;
uint8_t pedalPower;
uint8_t instantCadence;
uint16_t sumPower;
uint16_t instantPower;
} current;
struct {
uint16_t stub;
} previous;
} pwr; */
void payload_POWER(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength);
struct {
struct {
uint16_t speed;
uint16_t cadence;
uint8_t strides;
} current;
/* struct {
uint8_t strides;
uint16_t speed;
uint16_t cadence;
} previous; */
} stride;
void payload_STRIDE(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength);
struct {
struct {
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
} current;
struct {
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
} previous;
uint16_t wheelCircumference; // default is WHEEL_CIRCUMFERENCE (2122cm)
} spd;
void payload_SPEED(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength);
struct {
struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
} current;
struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
} previous;
} cad;
void payload_CADENCE(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength);

};



+ 90
- 245
antplus.cpp View File

@@ -1204,292 +1204,137 @@ const uint8_t * AntPlus::getAntKey(const uint8_t keyIdx)
}


#define WHEEL_CIRCUMFERENCE 2122


typedef struct {
struct {
uint16_t time;
uint16_t interval;
uint8_t bpm; // heart rate in beats per minute
uint8_t sequence;
}current;

struct {
uint8_t bpm;
uint8_t sequence;
uint16_t time;
uint16_t interval;
}previous;
}payload_HRM_t;

typedef struct {
struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
}current;

struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
}previous;

uint16_t wheelCircumference; // default is WHEEL_CIRCUMFERENCE (2122cm)
uint8_t spdChange;
uint8_t cadChange;
}payload_SPDCAD_t;



typedef struct {
struct {
uint8_t sequence;
uint16_t pedalPowerContribution;
uint8_t pedalPower;
uint8_t instantCadence;
uint16_t sumPower;
uint16_t instantPower;
} current;
struct {
uint16_t stub;
} previous;
}payload_POWER_t;

typedef struct {
struct {
uint16_t speed;
uint16_t cadence;
uint8_t strides;
}current;

struct {
uint8_t strides;
uint16_t speed;
uint16_t cadence;
}previous;
}payload_STRIDE_t;

typedef struct {
struct {
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
}current;

struct {
uint16_t speedTime;
uint16_t speed;
uint16_t speedCt;
uint32_t distance;
}previous;

uint16_t wheelCircumference; // default is WHEEL_CIRCUMFERENCE (2122cm)
uint8_t spdChange;
}payload_SPEED_t;

typedef struct {
struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
}current;

struct {
uint16_t cadenceTime;
uint16_t cadence;
uint16_t cadenceCt;
}previous;

uint8_t cadChange;
}payload_CADENCE_t;



typedef struct {
payload_HRM_t hrm;
payload_SPDCAD_t spdcad;
payload_POWER_t power;
payload_STRIDE_t stride;
payload_SPEED_t spd;
payload_CADENCE_t cad;
}payload_genstorage_t;

static payload_genstorage_t payload; // FIXME: should be in the AntPlus object



void AntPlus::payload_HRM(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
payload_HRM_t *hrm = &payload.hrm;
hrm->current.bpm = data[STREAM_RXBROADCAST_DEV120_HR];
hrm->current.sequence = data[STREAM_RXBROADCAST_DEV120_SEQ];

hrm.current.bpm = data[STREAM_RXBROADCAST_DEV120_HR];
hrm.current.sequence = data[STREAM_RXBROADCAST_DEV120_SEQ];
//const int page = data[1]&0x0F;
if (hrm->previous.sequence != hrm->current.sequence || hrm->previous.bpm != hrm->current.bpm){
if (hrm->current.bpm){
hrm->current.time = (data[STREAM_RXBROADCAST_DEV120_BEATLO] + (data[STREAM_RXBROADCAST_DEV120_BEATHI] << 8));
hrm->current.interval = hrm->current.time - hrm->previous.time;
hrm->current.interval = (((int32_t)hrm->current.interval) * (int32_t)1000) / (int32_t)1024;

if (hrm.previous.sequence != hrm.current.sequence || hrm.previous.bpm != hrm.current.bpm){
if (hrm.current.bpm) {
hrm.current.time = (data[STREAM_RXBROADCAST_DEV120_BEATLO]
+ (data[STREAM_RXBROADCAST_DEV120_BEATHI] << 8));
hrm.current.interval = hrm.current.time - hrm.previous.time;
hrm.current.interval = (((int32_t)hrm.current.interval)
* (int32_t)1000) / (int32_t)1024;
//printf("page %i", page);
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)hrm, PROFILE_HRM);

hrm->previous.time = hrm->current.time;
hrm->previous.interval = hrm->current.interval;
hrm->previous.sequence = hrm->current.sequence;
hrm->previous.bpm = hrm->current.bpm;
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)&hrm, PROFILE_HRM);
hrm.previous.time = hrm.current.time;
hrm.previous.interval = hrm.current.interval;
hrm.previous.sequence = hrm.current.sequence;
hrm.previous.bpm = hrm.current.bpm;
}
}

//int page = data[1]&0x0F;
//printf("payload_HRM: page:%i, Sequence:%i, BPM:%i, %i %i", page, hrm->current.sequence, hrm->current.bpm, hrm->current.time, hrm->current.interval);
printf("payload_HRM: page:%i, Sequence:%i, BPM:%i, %i %i", data[1]&0x0F,
hrm.current.sequence, hrm.current.bpm, hrm.current.time, hrm.current.interval);
}

void AntPlus::payload_SPDCAD(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
payload_SPDCAD_t *spdcad = &payload.spdcad;

spdcad->current.cadenceTime = data[1];
spdcad->current.cadenceTime |= (data[2] << 8);
spdcad->current.cadenceCt = data[3];
spdcad->current.cadenceCt |= (data[4] << 8);
spdcad->current.speedTime = data[5];
spdcad->current.speedTime |= (data[6] << 8);
spdcad->current.speedCt = data[7];
spdcad->current.speedCt |= (data[8] << 8);

spdcad->cadChange = (spdcad->current.cadenceTime != spdcad->previous.cadenceTime || spdcad->current.cadenceCt != spdcad->previous.cadenceCt);
spdcad->spdChange = (spdcad->current.speedTime != spdcad->previous.speedTime || spdcad->current.speedCt != spdcad->previous.speedCt);

if (spdcad->cadChange || spdcad->spdChange){
uint16_t cadence = (60 * (spdcad->current.cadenceCt - spdcad->previous.cadenceCt) * 1024) / (spdcad->current.cadenceTime - spdcad->previous.cadenceTime);
spdcad->current.cadence = cadence;

if (!spdcad->wheelCircumference) spdcad->wheelCircumference = WHEEL_CIRCUMFERENCE;
uint32_t speedRotationDelta = spdcad->current.speedCt - spdcad->previous.speedCt; // number wheel revolutions
float speedTimeDelta = (float)(spdcad->current.speedTime - spdcad->previous.speedTime) / 1024.0f; // time for above revolutions
float distance = (speedRotationDelta * (float)spdcad->wheelCircumference) / 1000.0f; // calculated distance (meters) travelled as per above
float speed = (distance / (speedTimeDelta / 3600.0f)) / 1000.0f; // its why we're here
spdcad->current.speed = speed * 100;
spdcad->current.distance += distance;

sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)spdcad, PROFILE_SPDCAD);

spdcad->previous.cadenceTime = spdcad->current.cadenceTime;
spdcad->previous.cadence = spdcad->current.cadence;
spdcad->previous.cadenceCt = spdcad->current.cadenceCt;

spdcad->previous.speedTime = spdcad->current.speedTime;
spdcad->previous.speedCt = spdcad->current.speedCt;
spdcad->previous.speed = spdcad->current.speed;
spdcad->previous.distance = spdcad->current.distance;

//printf("payload_SPDCAD: speed: %.2f, cadence: %i, total distance: %.2f", spdcad->current.speed/100.0f, spdcad->current.cadence, spdcad->current.distance/1000.0f);
spdcad.current.cadenceTime = data[1];
spdcad.current.cadenceTime |= (data[2] << 8);
spdcad.current.cadenceCt = data[3];
spdcad.current.cadenceCt |= (data[4] << 8);
spdcad.current.speedTime = data[5];
spdcad.current.speedTime |= (data[6] << 8);
spdcad.current.speedCt = data[7];
spdcad.current.speedCt |= (data[8] << 8);
if (spdcad.current.cadenceTime == spdcad.previous.cadenceTime
&& spdcad.current.cadenceCt != spdcad.previous.cadenceCt
&& spdcad.current.speedTime != spdcad.previous.speedTime
&& spdcad.current.speedCt != spdcad.previous.speedCt) {
return; // no change
}
uint16_t cadence = (60 * (spdcad.current.cadenceCt - spdcad.previous.cadenceCt) * 1024) / (spdcad.current.cadenceTime - spdcad.previous.cadenceTime);
spdcad.current.cadence = cadence;
if (!spdcad.wheelCircumference) spdcad.wheelCircumference = WHEEL_CIRCUMFERENCE;
uint32_t speedRotationDelta = spdcad.current.speedCt - spdcad.previous.speedCt; // number wheel revolutions
float speedTimeDelta = (float)(spdcad.current.speedTime - spdcad.previous.speedTime) / 1024.0f; // time for above revolutions
float distance = (speedRotationDelta * (float)spdcad.wheelCircumference) / 1000.0f; // calculated distance (meters) travelled as per above
float speed = (distance / (speedTimeDelta / 3600.0f)) / 1000.0f; // its why we're here
spdcad.current.speed = speed * 100;
spdcad.current.distance += distance;
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)&spdcad, PROFILE_SPDCAD);
spdcad.previous.cadenceTime = spdcad.current.cadenceTime;
spdcad.previous.cadence = spdcad.current.cadence;
spdcad.previous.cadenceCt = spdcad.current.cadenceCt;
spdcad.previous.speedTime = spdcad.current.speedTime;
spdcad.previous.speedCt = spdcad.current.speedCt;
spdcad.previous.speed = spdcad.current.speed;
spdcad.previous.distance = spdcad.current.distance;
printf("payload_SPDCAD: speed: %.2f, cadence: %i, total distance: %.2f",
spdcad.current.speed/100.0f, spdcad.current.cadence,
spdcad.current.distance/1000.0f);
}

void AntPlus::payload_POWER(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
//printf("payload_POWER: len:%i", dataLength);
payload_POWER_t *pwr = &payload.power;
/*
#if 0
uint8_t eventCount = data[2];
uint8_t pedalPowerContribution = ((data[3] != 0xFF) && (data[3]&0x80)) ; // left/right is defined if NOT 0xFF (= no Pedal Power) AND BIT 7 is set
uint8_t pedalPower = (data[3]&0x7F); // right pedalPower % - stored in bit 0-6
uint8_t instantCadence = data[4];
uint16_t sumPower = data[5] + (data[6]<<8);
uint16_t instantPower = data[7] + (data[8]<<8);
*/
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)pwr, PROFILE_POWER);
#endif
//sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)pwr, PROFILE_POWER);
}

void AntPlus::payload_STRIDE(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
//printf("payload_STRIDE: len:%i", dataLength);
payload_STRIDE_t *stride = &payload.stride;


int page = data[1];
if (page == 0){
stride->current.strides = data[7];
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)stride, PROFILE_STRIDE);
stride->previous.strides = stride->current.strides;

}else if (page == 1){
stride->current.speed = ((float)(data[4]&0x0f) + (float)(data[5]/256.0f));
stride->current.cadence = ((float)data[3] + (float)((data[4] << 4) / 16.0f));

sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)stride, PROFILE_STRIDE);

stride->previous.speed = stride->current.speed;
stride->previous.cadence = stride->current.cadence;
if (page == 0) {
stride.current.strides = data[7];
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)&stride, PROFILE_STRIDE);
//stride.previous.strides = stride.current.strides;
} else if (page == 1) {
stride.current.speed = ((float)(data[4]&0x0f) + (float)(data[5]/256.0f));
stride.current.cadence = ((float)data[3] + (float)((data[4] << 4) / 16.0f));
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)&stride, PROFILE_STRIDE);
//stride.previous.speed = stride.current.speed;
//stride.previous.cadence = stride.current.cadence;
}

}

void AntPlus::payload_SPEED(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
//printf("payload_SPEED: len:%i", dataLength);
payload_SPEED_t *spd = &payload.spd;

spd->current.speedTime = data[5];
spd->current.speedTime |= (data[6] << 8);
spd->current.speedCt = data[7];
spd->current.speedCt |= (data[8] << 8);

spd->spdChange = (spd->current.speedTime != spd->previous.speedTime || spd->current.speedCt != spd->previous.speedCt);

if (spd->spdChange){
uint32_t speedRotationDelta = spd->current.speedCt - spd->previous.speedCt; // number wheel revolutions
float speedTimeDelta = (float)(spd->current.speedTime - spd->previous.speedTime) / 1024.0f; // time for above revolutions
if (!spd->wheelCircumference) spd->wheelCircumference = WHEEL_CIRCUMFERENCE;
float distance = (speedRotationDelta * (float)spd->wheelCircumference) / 1000.0f; // calculated distance (meters) travelled as per above
float speed = (distance / (speedTimeDelta / 3600.0f)) / 1000.0f; // its why we're here
spd->current.speed = speed * 100;
spd->current.distance += distance;

sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)spd, PROFILE_SPEED);

spd->previous.speedTime = spd->current.speedTime;
spd->previous.speedCt = spd->current.speedCt;
spd->previous.speed = spd->current.speed;
spd->previous.distance = spd->current.distance;
spd.current.speedTime = data[5] | (data[6] << 8);
spd.current.speedCt = data[7] | (data[8] << 8);
if (spd.current.speedTime == spd.previous.speedTime
&& spd.current.speedCt == spd.previous.speedCt) {
return; // no change
}
uint32_t speedRotationDelta = spd.current.speedCt - spd.previous.speedCt; // number wheel revolutions
float speedTimeDelta = (float)(spd.current.speedTime - spd.previous.speedTime) / 1024.0f; // time for above revolutions
if (!spd.wheelCircumference) spd.wheelCircumference = WHEEL_CIRCUMFERENCE;
float distance = (speedRotationDelta * (float)spd.wheelCircumference) / 1000.0f; // calculated distance (meters) travelled as per above
float speed = (distance / (speedTimeDelta / 3600.0f)) / 1000.0f; // its why we're here
spd.current.speed = speed * 100;
spd.current.distance += distance;
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)&spd, PROFILE_SPEED);
spd.previous.speedTime = spd.current.speedTime;
spd.previous.speedCt = spd.current.speedCt;
spd.previous.speed = spd.current.speed;
spd.previous.distance = spd.current.distance;
}

void AntPlus::payload_CADENCE(TDCONFIG *cfg, const uint8_t *data, const size_t dataLength)
{
//printf("payload_CADENCE: len:%i", dataLength);
payload_CADENCE_t *cad = &payload.cad;

cad->current.cadenceTime = data[5];
cad->current.cadenceTime |= (data[6] << 8);
cad->current.cadenceCt = data[7];
cad->current.cadenceCt |= (data[8] << 8);

cad->cadChange = (cad->current.cadenceTime != cad->previous.cadenceTime || cad->current.cadenceCt != cad->previous.cadenceCt);

if (cad->cadChange){
uint16_t cadence = (60 * (cad->current.cadenceCt - cad->previous.cadenceCt) * 1024) / (cad->current.cadenceTime - cad->previous.cadenceTime);
cad->current.cadence = cadence;

sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)cad, PROFILE_CADENCE);

cad->previous.cadenceTime = cad->current.cadenceTime;
cad->previous.cadence = cad->current.cadence;
cad->previous.cadenceCt = cad->current.cadenceCt;
cad.current.cadenceTime = data[5] | (data[6] << 8);
cad.current.cadenceCt = data[7] | (data[8] << 8);
if (cad.current.cadenceTime == cad.previous.cadenceTime
&& cad.current.cadenceCt == cad.previous.cadenceCt) {
return; // no change
}
uint16_t cadence = (60 * (cad.current.cadenceCt - cad.previous.cadenceCt) * 1024) / (cad.current.cadenceTime - cad.previous.cadenceTime);
cad.current.cadence = cadence;
sendMessage(ANTP_MSG_PROFILE_DATA, (intptr_t*)&cad, PROFILE_CADENCE);
cad.previous.cadenceTime = cad.current.cadenceTime;
cad.previous.cadence = cad.current.cadence;
cad.previous.cadenceCt = cad.current.cadenceCt;
}



+ 5
- 3
antplusdefs.h View File

@@ -1,10 +1,12 @@
#ifndef _LIBANT_H_
#define _LIBANT_H_

#define ANTP_MSG_PROFILE_SELECT 1000
#define WHEEL_CIRCUMFERENCE 2122

//#define ANTP_MSG_PROFILE_SELECT 1000
#define ANTP_MSG_PROFILE_DATA 1010
#define ANTP_MSG_CHANNELSTATUS 1011
#define ANTP_MSG_DEVICEID 1012
//#define ANTP_MSG_CHANNELSTATUS 1011
//#define ANTP_MSG_DEVICEID 1012


#define ANT_TRANSMISSION_SLAVE 0x00

Write Preview
Loading…
Cancel
Save