allow imuf to choose ptn order (#602)

also remove sharpness on all fc, looking at refactoring the kalman filter anyhow. Was getting annoying compiler errors due to kalman.c and sharpness, so removing this is the way i'm going for now. Co-authored-by: nerdCopter <[email protected]>
emuflight · Jun 24, 2021 · b612d51 · b612d51
1 parent 61a96b9
commit b612d51
Show file tree

Hide file tree

Showing 9 changed files with 17 additions and 16 deletions.
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
@@ -1274,7 +1274,7 @@ static bool blackboxWriteSysinfo(void) {
         BLACKBOX_PRINT_HEADER_LINE("IMUF_lowpass_pitch", " %d",         gyroConfig()->imuf_pitch_lpf_cutoff_hz);
         BLACKBOX_PRINT_HEADER_LINE("IMUF_lowpass_yaw", " %d",           gyroConfig()->imuf_yaw_lpf_cutoff_hz);
         BLACKBOX_PRINT_HEADER_LINE("IMUF_acc_lpf_cutoff", " %d",        gyroConfig()->imuf_acc_lpf_cutoff_hz);
-        BLACKBOX_PRINT_HEADER_LINE("IMUF_sharpness", " %d",             gyroConfig()->imuf_sharpness);
+        BLACKBOX_PRINT_HEADER_LINE("IMUF_ptn_order", " %d",             gyroConfig()->imuf_ptn_order);
 #endif
         BLACKBOX_PRINT_HEADER_LINE("IMUF_roll_q", " %d",                gyroConfig()->imuf_roll_q);
         BLACKBOX_PRINT_HEADER_LINE("IMUF_pitch_q", " %d",               gyroConfig()->imuf_pitch_q);

diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
@@ -647,7 +647,7 @@ static uint16_t gyroConfig_imuf_pitch_lpf_cutoff_hz;
 static uint16_t gyroConfig_imuf_roll_lpf_cutoff_hz;
 static uint16_t gyroConfig_imuf_yaw_lpf_cutoff_hz;
 static uint16_t gyroConfig_imuf_acc_lpf_cutoff_hz;
-static uint16_t gyroConfig_imuf_sharpness;
+static uint8_t gyroConfig_imuf_ptn_order;
 #endif
 
 #if defined(USE_GYRO_IMUF9001)
@@ -660,7 +660,7 @@ static long cmsx_menuImuf_onEnter(void) {
     gyroConfig_imuf_roll_lpf_cutoff_hz = gyroConfig()->imuf_roll_lpf_cutoff_hz;
     gyroConfig_imuf_yaw_lpf_cutoff_hz = gyroConfig()->imuf_yaw_lpf_cutoff_hz;
     gyroConfig_imuf_acc_lpf_cutoff_hz = gyroConfig()->imuf_acc_lpf_cutoff_hz;
-    gyroConfig_imuf_sharpness = gyroConfig()->imuf_sharpness;
+    gyroConfig_imuf_ptn_order = gyroConfig()->imuf_ptn_order;
     return 0;
 }
 #endif
@@ -676,7 +676,7 @@ static long cmsx_menuImuf_onExit(const OSD_Entry *self) {
     gyroConfigMutable()->imuf_pitch_lpf_cutoff_hz = gyroConfig_imuf_pitch_lpf_cutoff_hz;
     gyroConfigMutable()->imuf_yaw_lpf_cutoff_hz = gyroConfig_imuf_yaw_lpf_cutoff_hz;
     gyroConfigMutable()->imuf_acc_lpf_cutoff_hz = gyroConfig_imuf_acc_lpf_cutoff_hz;
-    gyroConfigMutable()->imuf_sharpness = gyroConfig_imuf_sharpness;
+    gyroConfigMutable()->imuf_ptn_order = gyroConfig_imuf_ptn_order;
     return 0;
 }
 #endif
@@ -689,7 +689,7 @@ static OSD_Entry cmsx_menuImufEntries[] = {
     { "ROLL Q",          OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_q,              0, 16000, 100 }, 0 },
     { "PITCH Q",         OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_q,             0, 16000, 100 }, 0 },
     { "YAW Q",           OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_q,               0, 16000, 100 }, 0 },
-    { "IMUF SHARPNESS",  OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_sharpness,           0, 16000,    5 }, 0 },
+    { "IMUF PTN ORD",    OME_UINT8,  NULL, &(OSD_UINT8_t)  { &gyroConfig_imuf_ptn_order,           1, 4,       1 }, 0 },
     { "ROLL LPF",        OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_lpf_cutoff_hz,  0, 450,     1 }, 0 },
     { "PITCH LPF",       OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_lpf_cutoff_hz, 0, 450,     1 }, 0 },
     { "YAW LPF",         OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_lpf_cutoff_hz,   0, 450,     1 }, 0 },

diff --git a/src/main/common/kalman.c b/src/main/common/kalman.c
@@ -17,7 +17,7 @@
  *
  * If not, see <http://www.gnu.org/licenses/>.
  */
-
+#ifndef USE_GYRO_IMUF9001
 #include <string.h>
 #include "arm_math.h"
 
@@ -84,6 +84,7 @@ FAST_CODE float kalman_process(kalman_t* kalmanState, float input, float target)
 
     //prediction update
     kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e);
+
     //measurement update
     kalmanState->k = kalmanState->p / (kalmanState->p + 10.0f);
     kalmanState->k = pt1FilterApply(&kalmanState->kFilter, kalmanState->k);
@@ -101,3 +102,4 @@ FAST_CODE float kalman_update(float input, int axis) {
     DEBUG_SET(DEBUG_KALMAN, axis, Kgain);                               //Kalman gain
     return input;
 }
+#endif
diff --git a/src/main/drivers/accgyro/accgyro_imuf9001.c b/src/main/drivers/accgyro/accgyro_imuf9001.c
@@ -456,7 +456,7 @@ void setupImufParams(imufCommand_t * data) {
         data->param9 = ( (int16_t)boardAlignment()->yawDegrees << 16 )           | (int16_t)boardAlignment()->pitchDegrees;
     } else if (imufCurrentVersion < 225 && imufCurrentVersion > 107) {
         //Odin+ contract.
-        data->param2 = ( (uint16_t)(gyroConfig()->imuf_rate + 1) << 16)            | (uint16_t)gyroConfig()->imuf_w;
+        data->param2 = ( (uint16_t)(gyroConfig()->imuf_rate + 1) << 16)          | (uint16_t)gyroConfig()->imuf_w;
         data->param3 = ( (uint16_t)gyroConfig()->imuf_roll_q << 16)              | (uint16_t)gyroConfig()->imuf_pitch_q;
         data->param4 = ( (uint16_t)gyroConfig()->imuf_yaw_q << 16)               | (uint16_t)gyroConfig()->imuf_roll_lpf_cutoff_hz;
         data->param5 = ( (uint16_t)gyroConfig()->imuf_pitch_lpf_cutoff_hz << 16) | (uint16_t)gyroConfig()->imuf_yaw_lpf_cutoff_hz;
@@ -466,14 +466,14 @@ void setupImufParams(imufCommand_t * data) {
         data->param10 = ( (uint16_t)67.0f << 16)                                 | (int16_t)gyroConfig()->imuf_acc_lpf_cutoff_hz;
     } else {
         //QuickFlash contract!!!
-        data->param2 = ( (uint16_t)(gyroConfig()->imuf_rate + 1) << 16)            | (uint16_t)gyroConfig()->imuf_w;
+        data->param2 = ( (uint16_t)(gyroConfig()->imuf_rate + 1) << 16)          | (uint16_t)gyroConfig()->imuf_w;
         data->param3 = ( (uint16_t)gyroConfig()->imuf_roll_q << 16)              | (uint16_t)gyroConfig()->imuf_pitch_q;
         data->param4 = ( (uint16_t)gyroConfig()->imuf_yaw_q << 16)               | (uint16_t)gyroConfig()->imuf_roll_lpf_cutoff_hz;
         data->param5 = ( (uint16_t)gyroConfig()->imuf_pitch_lpf_cutoff_hz << 16) | (uint16_t)gyroConfig()->imuf_yaw_lpf_cutoff_hz;
         data->param7 = ( (uint16_t)0 << 16)                                      | (uint16_t)0;
         data->param8 = ( (int16_t)boardAlignment()->rollDegrees << 16 )          | imufGyroAlignment();
         data->param9 = ( (int16_t)boardAlignment()->yawDegrees << 16 )           | (int16_t)boardAlignment()->pitchDegrees;
-        data->param10 = ( (uint16_t)gyroConfig()->imuf_sharpness << 16)          | (int16_t)gyroConfig()->imuf_acc_lpf_cutoff_hz;
+        data->param10 = ( (uint16_t)gyroConfig()->imuf_ptn_order << 16)          | (int16_t)gyroConfig()->imuf_acc_lpf_cutoff_hz;
     }
 }
 

diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
@@ -1209,7 +1209,7 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
         sbufWriteU16(dst, gyroConfig()->imuf_pitch_q);
         sbufWriteU16(dst, gyroConfig()->imuf_yaw_q);
         sbufWriteU16(dst, gyroConfig()->imuf_w);
-        sbufWriteU16(dst, 0); // imuf_sharpness
+        sbufWriteU16(dst, 0); // was imuf_sharpness
 #ifdef  USE_GYRO_IMUF9001
         sbufWriteU16(dst, gyroConfig()->imuf_roll_lpf_cutoff_hz);
         sbufWriteU16(dst, gyroConfig()->imuf_pitch_lpf_cutoff_hz);
@@ -1792,7 +1792,7 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
         gyroConfigMutable()->imuf_pitch_q = sbufReadU16(src);
         gyroConfigMutable()->imuf_yaw_q = sbufReadU16(src);
         gyroConfigMutable()->imuf_w = sbufReadU16(src);
-        sbufReadU16(src); // imuf_sharpness
+        sbufReadU16(src); // was imuf_sharpness
 #ifdef USE_GYRO_IMUF9001
         gyroConfigMutable()->imuf_roll_lpf_cutoff_hz = sbufReadU16(src);
         gyroConfigMutable()->imuf_pitch_lpf_cutoff_hz = sbufReadU16(src);

diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
@@ -514,7 +514,7 @@ const clivalue_t valueTable[] = {
     { "imuf_roll_lpf_cutoff_hz",    VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 450   }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_roll_lpf_cutoff_hz) },
     { "imuf_yaw_lpf_cutoff_hz",     VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 450   }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_yaw_lpf_cutoff_hz) },
     { "imuf_acc_lpf_cutoff_hz",     VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 180  }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_acc_lpf_cutoff_hz) },
-    { "imuf_sharpness",             VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_sharpness) },
+    { "imuf_ptn_order",             VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 4     }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_ptn_order) },
 #else
     { "imuf_roll_q",                VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_roll_q) },
     { "imuf_pitch_q",               VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_pitch_q) },

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
@@ -262,7 +262,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
                   .imuf_pitch_lpf_cutoff_hz = IMUF_DEFAULT_LPF_HZ,
                   .imuf_yaw_lpf_cutoff_hz = IMUF_DEFAULT_LPF_HZ,
                   .imuf_acc_lpf_cutoff_hz = IMUF_DEFAULT_ACC_LPF_HZ,
-                  .imuf_sharpness = 2500,
+                  .imuf_ptn_order = 3,
                   .gyro_offset_yaw = 0,
                   .gyro_ABG_alpha = 0,
                   .gyro_ABG_boost = 275,

diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
@@ -143,7 +143,7 @@ typedef struct gyroConfig_s {
     uint16_t imuf_roll_lpf_cutoff_hz;
     uint16_t imuf_yaw_lpf_cutoff_hz;
     uint16_t imuf_acc_lpf_cutoff_hz;
-    uint16_t imuf_sharpness;
+    uint8_t imuf_ptn_order;
 #endif
     uint16_t imuf_pitch_q;
     uint16_t imuf_roll_q;

diff --git a/src/main/target/HELIOSPRING/target.mk b/src/main/target/HELIOSPRING/target.mk
@@ -1,8 +1,7 @@
-
 F405_TARGETS   += $(TARGET)
 FEATURES       += VCP ONBOARDFLASH
 
 TARGET_SRC = stm32f4xx_crc.c \
              drivers/dma_spi.c \
              drivers/accgyro/accgyro_imuf9001.c \
-             drivers/max7456.c
+             drivers/max7456.c