static uint8_t decodeEscFrame(void)
{
if (!isFrameComplete()) {
return ESC_SENSOR_FRAME_PENDING;
}
// Get CRC8 checksum
uint16_t chksum = calculateCrc8(telemetryBuffer, TELEMETRY_FRAME_SIZE - 1);
uint16_t tlmsum = telemetryBuffer[TELEMETRY_FRAME_SIZE - 1]; // last byte contains CRC value
uint8_t frameStatus;
if (chksum == tlmsum) {
escSensorData[escSensorMotor].dataAge = 0;
escSensorData[escSensorMotor].temperature = telemetryBuffer[0];
escSensorData[escSensorMotor].voltage = telemetryBuffer[1] << 8 | telemetryBuffer[2];
escSensorData[escSensorMotor].current = telemetryBuffer[3] << 8 | telemetryBuffer[4];
escSensorData[escSensorMotor].consumption = telemetryBuffer[5] << 8 | telemetryBuffer[6];
escSensorData[escSensorMotor].rpm = telemetryBuffer[7] << 8 | telemetryBuffer[8];
combinedDataNeedsUpdate = true;
frameStatus = ESC_SENSOR_FRAME_COMPLETE;
DEBUG_SET(DEBUG_ESC_SENSOR_RPM, escSensorMotor, calcEscRpm(escSensorData[escSensorMotor].rpm) / 10); // output actual rpm/10 to fit in 16bit signed.
DEBUG_SET(DEBUG_ESC_SENSOR_TMP, escSensorMotor, escSensorData[escSensorMotor].temperature);
} else {
frameStatus = ESC_SENSOR_FRAME_FAILED;
}
return frameStatus;
}
void escSensorProcess(timeUs_t currentTimeUs)
{
const timeMs_t currentTimeMs = currentTimeUs / 1000;
if (!escSensorPort) {
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;
tlmFramePending = true;
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;
}
}
static uint8_t decodeEscFrame(void)
{
if (tlmFramePending) {
return ESC_SENSOR_FRAME_PENDING;
}
// Get CRC8 checksum
uint16_t chksum = get_crc8(tlm, ESC_SENSOR_BUFFSIZE - 1);
uint16_t tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1]; // last byte contains CRC value
uint8_t frameStatus;
if (chksum == tlmsum) {
escSensorData[escSensorMotor].dataAge = 0;
escSensorData[escSensorMotor].temperature = tlm[0];
escSensorData[escSensorMotor].voltage = tlm[1] << 8 | tlm[2];
escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4];
escSensorData[escSensorMotor].consumption = tlm[5] << 8 | tlm[6];
escSensorData[escSensorMotor].rpm = tlm[7] << 8 | tlm[8];
combinedDataNeedsUpdate = true;
frameStatus = ESC_SENSOR_FRAME_COMPLETE;
DEBUG_SET(DEBUG_ESC_SENSOR_RPM, escSensorMotor, escSensorData[escSensorMotor].rpm);
DEBUG_SET(DEBUG_ESC_SENSOR_TMP, escSensorMotor, escSensorData[escSensorMotor].temperature);
} else {
frameStatus = ESC_SENSOR_FRAME_FAILED;
}
return frameStatus;
}
static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
#ifdef USE_RUNAWAY_TAKEOFF
// Check to see if runaway takeoff detection is active (anti-taz), the pidSum is over the threshold,
// and gyro rate for any axis is above the limit for at least the activate delay period.
// If so, disarm for safety
if (ARMING_FLAG(ARMED)
&& !STATE(FIXED_WING)
&& pidConfig()->runaway_takeoff_prevention
&& !runawayTakeoffCheckDisabled
&& !flipOverAfterCrashMode
&& !runawayTakeoffTemporarilyDisabled
&& (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {
if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
|| (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD))
&& ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) {
if (runawayTakeoffTriggerUs == 0) {
runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY;
} else if (currentTimeUs > runawayTakeoffTriggerUs) {
setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
disarm();
}
} else {
runawayTakeoffTriggerUs = 0;
}
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_TRUE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, runawayTakeoffTriggerUs == 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE : DEBUG_RUNAWAY_TAKEOFF_TRUE);
} else {
runawayTakeoffTriggerUs = 0;
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
}
#endif
#ifdef USE_PID_AUDIO
if (isModeActivationConditionPresent(BOXPIDAUDIO)) {
pidAudioUpdate();
}
#endif
}
void max7456ReInitIfRequired(void)
{
static uint32_t lastSigCheckMs = 0;
static uint32_t videoDetectTimeMs = 0;
static uint16_t reInitCount = 0;
__spiBusTransactionBegin(busdev);
const uint8_t stallCheck = max7456Send(MAX7456ADD_VM0|MAX7456ADD_READ, 0x00);
__spiBusTransactionEnd(busdev);
const timeMs_t nowMs = millis();
if (stallCheck != videoSignalReg) {
max7456ReInit();
} else if ((videoSignalCfg == VIDEO_SYSTEM_AUTO)
&& ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS)) {
// Adjust output format based on the current input format.
__spiBusTransactionBegin(busdev);
const uint8_t videoSense = max7456Send(MAX7456ADD_STAT, 0x00);
__spiBusTransactionEnd(busdev);
DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_MODEREG, videoSignalReg & VIDEO_MODE_MASK);
DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_SENSE, videoSense & 0x7);
DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_ROWS, max7456GetRowsCount());
if (videoSense & STAT_LOS) {
videoDetectTimeMs = 0;
} else {
if ((VIN_IS_PAL(videoSense) && VIDEO_MODE_IS_NTSC(videoSignalReg))
|| (VIN_IS_NTSC_alt(videoSense) && VIDEO_MODE_IS_PAL(videoSignalReg))) {
if (videoDetectTimeMs) {
if (millis() - videoDetectTimeMs > VIDEO_SIGNAL_DEBOUNCE_MS) {
max7456ReInit();
DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_REINIT, ++reInitCount);
}
} else {
// Wait for signal to stabilize
videoDetectTimeMs = millis();
}
}
}
lastSigCheckMs = nowMs;
}
//------------ end of (re)init-------------------------------------
}
escSensorData_t *getEscSensorData(uint8_t motorNumber)
{
if (motorNumber < getMotorCount()) {
return &escSensorData[motorNumber];
} else if (motorNumber == ESC_SENSOR_COMBINED) {
if (combinedDataNeedsUpdate) {
combinedEscSensorData.dataAge = 0;
combinedEscSensorData.temperature = 0;
combinedEscSensorData.voltage = 0;
combinedEscSensorData.current = 0;
combinedEscSensorData.consumption = 0;
combinedEscSensorData.rpm = 0;
for (int i = 0; i < getMotorCount(); i = i + 1) {
combinedEscSensorData.dataAge = MAX(combinedEscSensorData.dataAge, escSensorData[i].dataAge);
combinedEscSensorData.temperature = MAX(combinedEscSensorData.temperature, escSensorData[i].temperature);
combinedEscSensorData.voltage += escSensorData[i].voltage;
combinedEscSensorData.current += escSensorData[i].current;
combinedEscSensorData.consumption += escSensorData[i].consumption;
combinedEscSensorData.rpm += escSensorData[i].rpm;
}
combinedEscSensorData.voltage = combinedEscSensorData.voltage / getMotorCount();
combinedEscSensorData.rpm = combinedEscSensorData.rpm / getMotorCount();
combinedDataNeedsUpdate = false;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_DATA_AGE, combinedEscSensorData.dataAge);
}
return &combinedEscSensorData;
} else {
return NULL;
}
}
void frSkyDSetRcData(uint16_t *rcData, const uint8_t *packet)
{
static uint16_t dataErrorCount = 0;
uint16_t channels[RC_CHANNEL_COUNT_FRSKY_D];
bool dataError = false;
decodeChannelPair(channels, packet + 6, 4);
decodeChannelPair(channels + 2, packet + 8, 3);
decodeChannelPair(channels + 4, packet + 12, 4);
decodeChannelPair(channels + 6, packet + 14, 3);
for (int i = 0; i < RC_CHANNEL_COUNT_FRSKY_D; i++) {
if ((channels[i] < 800) || (channels[i] > 2200)) {
dataError = true;
break;
}
}
if (!dataError) {
for (int i = 0; i < RC_CHANNEL_COUNT_FRSKY_D; i++) {
rcData[i] = channels[i];
}
} else {
DEBUG_SET(DEBUG_RX_FRSKY_SPI, DEBUG_DATA_ERROR_COUNT, ++dataErrorCount);
}
}
// Receive ISR callback
static void sbusDataReceive(uint16_t c, void *data)
{
sbusFrameData_t *sbusFrameData = data;
const uint32_t nowUs = micros();
const int32_t sbusFrameTime = nowUs - sbusFrameData->startAtUs;
if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) {
sbusFrameData->position = 0;
}
if (sbusFrameData->position == 0) {
if (c != SBUS_FRAME_BEGIN_BYTE) {
return;
}
sbusFrameData->startAtUs = nowUs;
}
if (sbusFrameData->position < SBUS_FRAME_SIZE) {
sbusFrameData->frame.bytes[sbusFrameData->position++] = (uint8_t)c;
if (sbusFrameData->position < SBUS_FRAME_SIZE) {
sbusFrameData->done = false;
} else {
sbusFrameData->done = true;
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime);
}
}
}
// Function for loop trigger
FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
{
static uint32_t pidUpdateCounter = 0;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
if (lockMainPID() != 0) return;
#endif
// DEBUG_PIDLOOP, timings for:
// 0 - gyroUpdate()
// 1 - subTaskPidController()
// 2 - subTaskMotorUpdate()
// 3 - subTaskPidSubprocesses()
gyroUpdate(currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs);
if (pidUpdateCounter++ % pidConfig()->pid_process_denom == 0) {
subTaskRcCommand(currentTimeUs);
subTaskPidController(currentTimeUs);
subTaskMotorUpdate(currentTimeUs);
subTaskPidSubprocesses(currentTimeUs);
}
if (debugMode == DEBUG_CYCLETIME) {
debug[0] = getTaskDeltaTime(TASK_SELF);
debug[1] = averageSystemLoadPercent;
}
}
static void subTaskMotorUpdate(void)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_CYCLETIME) {
startTime = micros();
static uint32_t previousMotorUpdateTime;
const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
debug[2] = currentDeltaTime;
debug[3] = currentDeltaTime - targetPidLooptime;
previousMotorUpdateTime = startTime;
} else if (debugMode == DEBUG_PIDLOOP) {
startTime = micros();
}
mixTable(currentPidProfile);
#ifdef USE_SERVOS
// motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
if (isMixerUsingServos()) {
writeServos();
}
#endif
if (motorControlEnable) {
writeMotors();
}
DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
}
static void subTaskPidController(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
}
static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
sbusFrameData_t *sbusFrameData = rxRuntimeConfig->frameData;
if (!sbusFrameData->done) {
return RX_FRAME_PENDING;
}
sbusFrameData->done = false;
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_FLAGS, sbusFrameData->frame.frame.channels.flags);
return sbusChannelsDecode(rxRuntimeConfig, &sbusFrameData->frame.frame.channels);
}
static void taskHandleSerial(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
#if defined(USE_VCP)
DEBUG_SET(DEBUG_USB, 0, usbCableIsInserted());
DEBUG_SET(DEBUG_USB, 1, usbVcpIsConnected());
#endif
#ifdef USE_CLI
// in cli mode, all serial stuff goes to here. enter cli mode by sending #
if (cliMode) {
cliProcess();
return;
}
#endif
#ifndef OSD_SLAVE
bool evaluateMspData = ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;
#else
bool evaluateMspData = osdSlaveIsLocked ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;;
#endif
mspSerialProcess(evaluateMspData, mspFcProcessCommand, mspFcProcessReply);
}
void dynLpfGyroUpdate(float throttle)
{
if (dynLpfFilter != DYN_LPF_NONE) {
const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
if (dynLpfFilter == DYN_LPF_PT1) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
}
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
pt1FilterUpdateCutoff(&gyroSensor2.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
}
#else
pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
#endif
}
} else if (dynLpfFilter == DYN_LPF_BIQUAD) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
}
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
biquadFilterUpdateLPF(&gyroSensor2.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
}
#else
biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
#endif
}
}
}
}
STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) {
gyroSensor->calibration.sum[axis] = 0.0f;
devClear(&gyroSensor->calibration.var[axis]);
// gyroZero is set to zero until calibration complete
gyroSensor->gyroDev.gyroZero[axis] = 0.0f;
}
// Sum up CALIBRATING_GYRO_TIME_US readings
gyroSensor->calibration.sum[axis] += gyroSensor->gyroDev.gyroADCRaw[axis];
devPush(&gyroSensor->calibration.var[axis], gyroSensor->gyroDev.gyroADCRaw[axis]);
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]);
// DEBUG_GYRO_CALIBRATION records the standard deviation of roll
// into the spare field - debug[3], in DEBUG_GYRO_RAW
if (axis == X) {
DEBUG_SET(DEBUG_GYRO_RAW, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
}
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(gyroSensor);
return;
}
// please take care with exotic boardalignment !!
gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
if (axis == Z) {
gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
}
}
}
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) {
beeper(BEEPER_GYRO_CALIBRATED);
}
}
--gyroSensor->calibration.cyclesRemaining;
}
// Function for loop trigger
void taskMainPidLoop(timeUs_t currentTimeUs)
{
static bool runTaskMainSubprocesses;
static uint8_t pidUpdateCountdown;
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
if(lockMainPID() != 0) return;
#endif
if (debugMode == DEBUG_CYCLETIME) {
debug[0] = getTaskDeltaTime(TASK_SELF);
debug[1] = averageSystemLoadPercent;
}
if (runTaskMainSubprocesses) {
subTaskMainSubprocesses(currentTimeUs);
runTaskMainSubprocesses = false;
}
// DEBUG_PIDLOOP, timings for:
// 0 - gyroUpdate()
// 1 - pidController()
// 2 - subTaskMainSubprocesses()
// 3 - subTaskMotorUpdate()
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
gyroUpdate();
DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - startTime);
if (pidUpdateCountdown) {
pidUpdateCountdown--;
} else {
pidUpdateCountdown = setPidUpdateCountDown();
subTaskPidController(currentTimeUs);
subTaskMotorUpdate();
runTaskMainSubprocesses = true;
}
}
static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {
startTime = micros();
}
#ifdef USE_MAG
if (sensors(SENSOR_MAG)) {
updateMagHold();
}
#endif
#ifdef USE_BLACKBOX
if (!cliMode && blackboxConfig()->device) {
blackboxUpdate(currentTimeUs);
}
#else
UNUSED(currentTimeUs);
#endif
DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
}
/*
* processRx called from taskUpdateRxMain
*/
bool processRx(timeUs_t currentTimeUs)
{
static bool armedBeeperOn = false;
if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
return false;
}
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {
if (!IS_RC_MODE_ACTIVE(BOXARM))
disarm();
}
updateRSSI(currentTimeUs);
if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
failsafeStartMonitoring();
}
failsafeUpdateState();
const throttleStatus_e throttleStatus = calculateThrottleStatus();
const uint8_t throttlePercent = calculateThrottlePercent();
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (throttlePercent >= rxConfig()->airModeActivateThreshold) {
airmodeIsActivated = true; // Prevent Iterm from being reset
}
} else {
airmodeIsActivated = false;
}
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
pidResetITerm();
if (currentPidProfile->pidAtMinThrottle)
pidStabilisationState(PID_STABILISATION_ON);
else
pidStabilisationState(PID_STABILISATION_OFF);
} else {
pidStabilisationState(PID_STABILISATION_ON);
}
#ifdef USE_RUNAWAY_TAKEOFF
// If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle
// is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected
// to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low.
// Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable
// prevention for the remainder of the battery.
if (ARMING_FLAG(ARMED)
&& pidConfig()->runaway_takeoff_prevention
&& !runawayTakeoffCheckDisabled
&& !flipOverAfterCrashMode
&& !runawayTakeoffTemporarilyDisabled
&& !STATE(FIXED_WING)) {
// Determine if we're in "flight"
// - motors running
// - throttle over runaway_takeoff_deactivate_throttle_percent
// - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent
// - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit
bool inStableFlight = false;
if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running?
const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle;
const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT);
if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit))
&& (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) {
inStableFlight = true;
if (runawayTakeoffDeactivateUs == 0) {
runawayTakeoffDeactivateUs = currentTimeUs;
}
}
}
// If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds
if (inStableFlight) {
if (runawayTakeoffDeactivateUs == 0) {
runawayTakeoffDeactivateUs = currentTimeUs;
}
uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay;
// at high throttle levels reduce deactivation delay by 50%
if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) {
deactivateDelay = deactivateDelay / 2;
}
if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) {
runawayTakeoffCheckDisabled = true;
}
} else {
if (runawayTakeoffDeactivateUs != 0) {
runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs);
}
runawayTakeoffDeactivateUs = 0;
}
if (runawayTakeoffDeactivateUs == 0) {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000);
} else {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000);
}
} else {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE);
}
#endif
// When armed and motors aren't spinning, do beeps and then disarm
// board after delay so users without buzzer won't lose fingers.
// mixTable constrains motor commands, so checking throttleStatus is enough
if (ARMING_FLAG(ARMED)
&& feature(FEATURE_MOTOR_STOP)
&& !STATE(FIXED_WING)
&& !feature(FEATURE_3D)
&& !isAirmodeActive()
) {
if (isUsingSticksForArming()) {
if (throttleStatus == THROTTLE_LOW) {
if (armingConfig()->auto_disarm_delay != 0
&& (int32_t)(disarmAt - millis()) < 0
) {
// auto-disarm configured and delay is over
disarm();
armedBeeperOn = false;
} else {
// still armed; do warning beeps while armed
beeper(BEEPER_ARMED);
armedBeeperOn = true;
}
} else {
// throttle is not low
if (armingConfig()->auto_disarm_delay != 0) {
// extend disarm time
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
}
if (armedBeeperOn) {
beeperSilence();
armedBeeperOn = false;
}
}
} else {
// arming is via AUX switch; beep while throttle low
if (throttleStatus == THROTTLE_LOW) {
beeper(BEEPER_ARMED);
armedBeeperOn = true;
} else if (armedBeeperOn) {
beeperSilence();
armedBeeperOn = false;
}
}
}
processRcStickPositions();
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
updateInflightCalibrationState();
}
updateActivatedModes();
#ifdef USE_DSHOT
/* Enable beep warning when the crash flip mode is active */
if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
beeper(BEEPER_CRASH_FLIP_MODE);
}
#endif
if (!cliMode) {
updateAdjustmentStates();
processRcAdjustments(currentControlRateProfile);
}
bool canUseHorizonMode = true;
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) {
// bumpless transfer to Level mode
canUseHorizonMode = false;
if (!FLIGHT_MODE(ANGLE_MODE)) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
}
if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
DISABLE_FLIGHT_MODE(ANGLE_MODE);
if (!FLIGHT_MODE(HORIZON_MODE)) {
ENABLE_FLIGHT_MODE(HORIZON_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HORIZON_MODE);
}
#ifdef USE_GPS_RESCUE
if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) {
if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
}
#endif
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
LED1_ON;
// increase frequency of attitude task to reduce drift when in angle or horizon mode
rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500));
} else {
LED1_OFF;
rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100));
}
if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {
DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
}
#if defined(USE_ACC) || defined(USE_MAG)
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
#if defined(USE_GPS) || defined(USE_MAG)
if (IS_RC_MODE_ACTIVE(BOXMAG)) {
if (!FLIGHT_MODE(MAG_MODE)) {
ENABLE_FLIGHT_MODE(MAG_MODE);
magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
}
} else {
DISABLE_FLIGHT_MODE(MAG_MODE);
}
#endif
if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
if (!FLIGHT_MODE(HEADFREE_MODE)) {
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
}
} else {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
if (imuQuaternionHeadfreeOffsetSet()){
beeper(BEEPER_RX_SET);
}
}
}
#endif
if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
} else {
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
}
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
}
#ifdef USE_TELEMETRY
static bool sharedPortTelemetryEnabled = false;
if (feature(FEATURE_TELEMETRY)) {
bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY));
if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) {
mspSerialReleaseSharedTelemetryPorts();
telemetryCheckState();
sharedPortTelemetryEnabled = true;
} else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) {
// the telemetry state must be checked immediately so that shared serial ports are released.
telemetryCheckState();
mspSerialAllocatePorts();
sharedPortTelemetryEnabled = false;
}
}
#endif
#ifdef USE_VTX_CONTROL
vtxUpdateActivatedChannel();
if (canUpdateVTX()) {
handleVTXControlButton();
}
#endif
#ifdef USE_ACRO_TRAINER
pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC));
#endif // USE_ACRO_TRAINER
#ifdef USE_RC_SMOOTHING_FILTER
if (ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) {
beeper(BEEPER_RC_SMOOTHING_INIT_FAIL);
}
#endif
pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
return true;
}
static MC6845_UPDATE_ROW( vid_update_row )
{
mbc55x_state *mstate = device->machine().driver_data<mbc55x_state>();
const rgb_t *palette = palette_entry_list_raw(bitmap.palette());
UINT8 *ram = &mstate->m_ram->pointer()[0];
UINT8 *red = &mstate->m_video_mem[RED_PLANE_OFFSET];
UINT8 *blue = &mstate->m_video_mem[BLUE_PLANE_OFFSET];
UINT8 *green;
int offset;
UINT8 rpx,gpx,bpx;
UINT8 rb,gb,bb;
int x_pos;
int pixelno;
UINT8 bitno;
UINT8 shifts;
UINT8 colour;
switch(mstate->m_vram_page)
{
case 4 : green=&ram[0x08000]; break;
case 5 : green=&ram[0x1C000]; break;
case 6 : green=&ram[0x2C000]; break;
case 7 : green=&ram[0x3C000]; break;
default :
green=&ram[0x0C000];
}
if(DEBUG_SET(DEBUG_LINES))
logerror("MC6845_UPDATE_ROW: ma=%d, ra=%d, y=%d, x_count=%d\n",ma,ra,y,x_count);
offset=((ma*4) + ra) % COLOUR_PLANE_SIZE;
if(DEBUG_SET(DEBUG_LINES))
logerror("offset=%05X\n",offset);
for(x_pos=0; x_pos<x_count; x_pos++)
{
UINT16 mem = (offset+(x_pos*4)) % COLOUR_PLANE_SIZE;
rpx=red[mem];
gpx=green[mem];
bpx=blue[mem];
bitno=0x80;
shifts=7;
for(pixelno=0; pixelno<8; pixelno++)
{
rb=(rpx & bitno) >> shifts;
gb=(gpx & bitno) >> shifts;
bb=(bpx & bitno) >> shifts;
colour=(rb<<2) | (gb<<1) | (bb<<0);
bitmap.pix32(y, (x_pos*8)+pixelno)=palette[colour];
//logerror("set pixel (%d,%d)=%d\n",y, ((x_pos*8)+pixelno),colour);
bitno=bitno>>1;
shifts--;
}
}
}
void scheduler(void)
{
// Cache currentTime
const timeUs_t currentTimeUs = micros();
// Check for realtime tasks
timeUs_t timeToNextRealtimeTask = TIMEUS_MAX;
for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) {
const timeUs_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod;
if ((int32_t)(currentTimeUs - nextExecuteAt) >= 0) {
timeToNextRealtimeTask = 0;
} else {
const timeUs_t newTimeInterval = nextExecuteAt - currentTimeUs;
timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval);
}
}
const bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > 0);
// The task to be invoked
cfTask_t *selectedTask = NULL;
uint16_t selectedTaskDynamicPriority = 0;
// Update task dynamic priorities
uint16_t waitingTasks = 0;
for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
// Task has checkFunc - event driven
if (task->checkFunc) {
const timeUs_t currentTimeBeforeCheckFuncCallUs = micros();
// Increase priority for event driven tasks
if (task->dynamicPriority > 0) {
task->taskAgeCycles = 1 + ((timeDelta_t)(currentTimeUs - task->lastSignaledAt)) / task->desiredPeriod;
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++;
} else if (task->checkFunc(currentTimeBeforeCheckFuncCallUs, currentTimeBeforeCheckFuncCallUs - task->lastExecutedAt)) {
#ifndef SKIP_TASK_STATISTICS
const timeUs_t checkFuncExecutionTime = micros() - currentTimeBeforeCheckFuncCallUs;
checkFuncMovingSumExecutionTime -= checkFuncMovingSumExecutionTime / TASK_MOVING_SUM_COUNT;
checkFuncMovingSumExecutionTime += checkFuncExecutionTime;
checkFuncTotalExecutionTime += checkFuncExecutionTime; // time consumed by scheduler + task
checkFuncMaxExecutionTime = MAX(checkFuncMaxExecutionTime, checkFuncExecutionTime);
#endif
task->lastSignaledAt = currentTimeBeforeCheckFuncCallUs;
task->taskAgeCycles = 1;
task->dynamicPriority = 1 + task->staticPriority;
waitingTasks++;
} else {
task->taskAgeCycles = 0;
}
} else {
// Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
// Task age is calculated from last execution
task->taskAgeCycles = ((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) / task->desiredPeriod;
if (task->taskAgeCycles > 0) {
task->dynamicPriority = 1 + task->staticPriority * task->taskAgeCycles;
waitingTasks++;
}
}
if (task->dynamicPriority > selectedTaskDynamicPriority) {
const bool taskCanBeChosenForScheduling =
(outsideRealtimeGuardInterval) ||
(task->taskAgeCycles > 1) ||
(task->staticPriority == TASK_PRIORITY_REALTIME);
if (taskCanBeChosenForScheduling) {
selectedTaskDynamicPriority = task->dynamicPriority;
selectedTask = task;
}
}
}
totalWaitingTasksSamples++;
totalWaitingTasks += waitingTasks;
currentTask = selectedTask;
if (selectedTask) {
// Found a task that should be run
selectedTask->taskLatestDeltaTime = (timeDelta_t)(currentTimeUs - selectedTask->lastExecutedAt);
selectedTask->lastExecutedAt = currentTimeUs;
selectedTask->dynamicPriority = 0;
// Execute task
const timeUs_t currentTimeBeforeTaskCall = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCall);
#ifndef SKIP_TASK_STATISTICS
const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
#endif
#if defined(SCHEDULER_DEBUG)
DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs - taskExecutionTime); // time spent in scheduler
} else {
DEBUG_SET(DEBUG_SCHEDULER, 2, micros() - currentTimeUs);
#endif
}
}
void max7456Init(const max7456Config_t *max7456Config, const vcdProfile_t *pVcdProfile, bool cpuOverclock)
{
max7456HardwareReset();
if (!max7456Config->csTag) {
return;
}
busdev->busdev_u.spi.csnPin = IOGetByTag(max7456Config->csTag);
if (!IOIsFreeOrPreinit(busdev->busdev_u.spi.csnPin)) {
return;
}
IOInit(busdev->busdev_u.spi.csnPin, OWNER_OSD_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(busdev->busdev_u.spi.csnPin);
spiBusSetInstance(busdev, spiInstanceByDevice(SPI_CFG_TO_DEV(max7456Config->spiDevice)));
// Detect device type by writing and reading CA[8] bit at CMAL[6].
// Do this at half the speed for safety.
spiSetDivisor(busdev->busdev_u.spi.instance, MAX7456_SPI_CLK * 2);
max7456Send(MAX7456ADD_CMAL, (1 << 6)); // CA[8] bit
if (max7456Send(MAX7456ADD_CMAL|MAX7456ADD_READ, 0xff) & (1 << 6)) {
max7456DeviceType = MAX7456_DEVICE_TYPE_AT;
} else {
max7456DeviceType = MAX7456_DEVICE_TYPE_MAX;
}
#if defined(USE_OVERCLOCK)
// Determine SPI clock divisor based on config and the device type.
switch (max7456Config->clockConfig) {
case MAX7456_CLOCK_CONFIG_HALF:
max7456SpiClock = MAX7456_SPI_CLK * 2;
break;
case MAX7456_CLOCK_CONFIG_OC:
max7456SpiClock = (cpuOverclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? MAX7456_SPI_CLK * 2 : MAX7456_SPI_CLK;
break;
case MAX7456_CLOCK_CONFIG_FULL:
max7456SpiClock = MAX7456_SPI_CLK;
break;
}
DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_OVERCLOCK, cpuOverclock);
DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DEVTYPE, max7456DeviceType);
DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DIVISOR, max7456SpiClock);
#else
UNUSED(max7456Config);
UNUSED(cpuOverclock);
#endif
spiSetDivisor(busdev->busdev_u.spi.instance, max7456SpiClock);
// force soft reset on Max7456
__spiBusTransactionBegin(busdev);
max7456Send(MAX7456ADD_VM0, MAX7456_RESET);
__spiBusTransactionEnd(busdev);
// Setup values to write to registers
videoSignalCfg = pVcdProfile->video_system;
hosRegValue = 32 - pVcdProfile->h_offset;
vosRegValue = 16 - pVcdProfile->v_offset;
#ifdef MAX7456_DMA_CHANNEL_TX
dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0);
#endif
// Real init will be made later when driver detect idle.
}
// Receive ISR callback
static void sbusDataReceive(uint16_t c, void *data)
{
static uint16_t sbusDesyncCounter = 0;
sbusFrameData_t *sbusFrameData = data;
const timeUs_t currentTimeUs = micros();
const timeDelta_t timeSinceLastByteUs = cmpTimeUs(currentTimeUs, sbusFrameData->lastActivityTimeUs);
sbusFrameData->lastActivityTimeUs = currentTimeUs;
// Handle inter-frame gap. We dwell in STATE_SBUS_WAIT_SYNC state ignoring all incoming bytes until we get long enough quite period on the wire
if (sbusFrameData->state == STATE_SBUS_WAIT_SYNC && timeSinceLastByteUs >= rxConfig()->sbusSyncInterval) {
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_INTERFRAME_TIME, timeSinceLastByteUs);
sbusFrameData->state = STATE_SBUS_SYNC;
}
switch (sbusFrameData->state) {
case STATE_SBUS_SYNC:
if (c == SBUS_FRAME_BEGIN_BYTE) {
sbusFrameData->position = 0;
sbusFrameData->buffer[sbusFrameData->position++] = (uint8_t)c;
sbusFrameData->state = STATE_SBUS_PAYLOAD;
}
break;
case STATE_SBUS_PAYLOAD:
sbusFrameData->buffer[sbusFrameData->position++] = (uint8_t)c;
if (sbusFrameData->position == SBUS_FRAME_SIZE) {
const sbusFrame_t * frame = (sbusFrame_t *)&sbusFrameData->buffer[0];
bool frameValid = false;
// Do some sanity check
switch (frame->endByte) {
case 0x00: // This is S.BUS 1
case 0x04: // S.BUS 2 receiver voltage
case 0x14: // S.BUS 2 GPS/baro
case 0x24: // Unknown SBUS2 data
case 0x34: // Unknown SBUS2 data
frameValid = true;
sbusFrameData->state = STATE_SBUS_WAIT_SYNC;
break;
default: // Failed end marker
sbusFrameData->state = STATE_SBUS_WAIT_SYNC;
sbusDesyncCounter++;
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_DESYNC_COUNTER, sbusDesyncCounter);
break;
}
// Frame seems sane, pass data to decoder
if (!sbusFrameData->frameDone && frameValid) {
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_FLAGS, frame->channels.flags);
memcpy((void *)&sbusFrameData->frame, (void *)&sbusFrameData->buffer[0], SBUS_FRAME_SIZE);
sbusFrameData->frameDone = true;
}
}
break;
case STATE_SBUS_WAIT_SYNC:
// Stay at this state and do nothing. Exit will be handled before byte is processed if the
// inter-frame gap is long enough
break;
}
}
static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// Read out gyro temperature if used for telemmetry
if (feature(FEATURE_TELEMETRY)) {
gyroReadTemperature();
}
#ifdef MAG
if (sensors(SENSOR_MAG)) {
updateMagHold();
}
#endif
#if defined(BARO) || defined(SONAR)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
applyAltHold();
}
}
#endif
// If we're armed, at minimum throttle, and we do arming via the
// sticks, do not process yaw input from the rx. We do this so the
// motors do not spin up while we are trying to arm or disarm.
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
#ifndef USE_QUAD_MIXER_ONLY
#ifdef USE_SERVOS
&& !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
#endif
&& mixerConfig()->mixerMode != MIXER_AIRPLANE
&& mixerConfig()->mixerMode != MIXER_FLYING_WING
#endif
) {
resetYawAxis();
}
if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value);
}
processRcCommand();
#ifdef GPS
if (sensors(SENSOR_GPS)) {
if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
updateGpsStateForHomeAndHoldMode();
}
}
#endif
#ifdef USE_SDCARD
afatfs_poll();
#endif
#ifdef BLACKBOX
if (!cliMode && blackboxConfig()->device) {
handleBlackbox(currentTimeUs);
}
#else
UNUSED(currentTimeUs);
#endif
#ifdef TRANSPONDER
transponderUpdate(currentTimeUs);
#endif
DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
}
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
{
const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs;
switch (gyroToUse) {
case GYRO_CONFIG_USE_GYRO_1:
gyroUpdateSensor(&gyroSensor1, currentTimeUs);
if (isGyroSensorCalibrationComplete(&gyroSensor1)) {
gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X];
gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y];
gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z];
#ifdef USE_GYRO_OVERFLOW_CHECK
overflowDetected = gyroSensor1.overflowDetected;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
yawSpinDetected = gyroSensor1.yawSpinDetected;
#endif
}
if (useDualGyroDebugging) {
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 0, lrintf(gyro.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale));
}
break;
#ifdef USE_MULTI_GYRO
case GYRO_CONFIG_USE_GYRO_2:
gyroUpdateSensor(&gyroSensor2, currentTimeUs);
if (isGyroSensorCalibrationComplete(&gyroSensor2)) {
gyro.gyroADCf[X] = gyroSensor2.gyroDev.gyroADCf[X];
gyro.gyroADCf[Y] = gyroSensor2.gyroDev.gyroADCf[Y];
gyro.gyroADCf[Z] = gyroSensor2.gyroDev.gyroADCf[Z];
#ifdef USE_GYRO_OVERFLOW_CHECK
overflowDetected = gyroSensor2.overflowDetected;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
yawSpinDetected = gyroSensor2.yawSpinDetected;
#endif
}
if (useDualGyroDebugging) {
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 3, lrintf(gyro.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale));
}
break;
case GYRO_CONFIG_USE_GYRO_BOTH:
gyroUpdateSensor(&gyroSensor1, currentTimeUs);
gyroUpdateSensor(&gyroSensor2, currentTimeUs);
if (isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2)) {
gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f;
gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f;
gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f;
#ifdef USE_GYRO_OVERFLOW_CHECK
overflowDetected = gyroSensor1.overflowDetected || gyroSensor2.overflowDetected;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
yawSpinDetected = gyroSensor1.yawSpinDetected || gyroSensor2.yawSpinDetected;
#endif
}
if (useDualGyroDebugging) {
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X] - gyroSensor2.gyroDev.gyroADCf[X]));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y] - gyroSensor2.gyroDev.gyroADCf[Y]));
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z]));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale));
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale));
}
break;
#endif
}
if (!overflowDetected) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * sampleDeltaUs;
gyroPrevious[axis] = gyro.gyroADCf[axis];
}
}
}
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
static timeUs_t previousTimeUs = 0;
const uint32_t dTime = currentTimeUs - previousTimeUs;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
return;
}
previousTimeUs = currentTimeUs;
static float vel = 0.0f;
static float accAlt = 0.0f;
int32_t baroAlt = 0;
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
vel = 0;
accAlt = 0;
} else {
baroAlt = baroCalculateAltitude();
estimatedAltitude = baroAlt;
}
}
#endif
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
int32_t sonarAlt = sonarCalculateAltitude(sonarRead(), getCosTiltAngle());
if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
sonarAlt = (float)sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition);
estimatedAltitude = sonarAlt;
}
}
#endif
float accZ_tmp = 0;
#ifdef ACC
if (sensors(SENSOR_ACC)) {
const float dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / accSumCount;
}
const float vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt) + (float)baro.BaroAlt * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt)); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
estimatedAltitude = accAlt;
}
#endif
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_ACC, accSum[2] / accSumCount);
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_VEL, vel);
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_HEIGHT, accAlt);
imuResetAccelerationSum();
int32_t baroVel = 0;
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
return;
}
static int32_t lastBaroAlt = 0;
baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = baroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
}
#endif
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel));
int32_t vel_tmp = lrintf(vel);
// set vario
estimatedVario = applyDeadband(vel_tmp, 5);
static float accZ_old = 0.0f;
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
}
