Add hooks for triflight

make/source.mk

@@ -102,8 +102,10 @@ FC_SRC = \
             flight/failsafe.c \
             flight/imu.c \
             flight/mixer.c \
+            flight/mixer_tricopter.c \
             flight/pid.c \
             flight/servos.c \
+            flight/servos_tricopter.c \
             interface/cli.c \
             interface/settings.c \
             io/serial_4way.c \

src/main/flight/mixer.c

@@ -49,6 +49,7 @@
 #include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
+#include "flight/mixer_tricopter.h"
 #include "flight/pid.h"
 
 #include "rx/rx.h"
@@ -106,8 +107,6 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR
 
 #define PWM_RANGE_MID 1500
 
-#define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight
-
 static FAST_RAM uint8_t motorCount;
 static FAST_RAM float motorMixRange;
 
@@ -341,10 +340,19 @@ bool areMotorsRunning(void)
     return motorsRunning;
 }
 
+bool mixerIsTricopter(void)
+{
+#ifdef USE_SERVOS
+    return (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI);
+#else
+    return false;
+#endif
+}
+
 bool mixerIsOutputSaturated(int axis, float errorRate)
 {
-    if (axis == FD_YAW && (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI)) {
-        return errorRate > TRICOPTER_ERROR_RATE_YAW_SATURATED;
+    if (axis == FD_YAW && mixerIsTricopter()) {
+        return mixerTricopterIsServoSaturated(errorRate);
     } else {
         return motorMixRange >= 1.0f;
     }
@@ -404,6 +412,9 @@ void mixerInit(mixerMode_e mixerMode)
     currentMixerMode = mixerMode;
 
     initEscEndpoints();
+    if (mixerIsTricopter()) {
+        mixerTricopterInit();
+    }
 }
 
 #ifndef USE_QUAD_MIXER_ONLY
@@ -641,8 +652,11 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
 {
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
-    for (uint32_t i = 0; i < motorCount; i++) {
+    for (int i = 0; i < motorCount; i++) {
         float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle));
+        if (mixerIsTricopter()) {
+            motorOutput += mixerTricopterMotorCorrection(i);
+        }
         if (failsafeIsActive()) {
             if (isMotorProtocolDshot()) {
                 motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range

src/main/flight/mixer.h

@@ -128,3 +128,4 @@ void stopPwmAllMotors(void);
 
 float convertExternalToMotor(uint16_t externalValue);
 uint16_t convertMotorToExternal(float motorValue);
+bool mixerIsTricopter(void);

src/main/flight/mixer_tricopter.c

@@ -0,0 +1,58 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef USE_SERVOS
+
+#include "common/utils.h"
+
+#include "flight/mixer.h"
+#include "flight/mixer_tricopter.h"
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+
+PG_REGISTER_WITH_RESET_TEMPLATE(tricopterMixerConfig_t, tricopterMixerConfig, PG_TRICOPTER_CONFIG, 0);
+
+PG_RESET_TEMPLATE(tricopterMixerConfig_t, tricopterMixerConfig,
+    .dummy = 0
+);
+
+#define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight
+
+bool mixerTricopterIsServoSaturated(float errorRate)
+{
+    return errorRate > TRICOPTER_ERROR_RATE_YAW_SATURATED;
+}
+
+float mixerTricopterMotorCorrection(int motor)
+{
+    UNUSED(motor);
+    return 0.0f;
+}
+
+void mixerTricopterInit(void)
+{
+
+}
+
+#endif // USE_SERVOS

src/main/flight/mixer_tricopter.h

@@ -0,0 +1,33 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "platform.h"
+
+#include "pg/pg.h"
+
+typedef struct tricopterMixerConfig_s {
+    uint8_t dummy;
+} tricopterMixerConfig_t;
+
+PG_DECLARE(tricopterMixerConfig_t, tricopterMixerConfig);
+
+
+bool mixerTricopterIsServoSaturated(float errorRate);
+void mixerTricopterInit(void);
+float mixerTricopterMotorCorrection(int motor);

src/main/flight/servos.c

@@ -232,6 +232,10 @@ void servosInit(void)
     for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
         servo[i] = DEFAULT_SERVO_MIDDLE;
     }
+
+    if (mixerIsTricopter()) {
+        servosTricopterInit();
+    }
 }
 
 void loadCustomServoMixer(void)
@@ -319,7 +323,7 @@ void writeServos(void)
     switch (currentMixerMode) {
     case MIXER_TRI:
     case MIXER_CUSTOM_TRI:
-        if (servoConfig()->tri_unarmed_servo) {
+        if (servosTricopterIsEnabledServoUnarmed()) {
             // if unarmed flag set, we always move servo
             pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
         } else {
@@ -385,7 +389,7 @@ void writeServos(void)
     }
 }
 
-STATIC_UNIT_TESTED void servoMixer(void)
+void servoMixer(void)
 {
     int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
     static int16_t currentOutput[MAX_SERVO_RULES];
@@ -467,12 +471,14 @@ static void servoTable(void)
 {
     // airplane / servo mixes
     switch (currentMixerMode) {
+    case MIXER_CUSTOM_TRI:
+    case MIXER_TRI:
+        servosTricopterMixer();
+        break;
     case MIXER_CUSTOM_AIRPLANE:
     case MIXER_FLYING_WING:
     case MIXER_AIRPLANE:
     case MIXER_BICOPTER:
-    case MIXER_CUSTOM_TRI:
-    case MIXER_TRI:
     case MIXER_DUALCOPTER:
     case MIXER_SINGLECOPTER:
     case MIXER_HELI_120_CCPM:

src/main/flight/servos.h

@@ -140,3 +140,8 @@ int servoDirection(int servoIndex, int fromChannel);
 void servoConfigureOutput(void);
 void servosInit(void);
 void servosFilterInit(void);
+void servoMixer(void);
+// tricopter specific
+void servosTricopterInit(void);
+void servosTricopterMixer(void);
+bool servosTricopterIsEnabledServoUnarmed(void);

src/main/flight/servos_tricopter.c

@@ -0,0 +1,45 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef USE_SERVOS
+
+#include "flight/mixer.h"
+#include "flight/mixer_tricopter.h"
+#include "flight/servos.h"
+
+
+bool servosTricopterIsEnabledServoUnarmed(void)
+{
+    return servoConfig()->tri_unarmed_servo;
+}
+
+void servosTricopterMixer(void)
+{
+    servoMixer();
+}
+
+void servosTricopterInit(void)
+{
+
+}
+
+#endif // USE_SERVOS

src/main/pg/pg_ids.h

@@ -118,7 +118,8 @@
 #define PG_FLYSKY_CONFIG 525
 #define PG_TIME_CONFIG 526
 #define PG_RANGEFINDER_CONFIG 527 // iNav
-#define PG_BETAFLIGHT_END 527
+#define PG_TRICOPTER_CONFIG 528
+#define PG_BETAFLIGHT_END 528
 
 
 // OSD configuration (subject to change)