diff --git a/flix/control.ino b/flix/control.ino index f6c4a03..77c4a87 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -52,6 +52,7 @@ float thrustTarget; void control() { interpretRC(); + failsafe(); if (mode == STAB) { controlAttitude(); controlRate(); diff --git a/flix/failsafe.ino b/flix/failsafe.ino new file mode 100644 index 0000000..b076361 --- /dev/null +++ b/flix/failsafe.ino @@ -0,0 +1,22 @@ +// Copyright (c) 2023 Oleg Kalachev +// Repository: https://github.com/okalachev/flix + +// Fail-safe for RC loss + +#define RC_LOSS_TIMEOUT 0.2 +#define DESCEND_TIME 3.0 // time to descend from full throttle to zero + +void failsafe() { + if (t - controlsTime > RC_LOSS_TIMEOUT) { + descend(); + } +} + +void descend() { + // Smooth descend on RC lost + mode = STAB; + controls[RC_CHANNEL_ROLL] = 0; + controls[RC_CHANNEL_PITCH] = 0; + controls[RC_CHANNEL_YAW] = 0; + controls[RC_CHANNEL_THROTTLE] -= dt / DESCEND_TIME; +} diff --git a/flix/flix.ino b/flix/flix.ino index 33f9761..8183e55 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -28,6 +28,7 @@ float dt; // time delta from previous step, s float loopFreq; // loop frequency, Hz int16_t channels[RC_CHANNELS]; // raw rc channels float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle) +float controlsTime; // time of the last controls update Vector gyro; // gyroscope data Vector acc; // accelerometer data, m/s/s Vector rates; // filtered angular rates, rad/s diff --git a/flix/mavlink.ino b/flix/mavlink.ino index de7602a..3bf73a1 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -93,6 +93,7 @@ void handleMavlink(const void *_msg) { controls[RC_CHANNEL_YAW] = manualControl.r / 1000.0f * MAVLINK_CONTROL_SCALE; controls[RC_CHANNEL_MODE] = 1; // STAB mode controls[RC_CHANNEL_ARMED] = 1; // armed + controlsTime = t; if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0; } diff --git a/flix/rc.ino b/flix/rc.ino index c11618d..adc723a 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -21,6 +21,7 @@ void readRC() { SBUSData data = RC.data(); memcpy(channels, data.ch, sizeof(channels)); // copy channels data normalizeRC(); + controlsTime = t; } } diff --git a/gazebo/flix.h b/gazebo/flix.h index c4cbe63..bc15f94 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -25,6 +25,7 @@ float loopFreq; float motors[4]; int16_t channels[16]; // raw rc channels float controls[RC_CHANNELS]; +float controlsTime; Vector acc; Vector gyro; Vector rates; @@ -48,6 +49,8 @@ void sendMavlink(); void sendMessage(const void *msg); void receiveMavlink(); void handleMavlink(const void *_msg); +void failsafe(); +void descend(); inline Quaternion FLU2FRD(const Quaternion &q); // mocks diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index e67ea10..6684c6b 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -25,6 +25,7 @@ #include "log.ino" #include "cli.ino" #include "mavlink.ino" +#include "failsafe.ino" #include "lpf.h" using ignition::math::Vector3d;