mirror of
https://github.com/okalachev/flix.git
synced 2026-02-17 07:31:56 +00:00
6d01cd2e79
SF_RC_LOSS_TIME - time without rc to activate failsafe. SD_DESCEND_TIME - total time to decrease the throttle to zero. Make controlTime nan on the start to simplify the logic.
48 lines
1.0 KiB
C++
48 lines
1.0 KiB
C++
// Copyright (c) 2024 Oleg Kalachev <okalachev@gmail.com>
|
|
// Repository: https://github.com/okalachev/flix
|
|
|
|
// Fail-safe functions
|
|
|
|
extern float controlTime;
|
|
extern float controlRoll, controlPitch, controlThrottle, controlYaw;
|
|
|
|
float rcLossTimeout = 1;
|
|
float descendTime = 10;
|
|
|
|
void failsafe() {
|
|
rcLossFailsafe();
|
|
autoFailsafe();
|
|
}
|
|
|
|
// RC loss failsafe
|
|
void rcLossFailsafe() {
|
|
if (!armed) return;
|
|
if (t - controlTime > rcLossTimeout) {
|
|
descend();
|
|
}
|
|
}
|
|
|
|
// Smooth descend on RC lost
|
|
void descend() {
|
|
mode = AUTO;
|
|
attitudeTarget = Quaternion();
|
|
thrustTarget -= dt / descendTime;
|
|
if (thrustTarget < 0) {
|
|
thrustTarget = 0;
|
|
armed = false;
|
|
}
|
|
}
|
|
|
|
// Allow pilot to interrupt automatic flight
|
|
void autoFailsafe() {
|
|
static float roll, pitch, yaw, throttle;
|
|
if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
|
|
// controls changed
|
|
if (mode == AUTO) mode = STAB; // regain control by the pilot
|
|
}
|
|
roll = controlRoll;
|
|
pitch = controlPitch;
|
|
yaw = controlYaw;
|
|
throttle = controlThrottle;
|
|
}
|