Merge pull request #6685 from kernel-machine/flip-over-after-crash

Flip over after crash aka TURTLE MODE

docs/Settings.md

@@ -101,6 +101,7 @@
 | failsafe_throttle_low_delay | 100 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout |
 | fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
 | flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
+| flip_over_after_crash_power_factor | 65 | flip over after crash power factor |
 | fpv_mix_degrees |  |  |
 | frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA |
 | frsky_default_latitude | 0.000 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |

src/main/CMakeLists.txt

@@ -48,6 +48,8 @@ main_sources(COMMON_SRC
     common/typeconversion.h
     common/uvarint.c
     common/uvarint.h
+    common/circular_queue.c
+    common/circular_queue.h
 
     config/config_eeprom.c
     config/config_eeprom.h

src/main/common/circular_queue.c

@@ -0,0 +1,66 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "circular_queue.h"
+
+void circularBufferInit(circularBuffer_t *circular_buffer, uint8_t *buffer, size_t buffer_size,
+                          size_t buffer_element_size) {
+    circular_buffer->buffer = buffer;
+    circular_buffer->bufferSize = buffer_size;
+    circular_buffer->elementSize = buffer_element_size;
+    circular_buffer->head = 0;
+    circular_buffer->tail = 0;
+    circular_buffer->size = 0;
+}
+
+void circularBufferPushElement(circularBuffer_t *circularBuffer, uint8_t *element) {
+    if (circularBufferIsFull(circularBuffer))
+        return;
+
+    memcpy(circularBuffer->buffer + circularBuffer->tail, element, circularBuffer->elementSize);
+
+    circularBuffer->tail += circularBuffer->elementSize;
+    circularBuffer->tail %= circularBuffer->bufferSize;
+    circularBuffer->size += 1;
+}
+
+void circularBufferPopHead(circularBuffer_t *circularBuffer, uint8_t *element) {
+    memcpy(element, circularBuffer->buffer + circularBuffer->head, circularBuffer->elementSize);
+
+    circularBuffer->head += circularBuffer->elementSize;
+    circularBuffer->head %= circularBuffer->bufferSize;
+    circularBuffer->size -= 1;
+}
+
+int circularBufferIsFull(circularBuffer_t *circularBuffer) {
+    return circularBufferCountElements(circularBuffer) * circularBuffer->elementSize == circularBuffer->bufferSize;
+}
+
+int circularBufferIsEmpty(circularBuffer_t *circularBuffer) {
+    return circularBuffer==NULL || circularBufferCountElements(circularBuffer) == 0;
+}
+
+size_t circularBufferCountElements(circularBuffer_t *circularBuffer) {
+    return circularBuffer->size;
+}

src/main/common/circular_queue.h

@@ -0,0 +1,47 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#ifndef INAV_CIRCULAR_QUEUE_H
+#define INAV_CIRCULAR_QUEUE_H
+
+#include "stdint.h"
+#include "string.h"
+
+typedef struct circularBuffer_s{
+    size_t head;
+    size_t tail;
+    size_t bufferSize;
+    uint8_t * buffer;
+    size_t elementSize;
+    size_t size;
+}circularBuffer_t;
+
+void    circularBufferInit(circularBuffer_t * circularBuffer, uint8_t * buffer, size_t bufferSize, size_t bufferElementSize);
+void    circularBufferPushElement(circularBuffer_t * circularBuffer, uint8_t * element);
+void    circularBufferPopHead(circularBuffer_t * circularBuffer, uint8_t * element);
+int     circularBufferIsFull(circularBuffer_t * circularBuffer);
+int     circularBufferIsEmpty(circularBuffer_t *circularBuffer);
+size_t  circularBufferCountElements(circularBuffer_t * circularBuffer);
+
+#endif //INAV_CIRCULAR_QUEUE_H

src/main/common/maths.h

@@ -88,6 +88,8 @@
 #define _ABS_I(x, var) _ABS_II(x, var)
 #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__))
 
+#define power3(x) ((x)*(x)*(x))
+
 // Floating point Euler angles.
 typedef struct fp_angles {
     float roll;

src/main/drivers/pwm_output.c

@@ -28,6 +28,7 @@ FILE_COMPILE_FOR_SPEED
 
 #include "common/log.h"
 #include "common/maths.h"
+#include "common/circular_queue.h"
 
 #include "drivers/io.h"
 #include "drivers/timer.h"
@@ -60,6 +61,10 @@ FILE_COMPILE_FOR_SPEED
 #define DSHOT_MOTOR_BITLENGTH   20
 
 #define DSHOT_DMA_BUFFER_SIZE   18 /* resolution + frame reset (2us) */
+
+#define DSHOT_COMMAND_INTERVAL_US 1000
+#define DSHOT_COMMAND_QUEUE_LENGTH 8
+#define DHSOT_COMMAND_QUEUE_SIZE   DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e)
 #endif
 
 typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
@@ -110,6 +115,12 @@ static uint8_t allocatedOutputPortCount = 0;
 
 static bool pwmMotorsEnabled = true;
 
+#ifdef USE_DSHOT
+static circularBuffer_t commandsCircularBuffer;
+static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE];
+static currentExecutingCommand_t currentExecutingCommand;
+#endif
+
 static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value)
 {
     p->tch = tch;
@@ -340,8 +351,60 @@ void pwmRequestMotorTelemetry(int motorIndex)
     }
 }
 
-void pwmCompleteMotorUpdate(void)
-{
+#ifdef USE_DSHOT
+void sendDShotCommand(dshotCommands_e cmd) {
+    circularBufferPushElement(&commandsCircularBuffer, (uint8_t *) &cmd);
+}
+
+void initDShotCommands(void) {
+    circularBufferInit(&commandsCircularBuffer, commandsBuff,DHSOT_COMMAND_QUEUE_SIZE, sizeof(dshotCommands_e));
+
+    currentExecutingCommand.remainingRepeats = 0;
+}
+
+static int getDShotCommandRepeats(dshotCommands_e cmd) {
+    int repeats = 1;
+
+    switch (cmd) {
+        case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
+        case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
+            repeats = 6;
+            break;
+        default:
+            break;
+    }
+
+    return repeats;
+}
+
+static void executeDShotCommands(void){
+
+    if(currentExecutingCommand.remainingRepeats == 0) {
+
+        const int isTherePendingCommands = !circularBufferIsEmpty(&commandsCircularBuffer);
+
+        if (isTherePendingCommands) {
+            //Load the command
+            dshotCommands_e cmd;
+            circularBufferPopHead(&commandsCircularBuffer, (uint8_t *) &cmd);
+
+            currentExecutingCommand.cmd = cmd;
+            currentExecutingCommand.remainingRepeats = getDShotCommandRepeats(cmd);
+        }
+        else {
+            return;
+        }
+    }
+
+    for (uint8_t i = 0; i < getMotorCount(); i++) {
+        motors[i].requestTelemetry = true;
+        motors[i].value = currentExecutingCommand.cmd;
+    }
+    currentExecutingCommand.remainingRepeats--;
+}
+#endif
+
+void pwmCompleteMotorUpdate(void) {
     // This only makes sense for digital motor protocols
     if (!isMotorProtocolDigital()) {
         return;
@@ -359,6 +422,9 @@ void pwmCompleteMotorUpdate(void)
 
 #ifdef USE_DSHOT
     if (isMotorProtocolDshot()) {
+
+        executeDShotCommands();
+
         // Generate DMA buffers
         for (int index = 0; index < motorCount; index++) {
             if (motors[index].pwmPort && motors[index].pwmPort->configured) {

src/main/drivers/pwm_output.h

@@ -20,6 +20,16 @@
 #include "drivers/io_types.h"
 #include "drivers/time.h"
 
+typedef enum {
+    DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
+    DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
+} dshotCommands_e;
+
+typedef struct {
+    dshotCommands_e cmd;
+    int remainingRepeats;
+} currentExecutingCommand_t;
+
 void pwmRequestMotorTelemetry(int motorIndex);
 
 ioTag_t pwmGetMotorPinTag(int motorIndex);
@@ -28,6 +38,7 @@ void pwmWriteMotor(uint8_t index, uint16_t value);
 void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
 void pwmCompleteMotorUpdate(void);
 bool isMotorProtocolDigital(void);
+bool isMotorProtocolDshot(void);
 
 void pwmWriteServo(uint8_t index, uint16_t value);
 
@@ -41,4 +52,7 @@ bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIn
 void pwmServoPreconfigure(void);
 bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput);
 void pwmWriteBeeper(bool onoffBeep);
-void beeperPwmInit(ioTag_t tag, uint16_t frequency);
+void beeperPwmInit(ioTag_t tag, uint16_t frequency);
+
+void sendDShotCommand(dshotCommands_e directionSpin);
+void initDShotCommands(void);

src/main/fc/controlrate_profile.c

@@ -64,7 +64,7 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance)
             },
 
             .misc = {
-                .fpvCamAngleDegrees = 0
+                .fpvCamAngleDegrees = 0,
             }
         );
     }

src/main/fc/fc_core.c

@@ -398,7 +398,12 @@ void disarm(disarmReason_t disarmReason)
             blackboxFinish();
         }
 #endif
-
+#ifdef USE_DSHOT
+        if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
+            sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
+            DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH);
+        }
+#endif
         statsOnDisarm();
         logicConditionReset();
 #ifdef USE_PROGRAMMING_FRAMEWORK	    
@@ -459,6 +464,21 @@ void releaseSharedTelemetryPorts(void) {
 void tryArm(void)
 {
     updateArmingStatus();
+
+#ifdef USE_DSHOT
+    if (
+            STATE(MULTIROTOR) &&
+            IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) &&
+            emergencyArmingCanOverrideArmingDisabled() &&
+            isMotorProtocolDshot() &&
+            !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)
+            ) {
+        sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
+        ENABLE_ARMING_FLAG(ARMED);
+        enableFlightMode(FLIP_OVER_AFTER_CRASH);
+        return;
+    }
+#endif
 #ifdef USE_PROGRAMMING_FRAMEWORK
     if (
         !isArmingDisabled() || 

src/main/fc/fc_init.c

@@ -673,6 +673,10 @@ void init(void)
     rcdeviceInit();
 #endif // USE_RCDEVICE
 
+#ifdef USE_DSHOT
+    initDShotCommands();
+#endif
+
     // Latch active features AGAIN since some may be modified by init().
     latchActiveFeatures();
     motorControlEnable = true;

src/main/fc/fc_msp_box.c

@@ -33,6 +33,8 @@
 
 #include "io/osd.h"
 
+#include "drivers/pwm_output.h"
+
 #include "sensors/diagnostics.h"
 #include "sensors/sensors.h"
 
@@ -87,6 +89,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
     { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
     { BOXPREARM, "PREARM", 51 },
+    { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -306,6 +309,11 @@ void initActiveBoxIds(void)
 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
     activeBoxIds[activeBoxIdCount++] = BOXMSPRCOVERRIDE;
 #endif
+
+#ifdef USE_DSHOT
+    if(STATE(MULTIROTOR) && isMotorProtocolDshot())
+        activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH;
+#endif
 }
 
 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)

src/main/fc/rc_modes.h

@@ -69,6 +69,7 @@ typedef enum {
     BOXLOITERDIRCHN  = 40,
     BOXMSPRCOVERRIDE = 41,
     BOXPREARM        = 42,
+    BOXFLIPOVERAFTERCRASH = 43,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
@@ -128,4 +129,4 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 
 void updateActivatedModes(void);
 void updateUsedModeActivationConditionFlags(void);
-void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);
+void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);

src/main/fc/runtime_config.h

@@ -97,6 +97,7 @@ typedef enum {
     NAV_CRUISE_MODE = (1 << 12),
     FLAPERON        = (1 << 13),
     TURN_ASSISTANT  = (1 << 14),
+    FLIP_OVER_AFTER_CRASH = (1 << 15),
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;