WIP: Fixing unittests

.github/workflows/build.yml

@@ -9,17 +9,37 @@ on:
     - '*'
   # repository_dispatch is a newer github-actions feature that will allow building from triggers other than code merge/PR
   repository_dispatch:
-    types: [build]
+    types: [tests, build]
 
 name: Build EmuFlight
 jobs:
+  test:
+    name: Test EmuFlight
+    runs-on: ubuntu-latest
+    steps: 
+    - name: Checkout
+      uses: actions/checkout@v2
+      with:
+        fetch-depth: 0
+    - name: Setup Toolchain
+      uses: fiam/arm-none-eabi-gcc@master
+      with:
+        release: '9-2020-q2' # TODO: use 10-2020-q4-major when applicable
+    - name: Install Prerequisites
+      run: |
+        sudo apt install clang-10 lcov libblocksruntime-dev libc6-i386 
+    - name : Run Tests
+      run: |
+        make test
   build:
+    name: Build EmuFlight
     timeout-minutes: 75
     strategy:
       max-parallel: 4
       matrix:
         targets: [targets-group-1, targets-group-2, targets-group-3, targets-group-rest]
     runs-on: ubuntu-latest
+    needs: test
     steps:
 
     # curl, by default, may timeout easily

src/main/blackbox/blackbox.c

@@ -1202,7 +1202,6 @@ static bool blackboxWriteSysinfo(void) {
         BLACKBOX_PRINT_HEADER_LINE("acc_limit", "%d",                       currentPidProfile->rateAccelLimit);
         BLACKBOX_PRINT_HEADER_LINE("pidsum_limit", "%d",                    currentPidProfile->pidSumLimit);
         BLACKBOX_PRINT_HEADER_LINE("pidsum_limit_yaw", "%d",                currentPidProfile->pidSumLimitYaw);
-        BLACKBOX_PRINT_HEADER_LINE("nfe_racermode", "%d",                   currentPidProfile->nfe_racermode);
         BLACKBOX_PRINT_HEADER_LINE("iterm_rotation", "%d",                  currentPidProfile->iterm_rotation);
         BLACKBOX_PRINT_HEADER_LINE("throttle_boost", "%d",                  currentPidProfile->throttle_boost);
         // End of EmuFlight controller parameters

src/main/build/atomic.h

@@ -28,13 +28,15 @@
 // missing versions are implemented here
 
 // set BASEPRI register, do not create memory barrier
-__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_nb(uint32_t basePri) {
-    __ASM volatile ("\tMSR basepri, %0\n" : : "r" (basePri) );
+__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_nb(uint32_t basePri)
+{
+   __ASM volatile ("\tMSR basepri, %0\n" : : "r" (basePri) );
 }
 
 // set BASEPRI_MAX register, do not create memory barrier
-__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint32_t basePri) {
-    __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) );
+__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint32_t basePri)
+{
+   __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) );
 }
 
 #endif
@@ -44,18 +46,21 @@ __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint3
 
 extern uint8_t atomic_BASEPRI;
 
-static inline uint8_t __get_BASEPRI(void) {
+static inline uint8_t __get_BASEPRI(void)
+{
     return atomic_BASEPRI;
 }
 
 // restore BASEPRI (called as cleanup function), with global memory barrier
-static inline void __basepriRestoreMem(uint8_t *val) {
+static inline void __basepriRestoreMem(uint8_t *val)
+{
     atomic_BASEPRI = *val;
     asm volatile ("": : :"memory"); // compiler memory barrier
 }
 
 // increase BASEPRI, with global memory barrier, returns true
-static inline uint8_t __basepriSetMemRetVal(uint8_t prio) {
+static inline uint8_t __basepriSetMemRetVal(uint8_t prio)
+{
     if(prio && (atomic_BASEPRI == 0 || atomic_BASEPRI > prio)) {
         atomic_BASEPRI = prio;
     }
@@ -64,12 +69,14 @@ static inline uint8_t __basepriSetMemRetVal(uint8_t prio) {
 }
 
 // restore BASEPRI (called as cleanup function), no memory barrier
-static inline void __basepriRestore(uint8_t *val) {
+static inline void __basepriRestore(uint8_t *val)
+{
     atomic_BASEPRI = *val;
 }
 
 // increase BASEPRI, no memory barrier, returns true
-static inline uint8_t __basepriSetRetVal(uint8_t prio) {
+static inline uint8_t __basepriSetRetVal(uint8_t prio)
+{
     if(prio && (atomic_BASEPRI == 0 || atomic_BASEPRI > prio)) {
         atomic_BASEPRI = prio;
     }
@@ -80,23 +87,27 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) {
 // ARM BASEPRI manipulation
 
 // restore BASEPRI (called as cleanup function), with global memory barrier
-static inline void __basepriRestoreMem(uint8_t *val) {
+static inline void __basepriRestoreMem(uint8_t *val)
+{
     __set_BASEPRI(*val);
 }
 
 // set BASEPRI_MAX, with global memory barrier, returns true
-static inline uint8_t __basepriSetMemRetVal(uint8_t prio) {
+static inline uint8_t __basepriSetMemRetVal(uint8_t prio)
+{
     __set_BASEPRI_MAX(prio);
     return 1;
 }
 
 // restore BASEPRI (called as cleanup function), no memory barrier
-static inline void __basepriRestore(uint8_t *val) {
+static inline void __basepriRestore(uint8_t *val)
+{
     __set_BASEPRI_nb(*val);
 }
 
 // set BASEPRI_MAX, no memory barrier, returns true
-static inline uint8_t __basepriSetRetVal(uint8_t prio) {
+static inline uint8_t __basepriSetRetVal(uint8_t prio)
+{
     __set_BASEPRI_MAX_nb(prio);
     return 1;
 }
@@ -126,13 +137,6 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) {
 // On gcc 5 and higher, this protects only memory passed as parameter (any type can be used)
 // this macro can be used only ONCE PER LINE, but multiple uses per block are fine
 
-#if (__GNUC__ > 9)
-# warning "Please verify that ATOMIC_BARRIER works as intended"
-// increment version number if BARRIER works
-// TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead
-// you should check that local variable scope with cleanup spans entire block
-#endif
-
 #ifndef __UNIQL
 # define __UNIQL_CONCAT2(x,y) x ## y
 # define __UNIQL_CONCAT(x,y) __UNIQL_CONCAT2(x,y)
@@ -149,14 +153,12 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio) {
 // CLang version, using Objective C-style block
 // based on https://stackoverflow.com/questions/24959440/rewrite-gcc-cleanup-macro-with-nested-function-for-clang
 typedef void (^__cleanup_block)(void);
-static inline void __do_cleanup(__cleanup_block * b) {
-    (*b)();
-}
+static inline void __do_cleanup(__cleanup_block * b) { (*b)(); }
 
 #define ATOMIC_BARRIER(data)                                            \
     typeof(data) *__UNIQL(__barrier) = &data;                           \
     ATOMIC_BARRIER_ENTER(__UNIQL(__barrier), #data);                    \
-    __cleanup_block __attribute__((cleanup(__do_cleanup) __unused__)) __UNIQL(__cleanup) = \
+    __cleanup_block __attribute__((cleanup(__do_cleanup), __unused__)) __UNIQL(__cleanup) = \
         ^{  ATOMIC_BARRIER_LEAVE(__UNIQL(__barrier), #data); };         \
     do {} while(0)                                                      \
 /**/

src/main/cms/cms_menu_imu.c

@@ -344,7 +344,6 @@ static uint8_t  cmsx_P_angle_high;
 static uint8_t  cmsx_D_angle_high;
 static uint16_t cmsx_F_angle;
 static uint8_t  cmsx_horizonTransition;
-static uint8_t  cmsx_nfe_racermode;
 static uint8_t  cmsx_throttleBoost;
 static uint8_t  cmsx_motorOutputLimit;
 static int8_t   cmsx_autoProfileCellCount;
@@ -367,7 +366,6 @@ static long cmsx_profileOtherOnEnter(void) {
     cmsx_D_angle_high =      pidProfile->pid[PID_LEVEL_HIGH].D;
     cmsx_F_angle =       pidProfile->pid[PID_LEVEL_LOW].F;
     cmsx_horizonTransition = pidProfile->horizonTransition;
-    cmsx_nfe_racermode = pidProfile->nfe_racermode;
     cmsx_throttleBoost = pidProfile->throttle_boost;
     cmsx_motorOutputLimit = pidProfile->motor_output_limit;
     cmsx_autoProfileCellCount = pidProfile->auto_profile_cell_count;
@@ -393,7 +391,6 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self) {
     pidProfile->pid[PID_LEVEL_HIGH].D = cmsx_D_angle_high;
     pidProfile->pid[PID_LEVEL_LOW].F = cmsx_F_angle;
     pidProfile->horizonTransition = cmsx_horizonTransition;
-    pidProfile->nfe_racermode = cmsx_nfe_racermode;
     pidProfile->throttle_boost = cmsx_throttleBoost;
     pidProfile->motor_output_limit = cmsx_motorOutputLimit;
     pidProfile->auto_profile_cell_count = cmsx_autoProfileCellCount;
@@ -419,7 +416,6 @@ static OSD_Entry cmsx_menuProfileOtherEntries[] = {
     { "ANGLE D HIGH",    OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_D_angle_high,              0,    200,   1  }, 0 },
     { "ANGLE F",         OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_F_angle,                   0,    2000,  1  }, 0 },
     { "HORZN TRS",       OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_horizonTransition,         0,    200,   1  }, 0 },
-    { "NFE RACERMODE",   OME_TAB, NULL, &(OSD_TAB_t)  { &cmsx_nfe_racermode, 1, cms_offOnLabels }, 0 },
 #ifdef USE_THROTTLE_BOOST
     { "THR BOOST",       OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_throttleBoost,             0,    100,   1  }, 0 },
 #endif

src/main/common/maths.h

@@ -27,6 +27,8 @@
 #endif
 #define power3(x) ((x)*(x)*(x))
 
+#define SIGN(x) ((x > 0.0f) - (x < 0.0f))
+
 // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
 #define FAST_MATH             // order 9 approximation
 #define VERY_FAST_MATH      // order 7 approximation

src/main/fc/config.c

@@ -236,19 +236,6 @@ static void validateAndFixConfig(void) {
            ) {
             rxConfigMutable()->rssi_src_frame_errors = false;
         }
-    if (!rcSmoothingIsEnabled() || rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_T) {
-        for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
-            pidProfilesMutable(i)->pid[PID_ROLL].F = 0;
-            pidProfilesMutable(i)->pid[PID_PITCH].F = 0;
-        }
-    }
-    if (!rcSmoothingIsEnabled() ||
-            (rxConfig()->rcInterpolationChannels != INTERPOLATION_CHANNELS_RPY &&
-             rxConfig()->rcInterpolationChannels != INTERPOLATION_CHANNELS_RPYT)) {
-        for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
-            pidProfilesMutable(i)->pid[PID_YAW].F = 0;
-        }
-    }
 #if defined(USE_THROTTLE_BOOST)
     if (!rcSmoothingIsEnabled() ||
             !(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT
@@ -280,6 +267,10 @@ static void validateAndFixConfig(void) {
         featureClear(FEATURE_ESC_SENSOR);
     }
 #endif
+
+    if (currentPidProfile->horizonTransition >= currentPidProfile->horizon_tilt_effect) {
+        pidProfilesMutable(systemConfig()->pidProfileIndex)->horizonTransition = MAX(currentPidProfile->horizon_tilt_effect - 20, 0);
+    }
 // clear features that are not supported.
 // I have kept them all here in one place, some could be moved to sections of code above.
 #ifndef USE_PPM

src/main/fc/fc_core.c

@@ -681,6 +681,13 @@ bool processRx(timeUs_t currentTimeUs) {
     } else {
         DISABLE_FLIGHT_MODE(HORIZON_MODE);
     }
+    if (IS_RC_MODE_ACTIVE(BOXNFEMODE) && (FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLE_MODE))) {
+        if (!FLIGHT_MODE(NFE_RACE_MODE)) {
+            ENABLE_FLIGHT_MODE(NFE_RACE_MODE);
+        }
+    } else {
+        DISABLE_FLIGHT_MODE(NFE_RACE_MODE);
+    }
 #ifdef USE_GPS_RESCUE
     if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) ) {
         if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {

src/main/fc/rc_modes.h

@@ -40,7 +40,8 @@ typedef enum {
     BOXPASSTHRU,
     BOXFAILSAFE,
     BOXGPSRESCUE,
-    BOXID_FLIGHTMODE_LAST = BOXGPSRESCUE,
+    BOXNFEMODE,
+    BOXID_FLIGHTMODE_LAST = BOXNFEMODE,
 
 // When new flight modes are added, the parameter group version for 'modeActivationConditions' in src/main/fc/rc_modes.c has to be incremented to ensure that the RC modes configuration is reset.
 

src/main/fc/runtime_config.h

@@ -81,7 +81,8 @@ typedef enum {
     PASSTHRU_MODE   = (1 << 8),
 //    RANGEFINDER_MODE= (1 << 9),
     FAILSAFE_MODE   = (1 << 10),
-    GPS_RESCUE_MODE = (1 << 11)
+    GPS_RESCUE_MODE = (1 << 11),
+    NFE_RACE_MODE   = (1 << 12)
 } flightModeFlags_e;
 
 extern uint16_t flightModeFlags;
@@ -103,6 +104,7 @@ extern uint16_t flightModeFlags;
    [BOXPASSTHRU]    = LOG2(PASSTHRU_MODE),               \
    [BOXFAILSAFE]    = LOG2(FAILSAFE_MODE),               \
    [BOXGPSRESCUE]   = LOG2(GPS_RESCUE_MODE),             \
+   [BOXNFEMODE]     = LOG2(NFE_RACE_MODE),               \
 }                                                        \
 /**/
 

src/main/flight/pid.c

@@ -162,8 +162,6 @@ void resetPidProfile(pidProfile_t *pidProfile) {
     .crash_gthreshold = 400,                  // degrees/second
     .crash_setpoint_threshold = 350,          // degrees/second
     .crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
-    .horizon_tilt_effect = 130,
-    .nfe_racermode = false,
     .crash_limit_yaw = 200,
     .itermLimit = 400,
     .throttle_boost = 5,
@@ -175,7 +173,9 @@ void resetPidProfile(pidProfile_t *pidProfile) {
     .iterm_relax_threshold_yaw = 35,
     .motor_output_limit = 100,
     .auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY,
+    .horizon_tilt_effect = 80,
     .horizonTransition = 0,
+    .horizonStrength = 15,
                 );
 }
 
@@ -318,13 +318,12 @@ typedef struct pidCoefficient_s {
 static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT float maxVelocity[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT float feathered_pids;
-static FAST_RAM_ZERO_INIT uint8_t nfe_racermode;
 static FAST_RAM_ZERO_INIT float smart_dterm_smoothing[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT float setPointPTransition[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT float setPointITransition[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT float setPointDTransition[XYZ_AXIS_COUNT];
 static FAST_RAM_ZERO_INIT float dtermBoostMultiplier, dtermBoostLimitPercent;
-static FAST_RAM_ZERO_INIT float P_angle_low, D_angle_low, P_angle_high, D_angle_high, F_angle, horizonTransition, horizonCutoffDegrees;
+static FAST_RAM_ZERO_INIT float P_angle_low, D_angle_low, P_angle_high, D_angle_high, F_angle, horizonTransition, horizonCutoffDegrees, horizonStrength;
 static FAST_RAM_ZERO_INIT float ITermWindupPointInv;
 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs;
 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs;
@@ -359,7 +358,6 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
         smart_dterm_smoothing[axis] = pidProfile->dFilter[axis].smartSmoothing;
     }
     feathered_pids = pidProfile->feathered_pids / 100.0f;
-    nfe_racermode = pidProfile->nfe_racermode;
     dtermBoostMultiplier = (pidProfile->dtermBoost * pidProfile->dtermBoost / 1000000) * 0.003;
     dtermBoostLimitPercent = pidProfile->dtermBoostLimit / 100.0f;
     P_angle_low = pidProfile->pid[PID_LEVEL_LOW].P * 0.1f;
@@ -369,6 +367,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) {
     F_angle = pidProfile->pid[PID_LEVEL_LOW].F * 0.00000125f;
     horizonTransition = (float)pidProfile->horizonTransition;
     horizonCutoffDegrees = pidProfile->horizon_tilt_effect;
+    horizonStrength = pidProfile->horizonStrength / 50.0f;
     maxVelocity[FD_ROLL] = maxVelocity[FD_PITCH] = pidProfile->rateAccelLimit * 100 * dT;
     maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
     ITermWindupPointInv = 0.0f;
@@ -427,13 +426,11 @@ static float calcHorizonLevelStrength(void) {
         horizonLevelStrength = inclinationLevelRatio;
     } else {
         // if racemode_tilt_effect = 0 or horizonTransition>racemode_tilt_effect means no leveling
-        horizonLevelStrength = 0;
+        horizonLevelStrength = 1;
     }
     return constrainf(horizonLevelStrength, 0, 1);
 }
 
-#define SIGN(x) ((x > 0.0f) - (x < 0.0f))
-
 static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
     // calculate error angle and limit the angle to the max inclination
     // rcDeflection is in range [-1.0, 1.0]
@@ -452,12 +449,12 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
     angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
     float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) * 0.1f);
     errorAngle = constrainf(errorAngle, -90.0f, 90.0f);
-    const float errorAnglePercent = errorAngle / 90.0f;
+    const float errorAnglePercent = fabsf(errorAngle / 90.0f);
     // ANGLE mode - control is angle based
-    p_term_low = (1 - fabsf(errorAnglePercent)) * errorAngle * P_angle_low;
-    p_term_high = fabsf(errorAnglePercent) * errorAngle * P_angle_high;
-    d_term_low = (1 - fabsf(errorAnglePercent)) * (attitudePrevious[axis] - attitude.raw[axis]) * 0.1f * D_angle_low;
-    d_term_high = fabsf(errorAnglePercent) * (attitudePrevious[axis] - attitude.raw[axis]) * 0.1f * D_angle_high;
+    p_term_low = (1 - errorAnglePercent) * errorAngle * P_angle_low;
+    p_term_high = errorAnglePercent * errorAngle * P_angle_high;
+    d_term_low = (1 - errorAnglePercent) * (attitudePrevious[axis] - attitude.raw[axis]) * 0.1f * D_angle_low;
+    d_term_high = errorAnglePercent * (attitudePrevious[axis] - attitude.raw[axis]) * 0.1f * D_angle_high;
     attitudePrevious[axis] = attitude.raw[axis];
     currentPidSetpoint = p_term_low + p_term_high;
     currentPidSetpoint += d_term_low + d_term_high;
@@ -466,7 +463,7 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
         // HORIZON mode - mix of ANGLE and ACRO modes
         // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
         const float horizonLevelStrength = calcHorizonLevelStrength();
-        currentPidSetpoint = (((getSetpointRate(axis) * (1 - horizonLevelStrength)) + getSetpointRate(axis)) * 0.5f) + (currentPidSetpoint * horizonLevelStrength);
+        currentPidSetpoint = ((getSetpointRate(axis) * (1 - horizonLevelStrength)) + getSetpointRate(axis)) * 0.5f + (currentPidSetpoint * horizonLevelStrength * horizonStrength);
     }
     return currentPidSetpoint;
 }
@@ -616,9 +613,9 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
         // NFE racermode applies angle only to the roll axis
         if (FLIGHT_MODE(GPS_RESCUE_MODE) && axis != FD_YAW) {
             currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
-        } else if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && !nfe_racermode && (axis != FD_YAW)) {
+        } else if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && !FLIGHT_MODE(NFE_RACE_MODE) && (axis != FD_YAW)) {
             currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
-        } else if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && nfe_racermode && ((axis != FD_YAW) && (axis != FD_PITCH))) {
+        } else if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && FLIGHT_MODE(NFE_RACE_MODE) && ((axis != FD_YAW) && (axis != FD_PITCH))) {
             currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
         }
         // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery