mirror of
https://github.com/okalachev/flix.git
synced 2026-01-10 21:16:50 +00:00
49 lines
1.1 KiB
C++
49 lines
1.1 KiB
C++
// Copyright (c) 2024 Oleg Kalachev <okalachev@gmail.com>
|
|
// Repository: https://github.com/okalachev/flix
|
|
|
|
// Fail-safe functions
|
|
|
|
#define RC_LOSS_TIMEOUT 1
|
|
#define DESCEND_TIME 10
|
|
|
|
extern float controlTime;
|
|
extern float controlRoll, controlPitch, controlThrottle, controlYaw;
|
|
|
|
void failsafe() {
|
|
rcLossFailsafe();
|
|
autoFailsafe();
|
|
}
|
|
|
|
// RC loss failsafe
|
|
void rcLossFailsafe() {
|
|
if (controlTime == 0) return; // no RC at all
|
|
if (!armed) return;
|
|
if (t - controlTime > RC_LOSS_TIMEOUT) {
|
|
descend();
|
|
}
|
|
}
|
|
|
|
// Smooth descend on RC lost
|
|
void descend() {
|
|
mode = AUTO;
|
|
attitudeTarget = Quaternion();
|
|
thrustTarget -= dt / DESCEND_TIME;
|
|
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;
|
|
}
|