Improved CRSF flight mode reporting.

Makefile

@@ -105,7 +105,11 @@ ifneq ($(TRAVIS_BUILD_NUMBER),)
 BUILDNO := $(TRAVIS_BUILD_NUMBER)
 endif
 
+BUILDDATETIME := $(shell date +'%Y%m%d%Z')
+REVISION := uncommitted_$(BUILDDATETIME)
+ifeq ($(shell git diff --shortstat),)
 REVISION := $(shell git log -1 --format="%h")
+endif
 
 FC_VER_MAJOR := $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' )
 FC_VER_MINOR := $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk '{print $$3}' )

src/main/flight/pid.c

@@ -190,6 +190,7 @@ void resetPidProfile(pidProfile_t *pidProfile) {
     .dterm_ABG_boost = 275,
     .dterm_ABG_half_life = 50,
     .emuGravityGain = 100,
+    .angle_filter = 100,
                 );
 }
 
@@ -221,6 +222,8 @@ static FAST_RAM filterApplyFnPtr dtermLowpassApplyFn = nullFilterApply;
 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[XYZ_AXIS_COUNT];
 static FAST_RAM filterApplyFnPtr dtermLowpass2ApplyFn = nullFilterApply;
 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass2[XYZ_AXIS_COUNT];
+static FAST_RAM filterApplyFnPtr angleSetpointFilterApplyFn = nullFilterApply;
+static FAST_RAM_ZERO_INIT pt1Filter_t angleSetpointFilter[2];
 static FAST_RAM filterApplyFnPtr dtermABGapplyFn = nullFilterApply;
 static FAST_RAM_ZERO_INIT alphaBetaGammaFilter_t dtermABG[XYZ_AXIS_COUNT];
 
@@ -248,6 +251,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
     const uint32_t pidFrequencyNyquist = pidFrequency / 2; // No rounding needed
     dtermLowpassApplyFn = nullFilterApply;
     dtermLowpass2ApplyFn = nullFilterApply;
+    angleSetpointFilterApplyFn = nullFilterApply;
     for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
         if (pidProfile->dFilter[axis].dLpf && pidProfile->dFilter[axis].dLpf <= pidFrequencyNyquist) {
             switch (pidProfile->dterm_filter_type) {
@@ -275,6 +279,10 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
                 break;
             }
         }
+        if (pidProfile->angle_filter) {
+            angleSetpointFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
+            pt1FilterInit(&angleSetpointFilter[axis], pt1FilterGain(pidProfile->angle_filter, dT));
+        }
         if (pidProfile->dterm_ABG_alpha) {
             dtermABGapplyFn = (filterApplyFnPtr)alphaBetaGammaApply;
             ABGInit(&dtermABG[axis], pidProfile->dterm_ABG_alpha, pidProfile->dterm_ABG_boost, pidProfile->dterm_ABG_half_life, dT);
@@ -504,6 +512,7 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
         const float horizonLevelStrength = calcHorizonLevelStrength();
         currentPidSetpoint = ((getSetpointRate(axis) * (1 - horizonLevelStrength)) + getSetpointRate(axis)) * 0.5f + (currentPidSetpoint * horizonLevelStrength * horizonStrength);
     }
+    currentPidSetpoint = angleSetpointFilterApplyFn((filter_t *)&angleSetpointFilter[axis], currentPidSetpoint);
     directFF[axis] = (1 - fabsf(errorAnglePercent)) * DF_angle_low;
     directFF[axis] += fabsf(errorAnglePercent) * DF_angle_high;
     return currentPidSetpoint;
@@ -670,6 +679,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
         } else if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && FLIGHT_MODE(NFE_RACE_MODE) && (axis != FD_PITCH)) {
             currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
         }
+
         // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
         // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
 #ifdef USE_YAW_SPIN_RECOVERY

src/main/flight/pid.h

@@ -104,10 +104,11 @@ typedef struct pidProfile_s {
     uint8_t pidAtMinThrottle;               // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
     uint8_t levelAngleLimit;                // Max angle in degrees in level mode
     uint8_t angleExpo;                      // How much expo to add to the
+    uint8_t angle_filter;                   // filter we stick on the rate setpoint angle makes
 
     uint8_t horizonTransition;              // horizonTransition
     uint8_t horizon_tilt_effect;            // inclination factor for Horizon mode
-    uint8_t horizonStrength;               // boost or shrink to angle pids while in horizon mode
+    uint8_t horizonStrength;                // boost or shrink to angle pids while in horizon mode
 
     // EmuFlight PID controller parameters
     uint8_t feathered_pids;                 // determine how feathered your pids are

src/main/interface/msp.c

@@ -766,9 +766,9 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
     }
     break;
     case MSP_NAME: {
-      // Show warning for DJI OSD instead of pilot name
+        // Show warning for DJI OSD instead of pilot name
         // works if osd warnings enabled, osd_warn_dji is on and usb is not connected
-        if (osdWarnDjiEnabled()) {
+        if (osdWarnGetState(OSD_WARNING_DJI)) {
             sbufWriteString(dst, djiWarningBuffer);
             break;
         }

src/main/interface/settings.c

@@ -936,6 +936,7 @@ const clivalue_t valueTable[] = {
 
     { "level_limit",                VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 10, 90 }, PG_PID_PROFILE, offsetof(pidProfile_t, levelAngleLimit) },
     { "angle_expo",                 VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, angleExpo) },
+    { "angle_filter",               VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_filter) },
 
     { "horizon_transition",         VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0,  200 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizonTransition) },
     { "horizon_tilt_effect",        VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0,  180 }, PG_PID_PROFILE, offsetof(pidProfile_t, horizon_tilt_effect) },

src/main/io/osd.c

@@ -394,7 +394,7 @@ static void osdFormatMessage(char *buff, size_t size, const char *message) {
         memcpy(buff, message, strlen(message));
     }
     // Write warning for DJI
-    if (osdWarnDjiEnabled()) {
+    if (osdWarnGetState(OSD_WARNING_DJI)) {
         if (message) {
             tfp_sprintf(djiWarningBuffer, message);
         } else {
@@ -442,14 +442,6 @@ bool osdWarnGetState(uint8_t warningIndex) {
     return osdConfig()->enabledWarnings & (1 << warningIndex);
 }
 
-bool osdWarnDjiEnabled(void) {
-    return osdWarnGetState(OSD_WARNING_DJI)
-#ifdef USE_VCP
-           && !usbVcpIsConnected()
-#endif
-           ;
-}
-
 static bool osdDrawSingleElement(uint8_t item) {
     if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) {
         return false;

src/main/io/osd.h

@@ -229,5 +229,4 @@ void osdStatSetState(uint8_t statIndex, bool enabled);
 bool osdStatGetState(uint8_t statIndex);
 void osdWarnSetState(uint8_t warningIndex, bool enabled);
 bool osdWarnGetState(uint8_t warningIndex);
-bool osdWarnDjiEnabled(void);
 void setCrsfRssi(bool b);

src/main/target/RUSHBLADEF7/target.h

@@ -33,13 +33,13 @@
 //#define BEEPER_PWM_HZ           1100
 
 #define USE_EXTI
-#define MPU_INT_EXTI PC4
+#define MPU_INT_EXTI PA4
 #define USE_MPU_DATA_READY_SIGNAL
 
 #define USE_ACC
 #define USE_GYRO
 //------MPU6000
-#define MPU6000_CS_PIN           PA4
+#define MPU6000_CS_PIN           PC4
 #define MPU6000_SPI_INSTANCE     SPI1
 
 #define USE_GYRO_SPI_MPU6000

src/main/telemetry/crsf.c

@@ -258,19 +258,40 @@ void crsfFrameFlightMode(sbuf_t *dst) {
     uint8_t *lengthPtr = sbufPtr(dst);
     sbufWriteU8(dst, 0);
     sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
-    // use same logic as OSD, so telemetry displays same flight text as OSD
+
+    // Acro is the default mode
     const char *flightMode = "ACRO";
-    if (isAirmodeActive()) {
-        flightMode = "AIR";
-    }
+
+    // Modes that are only relevant when disarmed
+    if (!ARMING_FLAG(ARMED) && isArmingDisabled()) {
+        flightMode = "!ERR";
+    } else
+
+#if defined(USE_GPS)
+    if (!ARMING_FLAG(ARMED) && feature(FEATURE_GPS) && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
+        flightMode = "WAIT"; // Waiting for GPS lock
+    } else
+#endif
+
+    // Flight modes in decreasing order of importance
     if (FLIGHT_MODE(FAILSAFE_MODE)) {
-        flightMode = "!FS";
+        flightMode = "!FS!";
+    } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
+        flightMode = "RTH";
+    } else if (FLIGHT_MODE(PASSTHRU_MODE)) {
+        flightMode = "MANU";
     } else if (FLIGHT_MODE(ANGLE_MODE)) {
         flightMode = "STAB";
     } else if (FLIGHT_MODE(HORIZON_MODE)) {
         flightMode = "HOR";
+    } else if (isAirmodeActive()) {
+        flightMode = "AIR";
     }
+
     sbufWriteString(dst, flightMode);
+    if (!ARMING_FLAG(ARMED)) {
+        sbufWriteU8(dst, '*');
+    }
     sbufWriteU8(dst, '\0');     // zero-terminate string
     // write in the frame length
     *lengthPtr = sbufPtr(dst) - lengthPtr;