rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Improved distribution of telemetry bandwidth over sensors (fixes #5785). 

Signed-off-by: Mark Hale <mark.hale@physics.org>
This commit is contained in:
Mark Hale 2018-04-29 19:28:31 +01:00
parent 02efd46833
commit f2f5106101
1 changed files with 55 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
src/main/telemetry/smartport.c
View File

@ -143,76 +143,50 @@ enum
FSSP_DATAID_A4 = 0x0910 FSSP_DATAID_A4 = 0x0910
}; };
// high priority/fast-changing sensor data
const uint16_t frSkyDataIdTable[] = { const uint16_t frSkyDataIdTable[] = {
#ifdef USE_GPS
FSSP_DATAID_SPEED ,
#endif
FSSP_DATAID_VFAS , FSSP_DATAID_VFAS ,
FSSP_DATAID_CURRENT , FSSP_DATAID_CURRENT ,
FSSP_DATAID_ALTITUDE ,
FSSP_DATAID_FUEL , FSSP_DATAID_FUEL ,
//FSSP_DATAID_ADC1 , FSSP_DATAID_A4 ,
//FSSP_DATAID_ADC2 , FSSP_DATAID_T1 ,
#ifdef USE_GPS FSSP_DATAID_T2 ,
FSSP_DATAID_LATLONG ,
FSSP_DATAID_LATLONG , // twice
#endif
//FSSP_DATAID_CAP_USED ,
FSSP_DATAID_VARIO ,
//FSSP_DATAID_CELLS ,
//FSSP_DATAID_CELLS_LAST,
FSSP_DATAID_HEADING , FSSP_DATAID_HEADING ,
FSSP_DATAID_ACCX , FSSP_DATAID_ACCX ,
FSSP_DATAID_ACCY , FSSP_DATAID_ACCY ,
FSSP_DATAID_ACCZ , FSSP_DATAID_ACCZ ,
#ifdef USE_GPS #ifdef USE_GPS
FSSP_DATAID_SPEED ,
FSSP_DATAID_LATLONG ,
FSSP_DATAID_LATLONG , // twice (one for lat, one for long)
FSSP_DATAID_HOME_DIST , FSSP_DATAID_HOME_DIST ,
FSSP_DATAID_GPS_ALT , FSSP_DATAID_GPS_ALT ,
#endif #endif
FSSP_DATAID_ASPD , FSSP_DATAID_ALTITUDE ,
FSSP_DATAID_A4 FSSP_DATAID_VARIO
};
// low priority/slow-changing sensor data (IDs in each row are sent alternatively i.e. half-rate)
const uint16_t frSkyDataIdTable2[] = {
FSSP_DATAID_T1 , FSSP_DATAID_T2 ,
#ifdef USE_ESC_SENSOR
FSSP_DATAID_RPM , FSSP_DATAID_TEMP // ESC_SENSOR_COMBINED values
#endif
}; };
#ifdef USE_ESC_SENSOR #ifdef USE_ESC_SENSOR
// individual ESC data IDs - fast-changing data // number of sensors to send between sending the ESC sensors
#define ESC_SENSOR_PERIOD 8
const uint16_t frSkyEscDataIdTable[] = { const uint16_t frSkyEscDataIdTable[] = {
FSSP_DATAID_CURRENT1 , FSSP_DATAID_CURRENT ,
FSSP_DATAID_RPM1 FSSP_DATAID_RPM ,
}; FSSP_DATAID_VFAS ,
FSSP_DATAID_TEMP
// individual ESC data IDs - slow-changing data (half-rate)
const uint16_t frSkyEscDataIdTable2[] = {
FSSP_DATAID_VFAS1 , FSSP_DATAID_TEMP1
}; };
#endif #endif
typedef struct frSkyTableInfo_s { typedef struct frSkyTableInfo_s {
const uint16_t * table; const uint16_t * table;
uint8_t size; const uint8_t size;
uint8_t cycleLength; uint8_t index;
bool appendEscOffset;
} frSkyTableInfo_t; } frSkyTableInfo_t;
const frSkyTableInfo_t frSkyDataIdTables[] = { static frSkyTableInfo_t frSkyDataIdTableInfo = {frSkyDataIdTable, sizeof(frSkyDataIdTable)/sizeof(uint16_t), 0};
{frSkyDataIdTable, sizeof(frSkyDataIdTable)/sizeof(uint16_t), 1, false},
{frSkyDataIdTable2, sizeof(frSkyDataIdTable2)/sizeof(uint16_t), 2, false},
#ifdef USE_ESC_SENSOR #ifdef USE_ESC_SENSOR
{frSkyEscDataIdTable, sizeof(frSkyEscDataIdTable)/sizeof(uint16_t), 1, true}, static frSkyTableInfo_t frSkyEscDataIdTableInfo = {frSkyEscDataIdTable, sizeof(frSkyEscDataIdTable)/sizeof(uint16_t), 0};
{frSkyEscDataIdTable2, sizeof(frSkyEscDataIdTable2)/sizeof(uint16_t), 2, true}
#endif #endif
};
#define FRSKY_TABLE_COUNT sizeof(frSkyDataIdTables)/sizeof(frSkyDataIdTables[0])
#define DATAID_CYCLE_LENGTH 2 // lcd of table cycleLengths
#define __USE_C99_MATH // for roundf() #define __USE_C99_MATH // for roundf()
#define SMARTPORT_BAUD 57600 #define SMARTPORT_BAUD 57600
@ -458,34 +432,40 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
#endif #endif
// we can send back any data we want, our tables keep track of the order and frequency of each data type we send // we can send back any data we want, our tables keep track of the order and frequency of each data type we send
static uint8_t smartPortIdTableCnt = 0; frSkyTableInfo_t * tableInfo = &frSkyDataIdTableInfo;
static uint8_t smartPortIdCnt = 0;
#ifdef USE_ESC_SENSOR
static uint8_t smartPortIdCycleCnt = 0; static uint8_t smartPortIdCycleCnt = 0;
static uint8_t smartPortIdOffset = 0; static uint8_t smartPortIdOffset = 0;
frSkyTableInfo_t smartPortIdTableInfo = frSkyDataIdTables[smartPortIdTableCnt]; if (smartPortIdCycleCnt == ESC_SENSOR_PERIOD) {
if (smartPortIdCnt == smartPortIdTableInfo.size) { // end of table reached, move to next table // send ESC sensors
smartPortIdCnt = 0; tableInfo = &frSkyEscDataIdTableInfo;
smartPortIdTableCnt++; if (tableInfo->index == tableInfo->size) { // end of ESC table, return to other sensors
if (smartPortIdTableCnt == FRSKY_TABLE_COUNT) { // if last table then loop back round to first table tableInfo->index = 0;
smartPortIdTableCnt = 0; smartPortIdCycleCnt = 0;
// move to next ID in cycles smartPortIdOffset++;
smartPortIdCycleCnt++; if (smartPortIdOffset == getMotorCount() + 1) { // each motor and ESC_SENSOR_COMBINED
if (smartPortIdCycleCnt == DATAID_CYCLE_LENGTH) { smartPortIdOffset = 0;
smartPortIdCycleCnt = 0;
// on new cycle, move to next ESCon new cycle
smartPortIdOffset++;
if (smartPortIdOffset == getMotorCount()) {
smartPortIdOffset = 0;
}
} }
} }
smartPortIdTableInfo = frSkyDataIdTables[smartPortIdTableCnt];
} }
uint16_t id = smartPortIdTableInfo.table[smartPortIdCnt + (smartPortIdCycleCnt % smartPortIdTableInfo.cycleLength)]; if (smartPortIdCycleCnt != ESC_SENSOR_PERIOD) {
if (smartPortIdTableInfo.appendEscOffset) { // send other sensors
tableInfo = &frSkyDataIdTableInfo;
if (tableInfo->index == tableInfo->size) { // end of table reached, loop back
tableInfo->index = 0;
}
}
#endif
uint16_t id = tableInfo->table[tableInfo->index];
#ifdef USE_ESC_SENSOR
if (smartPortIdCycleCnt == ESC_SENSOR_PERIOD) {
id += smartPortIdOffset; id += smartPortIdOffset;
} else {
smartPortIdCycleCnt++;
} }
smartPortIdCnt += smartPortIdTableInfo.cycleLength; #endif
tableInfo->index++;
int32_t tmpi; int32_t tmpi;
uint32_t tmp2 = 0; uint32_t tmp2 = 0;
@ -549,8 +529,10 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
break; break;
case FSSP_DATAID_RPM : case FSSP_DATAID_RPM :
escData = getEscSensorData(ESC_SENSOR_COMBINED); escData = getEscSensorData(ESC_SENSOR_COMBINED);
smartPortSendPackage(id, escData->rpm); if (escData != NULL) {
*clearToSend = false; smartPortSendPackage(id, escData->rpm);
*clearToSend = false;
}
break; break;
case FSSP_DATAID_RPM1 : case FSSP_DATAID_RPM1 :
case FSSP_DATAID_RPM2 : case FSSP_DATAID_RPM2 :
@ -568,8 +550,10 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
break; break;
case FSSP_DATAID_TEMP : case FSSP_DATAID_TEMP :
escData = getEscSensorData(ESC_SENSOR_COMBINED); escData = getEscSensorData(ESC_SENSOR_COMBINED);
smartPortSendPackage(id, escData->temperature); if (escData != NULL) {
*clearToSend = false; smartPortSendPackage(id, escData->temperature);
*clearToSend = false;
}
break; break;
case FSSP_DATAID_TEMP1 : case FSSP_DATAID_TEMP1 :
case FSSP_DATAID_TEMP2 : case FSSP_DATAID_TEMP2 :
@ -598,9 +582,6 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
*clearToSend = false; *clearToSend = false;
} }
break; break;
//case FSSP_DATAID_ADC1 :
//case FSSP_DATAID_ADC2 :
//case FSSP_DATAID_CAP_USED :
case FSSP_DATAID_VARIO : case FSSP_DATAID_VARIO :
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, getEstimatedVario()); // unknown given unit but requested in 100 = 1m/s smartPortSendPackage(id, getEstimatedVario()); // unknown given unit but requested in 100 = 1m/s
@ -727,7 +708,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
// the same ID is sent twice, one for longitude, one for latitude // the same ID is sent twice, one for longitude, one for latitude
// the MSB of the sent uint32_t helps FrSky keep track // the MSB of the sent uint32_t helps FrSky keep track
// the even/odd bit of our counter helps us keep track // the even/odd bit of our counter helps us keep track
if (smartPortIdCnt & 1) { if (smartPortIdCycleCnt & 1) {
tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
if (gpsSol.llh.lon < 0) tmpui |= 0x40000000; if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
@ -762,9 +743,6 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
*clearToSend = false; *clearToSend = false;
} }
break; break;
case FSSP_DATAID_ASPD :
// Air speed. Not supported in BF yet.
break;
default: default:
break; break;
// if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start // if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start