void escSensorProcess(timeUs_t currentTimeUs)
{
const timeMs_t currentTimeMs = currentTimeUs / 1000;
if (!escSensorPort || !pwmAreMotorsEnabled()) {
return;
}
switch (escSensorTriggerState) {
case ESC_SENSOR_TRIGGER_STARTUP:
// Wait period of time before requesting telemetry (let the system boot first)
if (currentTimeMs >= ESC_BOOTTIME) {
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
}
break;
case ESC_SENSOR_TRIGGER_READY:
escTriggerTimestamp = currentTimeMs;
startEscDataRead(telemetryBuffer, TELEMETRY_FRAME_SIZE);
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
motor->requestTelemetry = true;
escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1);
break;
case ESC_SENSOR_TRIGGER_PENDING:
if (currentTimeMs < escTriggerTimestamp + ESC_REQUEST_TIMEOUT) {
uint8_t state = decodeEscFrame();
switch (state) {
case ESC_SENSOR_FRAME_COMPLETE:
selectNextMotor();
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
break;
case ESC_SENSOR_FRAME_FAILED:
increaseDataAge();
selectNextMotor();
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_CRC_ERRORS, ++totalCrcErrorCount);
break;
case ESC_SENSOR_FRAME_PENDING:
break;
}
} else {
// Move on to next ESC, we'll come back to this one
increaseDataAge();
selectNextMotor();
escSensorTriggerState = ESC_SENSOR_TRIGGER_READY;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_TIMEOUTS, ++totalTimeoutCount);
}
break;
}
}
void writeMotors(void)
{
if (pwmAreMotorsEnabled()) {
for (int i = 0; i < motorCount; i++) {
pwmWriteMotor(i, motor[i]);
}
pwmCompleteMotorUpdate(motorCount);
}
}
