Merge branch 'master' of https://github.com/borisbstyle/betaflight into PikoBLX_target_bf

src/main/config/config.c

@@ -199,7 +199,11 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig)
 
 static void resetPidProfile(pidProfile_t *pidProfile)
 {
+#if defined(SKIP_PID_FLOAT)
+    pidProfile->pidController = PID_CONTROLLER_LEGACY;
+#else
     pidProfile->pidController = PID_CONTROLLER_BETAFLIGHT;
+#endif
 
     pidProfile->P8[ROLL] = 45;
     pidProfile->I8[ROLL] = 40;
@@ -243,8 +247,8 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
 
     // Betaflight PID controller parameters
-    pidProfile->ptermSetpointWeight = 75;
+    pidProfile->ptermSetpointWeight = 80;
-    pidProfile->dtermSetpointWeight = 120;
+    pidProfile->dtermSetpointWeight = 150;
     pidProfile->yawRateAccelLimit = 220;
     pidProfile->rateAccelLimit = 0;
     pidProfile->toleranceBand = 0;
@@ -450,7 +454,7 @@ void createDefaultConfig(master_t *config)
     memset(config, 0, sizeof(master_t));
 
     intFeatureClearAll(config);
-    intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES, config);
+    intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE , config);
 #ifdef DEFAULT_FEATURES
     intFeatureSet(DEFAULT_FEATURES, config);
 #endif

src/main/config/config.h

@@ -50,7 +50,7 @@ typedef enum {
     FEATURE_CHANNEL_FORWARDING = 1 << 20,
     FEATURE_TRANSPONDER = 1 << 21,
     FEATURE_AIRMODE = 1 << 22,
-    FEATURE_SUPEREXPO_RATES = 1 << 23,
+    //FEATURE_SUPEREXPO_RATES = 1 << 23,
     FEATURE_VTX = 1 << 24,
     FEATURE_RX_NRF24 = 1 << 25,
     FEATURE_SOFTSPI = 1 << 26,

src/main/debug.h

@@ -54,5 +54,6 @@ typedef enum {
     DEBUG_RC_INTERPOLATION,
     DEBUG_VELOCITY,
     DEBUG_DTERM_FILTER,
+    DEBUG_ANGLERATE,
     DEBUG_COUNT
 } debugType_e;

src/main/io/rc_controls.c

@@ -75,10 +75,6 @@ bool isAirmodeActive(void) {
     return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
 }
 
-bool isSuperExpoActive(void) {
-    return (feature(FEATURE_SUPEREXPO_RATES));
-}
-
 void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
 #ifndef BLACKBOX
     UNUSED(adjustmentFunction);

src/main/io/rc_controls.h

@@ -254,7 +254,6 @@ typedef struct adjustmentState_s {
 #define MAX_ADJUSTMENT_RANGE_COUNT 15
 
 bool isAirmodeActive(void);
-bool isSuperExpoActive(void);
 void resetAdjustmentStates(void);
 void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
 void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);

src/main/io/rc_curves.c

@@ -45,12 +45,6 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
     }
 }
 
-int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate)
-{
-    float tmpf = tmp / 100.0f;
-    return (int16_t)((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf * (float)(rate) / 2500.0f );
-}
-
 int16_t rcLookupThrottle(int32_t tmp)
 {
     const int32_t tmp2 = tmp / 100;

src/main/io/rc_curves.h

@@ -19,6 +19,5 @@
 
 void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig);
 
-int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate);
 int16_t rcLookupThrottle(int32_t tmp);
 

src/main/io/serial_cli.c

@@ -487,6 +487,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
     "RC_INTERPOLATION",
     "VELOCITY",
     "DFILTER",
+    "ANGLERATE",
 };
 
 #ifdef OSD
@@ -801,9 +802,9 @@ const clivalue_t valueTable[] = {
     { "rc_yaw_expo",                VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0,  100 } },
     { "thr_mid",                    VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0,  100 } },
     { "thr_expo",                   VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0,  100 } },
-    { "roll_rate",                  VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0,  CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
+    { "roll_srate",                 VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0,  CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
-    { "pitch_rate",                 VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0,  CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
+    { "pitch_srate",                VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0,  CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
-    { "yaw_rate",                   VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0,  CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
+    { "yaw_srate",                  VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0,  CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
     { "tpa_rate",                   VAR_UINT8  | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0,  CONTROL_RATE_CONFIG_TPA_MAX} },
     { "tpa_breakpoint",             VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN,  PWM_RANGE_MAX} },
     { "airmode_activate_throttle",  VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } },
@@ -853,7 +854,7 @@ const clivalue_t valueTable[] = {
     { "zero_cross_allowance",       VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.zeroCrossAllowanceCount, .config.minmax = {0, 50 } },
     { "iterm_throttle_gain",        VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermThrottleGain, .config.minmax = {0, 200 } },
     { "pterm_setpoint_weight",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.ptermSetpointWeight, .config.minmax = {30, 100 } },
-    { "dterm_setpoint_weight",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 200 } },
+    { "dterm_setpoint_weight",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dtermSetpointWeight, .config.minmax = {0, 255 } },
     { "yaw_rate_accel_limit",       VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawRateAccelLimit, .config.minmax = {0, 1000 } },
     { "rate_accel_limit",           VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rateAccelLimit, .config.minmax = {0, 1000 } },
 
@@ -862,7 +863,9 @@ const clivalue_t valueTable[] = {
     { "yaw_lowpass",                VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
     { "pid_process_denom",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.pid_process_denom, .config.minmax = { 1,  8 } },
 
+#ifndef SKIP_PID_FLOAT
     { "pid_controller",             VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } },
+#endif
 
     { "p_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0,  200 } },
     { "i_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0,  200 } },

src/main/io/serial_msp.c

@@ -1353,8 +1353,10 @@ static bool processInCommand(void)
         read16();
         break;
     case MSP_SET_PID_CONTROLLER:
+#ifndef SKIP_PID_FLOAT
         currentProfile->pidProfile.pidController = constrain(read8(), 0, 1);
         pidSetController(currentProfile->pidProfile.pidController);
+#endif
         break;
     case MSP_SET_PID:
         for (i = 0; i < PID_ITEM_COUNT; i++) {

src/main/main.c

@@ -106,8 +106,6 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 
-#define LOOPTIME_SUSPEND_TIME 3  // Prevent too long busy wait times
-
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
 #endif
@@ -599,8 +597,7 @@ void init(void)
         masterConfig.gyro_sync_denom = 1;
     }
 
-    setTargetPidLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime
+    setTargetPidLooptime(gyro.targetLooptime * masterConfig.pid_process_denom); // Initialize pid looptime
-
 
 #ifdef BLACKBOX
     initBlackbox();
@@ -677,7 +674,7 @@ void main_init(void)
 
     /* Setup scheduler */
     schedulerInit();
-    rescheduleTask(TASK_GYROPID, gyro.targetLooptime + LOOPTIME_SUSPEND_TIME); // Add a littlebit of extra time to reduce busy wait
+    rescheduleTask(TASK_GYROPID, gyro.targetLooptime); // Add a littlebit of extra time to reduce busy wait
     setTaskEnabled(TASK_GYROPID, true);
 
     if (sensors(SENSOR_ACC)) {

src/main/mw.c

@@ -173,22 +173,44 @@ bool isCalibrating()
     return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 }
 
+#define RC_RATE_INCREMENTAL 14.54f
+
 float calculateSetpointRate(int axis, int16_t rc) {
-    float angleRate;
+    float angleRate, rcRate, rcSuperfactor, rcCommandf;
+    uint8_t rcExpo;
 
-    if (isSuperExpoActive()) {
+    if (axis != YAW) {
-        rcInput[axis] = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f)));
+        rcExpo = currentControlRateProfile->rcExpo8;
-        float rcFactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
+        rcRate = currentControlRateProfile->rcRate8 / 100.0f;
-
-        angleRate = rcFactor * ((27 * rc) / 16.0f);
     } else {
-        angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f;
+        rcExpo = currentControlRateProfile->rcYawExpo8;
+        rcRate = currentControlRateProfile->rcYawRate8 / 100.0f;
+    }
+
+    if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f));
+    rcCommandf = rc / 500.0f;
+    rcInput[axis] = ABS(rcCommandf);
+
+    if (rcExpo) {
+        float expof = rcExpo / 100.0f;
+        rcCommandf = rcCommandf * (expof * (rcInput[axis] * rcInput[axis] * rcInput[axis]) + rcInput[axis]*(1-expof));
+    }
+
+    angleRate = 200.0f * rcRate * rcCommandf;
+
+    if (currentControlRateProfile->rates[axis]) {
+        rcSuperfactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
+        angleRate *= rcSuperfactor;
+    }
+
+    if (debugMode == DEBUG_ANGLERATE) {
+        debug[axis] = angleRate;
     }
 
     if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY)
-	    return  constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
+	    return  constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection
     else
-        return  constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec)
+        return  constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec)
 }
 
 void scaleRcCommandToFpvCamAngle(void) {
@@ -298,14 +320,14 @@ static void updateRcCommands(void)
             } else {
                 tmp = 0;
             }
-            rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcExpo8, currentControlRateProfile->rcRate8);
+            rcCommand[axis] = tmp;
         } else if (axis == YAW) {
             if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
                 tmp -= masterConfig.rcControlsConfig.yaw_deadband;
             } else {
                 tmp = 0;
             }
-            rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcYawExpo8, currentControlRateProfile->rcYawRate8) * -masterConfig.yaw_control_direction;;
+            rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
         }
         if (rcData[axis] < masterConfig.rxConfig.midrc) {
             rcCommand[axis] = -rcCommand[axis];
@@ -813,12 +835,10 @@ uint8_t setPidUpdateCountDown(void) {
 // Function for loop trigger
 void taskMainPidLoopCheck(void)
 {
-    static uint32_t previousTime;
     static bool runTaskMainSubprocesses;
     static uint8_t pidUpdateCountdown;
 
-    cycleTime = micros() - previousTime;
+    cycleTime = getTaskDeltaTime(TASK_SELF);
-    previousTime = micros();
 
     if (debugMode == DEBUG_CYCLETIME) {
         debug[0] = cycleTime;

src/main/target/CC3D/target.h

@@ -118,6 +118,7 @@
 #undef SONAR
 #undef USE_SOFTSERIAL1
 #undef LED_STRIP
+#define SKIP_PID_FLOAT
 #endif
 
 #define DEFAULT_RX_FEATURE      FEATURE_RX_PPM

src/main/target/NAZE/target.h

@@ -104,11 +104,11 @@
 #define USE_MAG_HMC5883
 #define MAG_HMC5883_ALIGN       CW180_DEG
 
-#define SONAR
+//#define SONAR
-#define SONAR_TRIGGER_PIN       PB0
+//#define SONAR_TRIGGER_PIN       PB0
-#define SONAR_ECHO_PIN          PB1
+//#define SONAR_ECHO_PIN          PB1
-#define SONAR_TRIGGER_PIN_PWM   PB8
+//#define SONAR_TRIGGER_PIN_PWM   PB8
-#define SONAR_ECHO_PIN_PWM      PB9
+//#define SONAR_ECHO_PIN_PWM      PB9
 
 //#define DISPLAY
 

src/main/target/SPARKY/target.h

@@ -72,6 +72,9 @@
 #define USE_I2C
 #define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4)
 
+#define I2C2_SCL                PA9
+#define I2C2_SDA                PA10
+
 #define USE_ADC
 #define ADC_INSTANCE            ADC2
 #define VBAT_ADC_PIN            PA4