Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 23:05:19 +03:00
Code Issues Wiki Activity

Improved retry timeout check and 10 seconds of no response from ESCs check

Browse source
This commit is contained in:
Bas Delfos 2016-11-27 12:27:25 +01:00
parent 2ffe57761e
commit c1d442279f
1 changed files with 39 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

75
src/main/sensors/esc_sensor.c
View file

@ -89,7 +89,7 @@ static uint8_t tlmFramePosition = 0;
static serialPort_t *escSensorPort = NULL; static serialPort_t *escSensorPort = NULL;
static esc_telemetry_t escSensorData[MAX_SUPPORTED_MOTORS]; static esc_telemetry_t escSensorData[MAX_SUPPORTED_MOTORS];
static uint32_t escTriggerTimestamp = -1; static uint32_t escTriggerTimestamp = -1;
static uint32_t escTriggerLastTimestamp = -1; static uint32_t escLastResponseTimestamp;
static uint8_t timeoutRetryCount = 0; static uint8_t timeoutRetryCount = 0;
static uint8_t totalRetryCount = 0; static uint8_t totalRetryCount = 0;
@ -208,7 +208,7 @@ void escSensorProcess(timeUs_t currentTimeUs)
} }
// Wait period of time before requesting telemetry (let the system boot first) // Wait period of time before requesting telemetry (let the system boot first)
if (millis() < ESC_BOOTTIME) { if (currentTimeMs < ESC_BOOTTIME) {
return; return;
} }
else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) { else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_WAIT) {
@ -216,7 +216,7 @@ void escSensorProcess(timeUs_t currentTimeUs)
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
escSensorMotor = 0; escSensorMotor = 0;
escTriggerTimestamp = currentTimeMs; escTriggerTimestamp = currentTimeMs;
escTriggerLastTimestamp = escTriggerTimestamp; escLastResponseTimestamp = escTriggerTimestamp;
} }
else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) { else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) {
DEBUG_SET(DEBUG_ESC_SENSOR, 0, escSensorMotor+1); DEBUG_SET(DEBUG_ESC_SENSOR, 0, escSensorMotor+1);
@ -225,49 +225,52 @@ void escSensorProcess(timeUs_t currentTimeUs)
motor->requestTelemetry = true; motor->requestTelemetry = true;
escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING; escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
} }
else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_PENDING) {
if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs) { if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs) {
// ESC did not repond in time, retry // ESC did not repond in time, retry
timeoutRetryCount++; timeoutRetryCount++;
escTriggerTimestamp = currentTimeMs; escTriggerTimestamp = currentTimeMs;
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
if (timeoutRetryCount == 4) { if (timeoutRetryCount == 4) {
// Not responding after 3 times, skip motor // Not responding after 3 times, skip motor
escSensorData[escSensorMotor].skipped = true; escSensorData[escSensorMotor].skipped = true;
selectNextMotor(); selectNextMotor();
}
DEBUG_SET(DEBUG_ESC_SENSOR, 1, ++totalRetryCount);
} }
DEBUG_SET(DEBUG_ESC_SENSOR, 1, ++totalRetryCount); // Get received frame status
} uint8_t state = escSensorFrameStatus();
// Get received frame status if (state == ESC_SENSOR_FRAME_COMPLETE) {
uint8_t state = escSensorFrameStatus(); // Wait until all ESCs are processed
if (escSensorMotor == getMotorCount()-1) {
escCurrent = 0;
escConsumption = 0;
escVbat = 0;
if (state == ESC_SENSOR_FRAME_COMPLETE) { for (int i = 0; i < getMotorCount(); i++) {
// Wait until all ESCs are processed if (!escSensorData[i].skipped) {
if (escSensorMotor == getMotorCount()-1) { escVbat = i > 0 ? ((escVbat + escSensorData[i].voltage) / 2) : escSensorData[i].voltage;
escCurrent = 0; escCurrent = escCurrent + escSensorData[i].current;
escConsumption = 0; escConsumption = escConsumption + escSensorData[i].consumption;
escVbat = 0; }
for (int i = 0; i < getMotorCount(); i++) {
if (!escSensorData[i].skipped) {
escVbat = i > 0 ? ((escVbat + escSensorData[i].voltage) / 2) : escSensorData[i].voltage;
escCurrent = escCurrent + escSensorData[i].current;
escConsumption = escConsumption + escSensorData[i].consumption;
} }
} }
DEBUG_SET(DEBUG_ESC_SENSOR, 2, escVbat);
DEBUG_SET(DEBUG_ESC_SENSOR, 3, escCurrent);
selectNextMotor();
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
escLastResponseTimestamp = currentTimeMs;
} }
DEBUG_SET(DEBUG_ESC_SENSOR, 2, escVbat);
DEBUG_SET(DEBUG_ESC_SENSOR, 3, escCurrent);
selectNextMotor();
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
} }
if (escTriggerLastTimestamp + 10000 < currentTimeMs) { if (escLastResponseTimestamp + 10000 < currentTimeMs) {
// ESCs did not respond for 10 seconds // ESCs did not respond for 10 seconds
// Disable ESC telemetry and reset voltage and current to let the use know something is wrong // Disable ESC telemetry and reset voltage and current to let the use know something is wrong
freeEscSensorPort(); freeEscSensorPort();
@ -282,8 +285,8 @@ static void selectNextMotor(void)
if (escSensorMotor == getMotorCount()) { if (escSensorMotor == getMotorCount()) {
escSensorMotor = 0; escSensorMotor = 0;
} }
timeoutRetryCount = 0;
escTriggerTimestamp = millis(); escTriggerTimestamp = millis();
escTriggerLastTimestamp = escTriggerTimestamp;
} }
//-- CRC //-- CRC