Merge pull request #1122 from flapper-drones/migrate_platform_defaults

Migrating default PID gains and filter settings to platform defaults and Kconfig

bindings/setup.py

@@ -12,6 +12,7 @@
     "build/include/generated",
     "src/config",
     "src/drivers/interface",
+    "src/platform/interface",
 ]
 
 fw_sources = [

src/hal/interface/imu.h

@@ -60,17 +60,4 @@ bool imu6IsCalibrated(void);
 bool imuHasBarometer(void);
 bool imuHasMangnetometer(void);
 
-// Euler angles defining IMU orientation on the airframe
-#ifndef IMU_PHI
-#define IMU_PHI 0.0f
-#endif
-#ifndef IMU_THETA
-#define IMU_THETA 0.0f
-#endif
-#ifndef IMU_PSI
-#define IMU_PSI 0.0f
-#endif
-
-
-
 #endif /* IMU_H_ */

src/hal/src/sensors_bmi088_bmp388.c

@@ -54,6 +54,7 @@
 #include "estimator.h"
 
 #include "sensors_bmi088_common.h"
+#include "platform_defaults.h"
 
 #define GYRO_ADD_RAW_AND_VARIANCE_LOG_VALUES
 
@@ -89,6 +90,7 @@
 
 #define SENSORS_ACC_SCALE_SAMPLES  200
 
+
 typedef struct
 {
   Axis3f     bias;
@@ -145,13 +147,10 @@ static void applyAxis3fLpf(lpf2pData *data, Axis3f* in);
 static bool isBarometerPresent = false;
 static uint8_t baroMeasDelayMin = SENSORS_DELAY_BARO;
 
-// Precalculated values for IMU alignment
-static float sphi   = sinf(IMU_PHI * (float) M_PI / 180);
-static float cphi   = cosf(IMU_PHI * (float) M_PI / 180);
-static float stheta = sinf(IMU_THETA * (float) M_PI / 180);
-static float ctheta = cosf(IMU_THETA * (float) M_PI / 180);
-static float spsi   = sinf(IMU_PSI * (float) M_PI / 180);
-static float cpsi   = cosf(IMU_PSI * (float) M_PI / 180);
+// IMU alignment Euler angles
+static float imuPhi = IMU_PHI;
+static float imuTheta = IMU_THETA;
+static float imuPsi = IMU_PSI;
 
 static float R[3][3];
 
@@ -879,6 +878,16 @@ bool sensorsBmi088Bmp388ManufacturingTest(void)
  */
 static void sensorsAlignToAirframe(Axis3f* in, Axis3f* out)
 {
+  // IMU alignment
+  static float sphi, cphi, stheta, ctheta, spsi, cpsi;
+
+  sphi   = sinf(imuPhi * (float) M_PI / 180);
+  cphi   = cosf(imuPhi * (float) M_PI / 180);
+  stheta = sinf(imuTheta * (float) M_PI / 180);
+  ctheta = cosf(imuTheta * (float) M_PI / 180);
+  spsi   = sinf(imuPsi * (float) M_PI / 180);
+  cpsi   = cosf(imuPsi * (float) M_PI / 180);
+
   R[0][0] = ctheta * cpsi;
   R[0][1] = ctheta * spsi;
   R[0][2] = -stheta;
@@ -992,4 +1001,20 @@ PARAM_GROUP_START(imu_sensors)
  * @brief Nonzero if BMP388 barometer is present
  */
 PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, BMP388, &isBarometerPresent)
+
+/**
+ * @brief Euler angle Phi defining IMU orientation on the airframe (in degrees)
+ */
+PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, IMU Phi, &imuPhi)
+
+/**
+ * @brief Euler angle Theta defining IMU orientation on the airframe (in degrees)
+ */
+PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, IMU Theta, &imuTheta)
+
+/**
+ * @brief Euler angle Psi defining IMU orientation on the airframe (in degrees)
+ */
+PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, IMU Psi, &imuPsi)
+
 PARAM_GROUP_STOP(imu_sensors)

src/hal/src/sensors_mpu9250_lps25h.c

@@ -51,6 +51,7 @@
 #include "filter.h"
 #include "static_mem.h"
 #include "estimator.h"
+#include "platform_defaults.h"
 
 #define SENSORS_ENABLE_PRESSURE_LPS25H
 //#define GYRO_ADD_RAW_AND_VARIANCE_LOG_VALUES
@@ -142,13 +143,10 @@ static bool isMpu6500TestPassed = false;
 static bool isAK8963TestPassed = false;
 static bool isLPS25HTestPassed = false;
 
-// Precalculated values for IMU alignment
-static float sphi   = sinf(IMU_PHI * (float) M_PI / 180);
-static float cphi   = cosf(IMU_PHI * (float) M_PI / 180);
-static float stheta = sinf(IMU_THETA * (float) M_PI / 180);
-static float ctheta = cosf(IMU_THETA * (float) M_PI / 180);
-static float spsi   = sinf(IMU_PSI * (float) M_PI / 180);
-static float cpsi   = cosf(IMU_PSI * (float) M_PI / 180);
+// IMU alignment Euler angles
+static float imuPhi = IMU_PHI;
+static float imuTheta = IMU_THETA;
+static float imuPsi = IMU_PSI;
 
 static float R[3][3];
 
@@ -433,10 +431,10 @@ static void sensorsDeviceInit(void)
   }
 #endif
 
-  cosPitch = cosf(configblockGetCalibPitch() * (float) M_PI/180);
-  sinPitch = sinf(configblockGetCalibPitch() * (float) M_PI/180);
-  cosRoll = cosf(configblockGetCalibRoll() * (float) M_PI/180);
-  sinRoll = sinf(configblockGetCalibRoll() * (float) M_PI/180);
+  cosPitch = cosf(configblockGetCalibPitch() * (float) M_PI / 180);
+  sinPitch = sinf(configblockGetCalibPitch() * (float) M_PI / 180);
+  cosRoll = cosf(configblockGetCalibRoll() * (float) M_PI / 180);
+  sinRoll = sinf(configblockGetCalibRoll() * (float) M_PI / 180);
 }
 
 
@@ -870,6 +868,16 @@ void __attribute__((used)) EXTI13_Callback(void)
  */
 static void sensorsAlignToAirframe(Axis3f* in, Axis3f* out)
 {
+  // IMU alignment
+  static float sphi, cphi, stheta, ctheta, spsi, cpsi;
+
+  sphi   = sinf(imuPhi * (float) M_PI / 180);
+  cphi   = cosf(imuPhi * (float) M_PI / 180);
+  stheta = sinf(imuTheta * (float) M_PI / 180);
+  ctheta = cosf(imuTheta * (float) M_PI / 180);
+  spsi   = sinf(imuPsi * (float) M_PI / 180);
+  cpsi   = cosf(imuPsi * (float) M_PI / 180);
+
   R[0][0] = ctheta * cpsi;
   R[0][1] = ctheta * spsi;
   R[0][2] = -stheta;
@@ -987,4 +995,19 @@ PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, AK8963, &isAK8963TestPassed)
  */
 PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, LPS25H, &isLPS25HTestPassed)
 
+/**
+ * @brief Euler angle Phi defining IMU orientation on the airframe (in degrees)
+ */
+PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, IMU Phi, &imuPhi)
+
+/**
+ * @brief Euler angle Theta defining IMU orientation on the airframe (in degrees)
+ */
+PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, IMU Theta, &imuTheta)
+
+/**
+ * @brief Euler angle Psi defining IMU orientation on the airframe (in degrees)
+ */
+PARAM_ADD(PARAM_FLOAT | PARAM_PERSISTENT, IMU Psi, &imuPsi)
+
 PARAM_GROUP_STOP(imu_tests)

src/modules/interface/attitude_controller.h

@@ -73,5 +73,10 @@ void attitudeControllerResetAllPID(void);
  */
 void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw);
 
+/**
+ * Get yaw max delta
+ */
+float attitudeControllerGetYawMaxDelta(void);
+
 
 #endif /* ATTITUDE_CONTROLLER_H_ */

src/modules/interface/pid.h

@@ -30,37 +30,6 @@
 #include <stdbool.h>
 #include "filter.h"
 
-#define PID_ROLL_RATE_KP  250.0
-#define PID_ROLL_RATE_KI  500.0
-#define PID_ROLL_RATE_KD  2.5
-#define PID_ROLL_RATE_INTEGRATION_LIMIT    33.3
-
-#define PID_PITCH_RATE_KP  250.0
-#define PID_PITCH_RATE_KI  500.0
-#define PID_PITCH_RATE_KD  2.5
-#define PID_PITCH_RATE_INTEGRATION_LIMIT   33.3
-
-#define PID_YAW_RATE_KP  120.0
-#define PID_YAW_RATE_KI  16.7
-#define PID_YAW_RATE_KD  0.0
-#define PID_YAW_RATE_INTEGRATION_LIMIT     166.7
-
-#define PID_ROLL_KP  6.0
-#define PID_ROLL_KI  3.0
-#define PID_ROLL_KD  0.0
-#define PID_ROLL_INTEGRATION_LIMIT    20.0
-
-#define PID_PITCH_KP  6.0
-#define PID_PITCH_KI  3.0
-#define PID_PITCH_KD  0.0
-#define PID_PITCH_INTEGRATION_LIMIT   20.0
-
-#define PID_YAW_KP  6.0
-#define PID_YAW_KI  1.0
-#define PID_YAW_KD  0.35
-#define PID_YAW_INTEGRATION_LIMIT     360.0
-
-
 #define DEFAULT_PID_INTEGRATION_LIMIT 5000.0
 #define DEFAULT_PID_OUTPUT_LIMIT      0.0
 
@@ -75,9 +44,11 @@ typedef struct
   float kp;           //< proportional gain
   float ki;           //< integral gain
   float kd;           //< derivative gain
+  float kff;          //< feedforward gain
   float outP;         //< proportional output (debugging)
   float outI;         //< integral output (debugging)
   float outD;         //< derivative output (debugging)
+  float outFF;        //< feedforward output (debugging)
   float iLimit;       //< integral limit, absolute value. '0' means no limit.
   float outputLimit;  //< total PID output limit, absolute value. '0' means no limit.
   float dt;           //< delta-time dt
@@ -93,13 +64,14 @@ typedef struct
  * @param[in] kp        The proportional gain
  * @param[in] ki        The integral gain
  * @param[in] kd        The derivative gain
+ * @param[in] kff       The feedforward gain
  * @param[in] dt        Delta time since the last call
  * @param[in] samplingRate Frequency the update will be called
  * @param[in] cutoffFreq   Frequency to set the low pass filter cutoff at
  * @param[in] enableDFilter Enable setting for the D lowpass filter
  */
  void pidInit(PidObject* pid, const float desired, const float kp,
-              const float ki, const float kd, const float dt,
+              const float ki, const float kd, const float kff, const float dt,
               const float samplingRate, const float cutoffFreq,
               bool enableDFilter);
 
@@ -182,6 +154,14 @@ void pidSetKi(PidObject* pid, const float ki);
  */
 void pidSetKd(PidObject* pid, const float kd);
 
+/**
+ * Set a new feed-froward gain for the PID.
+ *
+ * @param[in] pid   A pointer to the pid object.
+ * @param[in] kff    The new proportional gain
+ */
+void pidSetKff(PidObject* pid, const float kff);
+
 /**
  * Set a new dt gain for the PID. Defaults to IMU_UPDATE_DT upon construction
  *
@@ -201,4 +181,3 @@ void pidSetDt(PidObject* pid, const float dt);
 void filterReset(PidObject* pid, const float samplingRate, const float cutoffFreq, bool enableDFilter);
 
 #endif /* PID_H_ */
-

src/modules/src/Kconfig

@@ -14,6 +14,18 @@ config CONTROLLER_PID
     help
         Use the PID (proportional–integral–derivative) controller as default
 
+config CONTROLLER_PID_FILTER_ALL
+    bool "Apply LPF filter to all PID components"
+    depends on CONTROLLER_PID
+    help
+        Apply a low pass filter to all PID components instead of only the D component (default)
+
+config CONTROLLER_PID_IMPROVED_BARO_Z_HOLD
+    bool "Use improved baro z hold"
+    depends on CONTROLLER_PID_FILTER_ALL
+    help
+        Uses an improved z hold based on barometric pressure
+
 config CONTROLLER_INDI
     bool "INDI controller"
     help