Commit d5b44f69 authored by Dominic Clifton's avatar Dominic Clifton
Browse files

Merge branch 'sport_fixes' of https://github.com/kilrah/cleanflight into kilrah-sport_fixes

parents e5a50654 c1243981
...@@ -135,7 +135,7 @@ const uint16_t frSkyDataIdTable[] = { ...@@ -135,7 +135,7 @@ const uint16_t frSkyDataIdTable[] = {
#define __USE_C99_MATH // for roundf() #define __USE_C99_MATH // for roundf()
#define SMARTPORT_BAUD 57600 #define SMARTPORT_BAUD 57600
#define SMARTPORT_UART_MODE MODE_RXTX #define SMARTPORT_UART_MODE MODE_RXTX
#define SMARTPORT_SERVICE_DELAY_MS 5 // telemetry requests comes in at roughly 12 ms intervals, keep this under that #define SMARTPORT_SERVICE_TIMEOUT_MS 1 // max allowed time to find a value to send
#define SMARTPORT_NOT_CONNECTED_TIMEOUT_MS 7000 #define SMARTPORT_NOT_CONNECTED_TIMEOUT_MS 7000
static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port. static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
...@@ -151,7 +151,6 @@ char smartPortState = SPSTATE_UNINITIALIZED; ...@@ -151,7 +151,6 @@ char smartPortState = SPSTATE_UNINITIALIZED;
static uint8_t smartPortHasRequest = 0; static uint8_t smartPortHasRequest = 0;
static uint8_t smartPortIdCnt = 0; static uint8_t smartPortIdCnt = 0;
static uint32_t smartPortLastRequestTime = 0; static uint32_t smartPortLastRequestTime = 0;
static uint32_t smartPortLastServiceTime = 0;
static void smartPortDataReceive(uint16_t c) static void smartPortDataReceive(uint16_t c)
{ {
...@@ -161,8 +160,8 @@ static void smartPortDataReceive(uint16_t c) ...@@ -161,8 +160,8 @@ static void smartPortDataReceive(uint16_t c)
static uint8_t lastChar; static uint8_t lastChar;
if (lastChar == FSSP_START_STOP) { if (lastChar == FSSP_START_STOP) {
smartPortState = SPSTATE_WORKING; smartPortState = SPSTATE_WORKING;
smartPortLastRequestTime = now; if (c == FSSP_SENSOR_ID1 && (serialTotalBytesWaiting(smartPortSerialPort) == 0)) {
if (c == FSSP_SENSOR_ID1) { smartPortLastRequestTime = now;
smartPortHasRequest = 1; smartPortHasRequest = 1;
// we only responde to these IDs // we only responde to these IDs
// the X4R-SB does send other IDs, we ignore them, but take note of the time // the X4R-SB does send other IDs, we ignore them, but take note of the time
...@@ -204,8 +203,6 @@ static void smartPortSendPackage(uint16_t id, uint32_t val) ...@@ -204,8 +203,6 @@ static void smartPortSendPackage(uint16_t id, uint32_t val)
smartPortSendByte(u8p[2], &crc); smartPortSendByte(u8p[2], &crc);
smartPortSendByte(u8p[3], &crc); smartPortSendByte(u8p[3], &crc);
smartPortSendByte(0xFF - (uint8_t)crc, NULL); smartPortSendByte(0xFF - (uint8_t)crc, NULL);
smartPortLastServiceTime = millis();
} }
void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig) void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig)
...@@ -275,6 +272,8 @@ void checkSmartPortTelemetryState(void) ...@@ -275,6 +272,8 @@ void checkSmartPortTelemetryState(void)
void handleSmartPortTelemetry(void) void handleSmartPortTelemetry(void)
{ {
uint32_t smartPortLastServiceTime = millis();
if (!smartPortTelemetryEnabled) { if (!smartPortTelemetryEnabled) {
return; return;
} }
...@@ -296,11 +295,13 @@ void handleSmartPortTelemetry(void) ...@@ -296,11 +295,13 @@ void handleSmartPortTelemetry(void)
return; return;
} }
// limit the rate at which we send responses, we don't want to affect flight characteristics while (smartPortHasRequest) {
if ((now - smartPortLastServiceTime) < SMARTPORT_SERVICE_DELAY_MS) // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
return; if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) {
smartPortHasRequest = 0;
if (smartPortHasRequest) { return;
}
// we can send back any data we want, our table keeps track of the order and frequency of each data type we send // we can send back any data we want, our table keeps track of the order and frequency of each data type we send
uint16_t id = frSkyDataIdTable[smartPortIdCnt]; uint16_t id = frSkyDataIdTable[smartPortIdCnt];
if (id == 0) { // end of table reached, loop back if (id == 0) { // end of table reached, loop back
...@@ -323,21 +324,29 @@ void handleSmartPortTelemetry(void) ...@@ -323,21 +324,29 @@ void handleSmartPortTelemetry(void)
break; break;
#endif #endif
case FSSP_DATAID_VFAS : case FSSP_DATAID_VFAS :
smartPortSendPackage(id, vbat * 10); // given in 0.1V, convert to volts if (feature(FEATURE_VBAT)) {
smartPortHasRequest = 0; smartPortSendPackage(id, vbat * 10); // given in 0.1V, convert to volts
smartPortHasRequest = 0;
}
break; break;
case FSSP_DATAID_CURRENT : case FSSP_DATAID_CURRENT :
smartPortSendPackage(id, amperage); // given in 10mA steps, unknown requested unit if (feature(FEATURE_CURRENT_METER)) {
smartPortHasRequest = 0; smartPortSendPackage(id, amperage); // given in 10mA steps, unknown requested unit
smartPortHasRequest = 0;
}
break; break;
//case FSSP_DATAID_RPM : //case FSSP_DATAID_RPM :
case FSSP_DATAID_ALTITUDE : case FSSP_DATAID_ALTITUDE :
smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter if (sensors(SENSOR_BARO)) {
smartPortHasRequest = 0; smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter
smartPortHasRequest = 0;
}
break; break;
case FSSP_DATAID_FUEL : case FSSP_DATAID_FUEL :
smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit if (feature(FEATURE_CURRENT_METER)) {
smartPortHasRequest = 0; smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
smartPortHasRequest = 0;
}
break; break;
//case FSSP_DATAID_ADC1 : //case FSSP_DATAID_ADC1 :
//case FSSP_DATAID_ADC2 : //case FSSP_DATAID_ADC2 :
...@@ -365,8 +374,10 @@ void handleSmartPortTelemetry(void) ...@@ -365,8 +374,10 @@ void handleSmartPortTelemetry(void)
#endif #endif
//case FSSP_DATAID_CAP_USED : //case FSSP_DATAID_CAP_USED :
case FSSP_DATAID_VARIO : case FSSP_DATAID_VARIO :
smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s if (sensors(SENSOR_BARO)) {
smartPortHasRequest = 0; smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s
smartPortHasRequest = 0;
}
break; break;
case FSSP_DATAID_HEADING : case FSSP_DATAID_HEADING :
smartPortSendPackage(id, heading * 100); // given in deg, requested in 10000 = 100 deg smartPortSendPackage(id, heading * 100); // given in deg, requested in 10000 = 100 deg
...@@ -440,7 +451,7 @@ void handleSmartPortTelemetry(void) ...@@ -440,7 +451,7 @@ void handleSmartPortTelemetry(void)
smartPortHasRequest = 0; smartPortHasRequest = 0;
#endif #endif
} }
else { else if (feature(FEATURE_GPS)) {
smartPortSendPackage(id, 0); smartPortSendPackage(id, 0);
smartPortHasRequest = 0; smartPortHasRequest = 0;
} }
...@@ -455,7 +466,7 @@ void handleSmartPortTelemetry(void) ...@@ -455,7 +466,7 @@ void handleSmartPortTelemetry(void)
#endif #endif
default: default:
break; break;
// if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just wait for the next loop // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start
} }
} }
} }
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment