Added pitotmeterConfig parameter group

src/main/config/config_master.h

@@ -57,7 +57,6 @@
 #include "sensors/sensors.h"
 #include "sensors/boardalignment.h"
 #include "sensors/battery.h"
-#include "sensors/pitotmeter.h"
 
 #include "telemetry/telemetry.h"
 
@@ -67,7 +66,6 @@
 #define servoMixerConfig(x) (&masterConfig.servoMixerConfig)
 #define boardAlignment(x) (&masterConfig.boardAlignment)
 #define imuConfig(x) (&masterConfig.imuConfig)
-#define pitotmeterConfig(x) (&masterConfig.pitotmeterConfig)
 #define batteryConfig(x) (&masterConfig.batteryConfig)
 #define gpsConfig(x) (&masterConfig.gpsConfig)
 #define navConfig(x) (&masterConfig.navConfig)
@@ -138,8 +136,6 @@ typedef struct master_s {
 
     imuConfig_t imuConfig;
 
-    pitotmeterConfig_t pitotmeterConfig;
-
     batteryConfig_t batteryConfig;
 
 #ifdef GPS

src/main/config/parameter_group_ids.h

@@ -67,6 +67,9 @@
 //#define PG_DRIVER_PWM_RX_CONFIG 100
 //#define PG_DRIVER_FLASHCHIP_CONFIG 101
 
+// iNav specific parameter group ids start at 1000
+#define PG_PITOTMETER_CONFIG 1000
+
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
 //#define PG_OSD_VIDEO_CONFIG 2046

src/main/fc/config.c

@@ -41,7 +41,6 @@
 #include "sensors/acceleration.h"
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
-#include "sensors/pitotmeter.h"
 
 #include "io/beeper.h"
 #include "io/serial.h"
@@ -242,13 +241,6 @@ void validateNavConfig(navConfig_t * navConfig)
 }
 #endif
 
-void resetPitotmeterConfig(pitotmeterConfig_t *pitotmeterConfig)
-{
-    pitotmeterConfig->use_median_filtering = 1;
-    pitotmeterConfig->pitot_noise_lpf = 0.6f;
-    pitotmeterConfig->pitot_scale = 1.00f;
-}
-
 void resetMotorConfig(motorConfig_t *motorConfig)
 {
 #ifdef BRUSHED_MOTORS
@@ -472,12 +464,6 @@ void createDefaultConfig(master_t *config)
     config->boardAlignment.pitchDeciDegrees = 0;
     config->boardAlignment.yawDeciDegrees = 0;
 
-#ifdef PITOT
-    config->pitotmeterConfig.pitot_hardware = PITOT_AUTODETECT;
-#else
-    config->pitotmeterConfig.pitot_hardware = PITOT_NONE;
-#endif
-
     resetBatteryConfig(&config->batteryConfig);
 
 #ifdef TELEMETRY
@@ -562,8 +548,6 @@ void createDefaultConfig(master_t *config)
 
     config->modeActivationOperator = MODE_OPERATOR_OR; // default is to OR multiple-channel mode activation conditions
 
-    resetPitotmeterConfig(&config->pitotmeterConfig);
-
     // Radio
 #ifdef RX_CHANNELS_TAER
     parseRcChannels("TAER1234", &config->rxConfig);
@@ -770,10 +754,6 @@ static void activateConfig(void)
     navigationUseFlight3DConfig(&masterConfig.flight3DConfig);
     navigationUsemotorConfig(&masterConfig.motorConfig);
 #endif
-
-#ifdef PITOT
-    usePitotmeterConfig(&masterConfig.pitotmeterConfig);
-#endif
 }
 
 void validateAndFixConfig(void)

src/main/fc/fc_init.c

@@ -498,10 +498,7 @@ void init(void)
     }
 #endif
 
-    if (!sensorsAutodetect(
+    if (!sensorsAutodetect()) {
-            &masterConfig.pitotmeterConfig
-            )) {
-
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);
     }

src/main/fc/fc_msp.c

@@ -1155,7 +1155,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
 #else
         sbufWriteU8(dst, 0);
 #endif
+#ifdef PITOT
         sbufWriteU8(dst, pitotmeterConfig()->pitot_hardware);
+#else
+        sbufWriteU8(dst, 0);
+#endif
         sbufWriteU8(dst, 0);    // rangefinder hardware
         sbufWriteU8(dst, 0);    // optical flow hardware
         break;
@@ -1580,7 +1584,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 #else
         sbufReadU8(src);
 #endif
+#ifdef PITOT
         pitotmeterConfig()->pitot_hardware = sbufReadU8(src);
+#else
+        sbufReadU8(src);
+#endif
         sbufReadU8(src);        // rangefinder hardware
         sbufReadU8(src);        // optical flow hardware
         break;

src/main/io/serial_cli.c

@@ -528,6 +528,14 @@ static const clivalue_t valueTable[] = {
     { "baro_hardware",              VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BARO_HARDWARE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_hardware) },
     { "baro_use_median_filter",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, use_median_filtering) },
 #endif
+
+#ifdef PITOT
+    { "pitot_hardware",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_PITOT_HARDWARE }, PG_PITOTMETER_CONFIG, offsetof(pitotmeterConfig_t, pitot_hardware) },
+    { "pitot_use_median_filter",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PITOTMETER_CONFIG, offsetof(pitotmeterConfig_t, use_median_filtering) },
+    { "pitot_noise_lpf",            VAR_FLOAT  | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_PITOTMETER_CONFIG, offsetof(pitotmeterConfig_t, pitot_noise_lpf) },
+    { "pitot_scale",                VAR_FLOAT  | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_PITOTMETER_CONFIG, offsetof(pitotmeterConfig_t, pitot_scale) },
+#endif
+
 };
 
 #else
@@ -764,13 +772,6 @@ const clivalue_t valueTable[] = {
     { "rx_min_usec",                VAR_UINT16 | MASTER_VALUE,  &rxConfig()->rx_min_usec, .config.minmax = { PWM_PULSE_MIN,  PWM_PULSE_MAX } },
     { "rx_max_usec",                VAR_UINT16 | MASTER_VALUE,  &rxConfig()->rx_max_usec, .config.minmax = { PWM_PULSE_MIN,  PWM_PULSE_MAX } },
 
-#ifdef PITOT
-    { "pitot_hardware",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &pitotmeterConfig()->pitot_hardware, .config.lookup = { TABLE_PITOT_HARDWARE } },
-    { "pitot_use_median_filter",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &pitotmeterConfig()->use_median_filtering, .config.lookup = { TABLE_OFF_ON } },
-    { "pitot_noise_lpf",            VAR_FLOAT  | MASTER_VALUE, &pitotmeterConfig()->pitot_noise_lpf, .config.minmax = { 0, 1 } },
-    { "pitot_scale",                VAR_FLOAT  | MASTER_VALUE, &pitotmeterConfig()->pitot_scale, .config.minmax = { 0, 100 } },
-#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 }, },
     { "d_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PITCH], .config.minmax = { 0,  200 }, },
@@ -1068,6 +1069,9 @@ static compassConfig_t compassConfigCopy;
 #ifdef BARO
 static barometerConfig_t barometerConfigCopy;
 #endif
+#ifdef PITOT
+static pitotmeterConfig_t pitotmeterConfigCopy;
+#endif
 
 static void backupConfigs(void)
 {
@@ -1080,8 +1084,11 @@ static void backupConfigs(void)
 #ifdef BARO
     barometerConfigCopy = *barometerConfig();
 #endif
-
+#ifdef PITOT
+    pitotmeterConfigCopy = *pitotmeterConfig();
+#endif
 }
+
 static void restoreConfigs(void)
 {
     *gyroConfig() = gyroConfigCopy;
@@ -1092,6 +1099,9 @@ static void restoreConfigs(void)
 #ifdef BARO
     *barometerConfig() = barometerConfigCopy;
 #endif
+#ifdef PITOT
+    *pitotmeterConfig() = pitotmeterConfigCopy;
+#endif
 }
 
 #endif
@@ -1130,6 +1140,9 @@ static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *
 #endif
 #ifdef BARO
         dumpPgValues(MASTER_VALUE, dumpMask, PG_BAROMETER_CONFIG, &barometerConfigCopy, barometerConfig());
+#endif
+#ifdef PITOT
+        dumpPgValues(MASTER_VALUE, dumpMask, PG_PITOT_CONFIG, &pitotmeterConfigCopy, pitotmeterConfig());
 #endif
         return;
     }

src/main/sensors/initialisation.c

@@ -42,8 +42,7 @@ uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO
 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE };
 
 
-bool sensorsAutodetect(
+bool sensorsAutodetect(void)
-                pitotmeterConfig_t *pitotConfig)
 {
     bool eepromUpdatePending = false;
 
@@ -64,9 +63,7 @@ bool sensorsAutodetect(
 #endif
 
 #ifdef PITOT
-    pitotDetect(&pitot.dev, pitotConfig->pitot_hardware);
+    pitotInit();
-#else
-    UNUSED(pitotConfig);
 #endif
 
     // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
@@ -97,10 +94,12 @@ bool sensorsAutodetect(
         eepromUpdatePending = true;
     }
 
-    if (pitotConfig->pitot_hardware == PITOT_AUTODETECT) {
+#ifdef PITOT
-        pitotConfig->pitot_hardware = detectedSensors[SENSOR_INDEX_PITOT];
+    if (pitotmeterConfig()->pitot_hardware == PITOT_AUTODETECT) {
+        pitotmeterConfig()->pitot_hardware = detectedSensors[SENSOR_INDEX_PITOT];
         eepromUpdatePending = true;
     }
+#endif
 
     if (eepromUpdatePending) {
         writeEEPROM();

src/main/sensors/initialisation.h

@@ -17,5 +17,4 @@
 
 #pragma once
 
-bool sensorsAutodetect(
+bool sensorsAutodetect(void);
-                pitotmeterConfig_t *pitotConfig);

src/main/sensors/pitotmeter.c

@@ -25,6 +25,9 @@
 
 #include "common/maths.h"
 
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
 #include "drivers/logging.h"
 #include "drivers/pitotmeter.h"
 #include "drivers/pitotmeter_ms4525.h"
@@ -45,7 +48,19 @@ static float pitotPressure = 0;
 static float pitotTemperature = 0;
 static float CalibratedAirspeed = 0;
 
-pitotmeterConfig_t *pitotmeterConfig;
+PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 0);
+
+#ifdef PITOT
+#define PITOT_HARDWARE_DEFAULT    PITOT_AUTODETECT
+#else
+#define PITOT_HARDWARE_DEFAULT    PITOT_NONE
+#endif
+PG_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig,
+    .pitot_hardware = PITOT_HARDWARE_DEFAULT,
+    .use_median_filtering = 1,
+    .pitot_noise_lpf = 0.6f,
+    .pitot_scale = 1.00f
+);
 
 bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
 {
@@ -95,9 +110,12 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
     return true;
 }
 
-void usePitotmeterConfig(pitotmeterConfig_t *pitotmeterConfigToUse)
+bool pitotInit(void)
 {
-    pitotmeterConfig = pitotmeterConfigToUse;
+    if (!pitotDetect(&pitot.dev, pitotmeterConfig()->pitot_hardware)) {
+        return false;
+    }
+    return true;
 }
 
 bool isPitotCalibrationComplete(void)
@@ -161,7 +179,7 @@ uint32_t pitotUpdate(void)
         case PITOTMETER_NEEDS_CALCULATION:
             pitot.dev.get();
             pitot.dev.calculate(&pitotPressure, &pitotTemperature);
-            if (pitotmeterConfig->use_median_filtering) {
+            if (pitotmeterConfig()->use_median_filtering) {
                 pitotPressure = applyPitotmeterMedianFilter(pitotPressure);
             }
             state = PITOTMETER_NEEDS_SAMPLES;
@@ -187,11 +205,9 @@ int32_t pitotCalculateAirSpeed(void)
     if (!isPitotCalibrationComplete()) {
         performPitotCalibrationCycle();
         pitot.airSpeed = 0;
-    }
+    } else {
-    else {
+        const float CalibratedAirspeed_tmp = pitotmeterConfig()->pitot_scale * CASFACTOR * sqrtf(powf(fabsf(pitotPressure - pitotPressureZero) / P0 + 1.0f, CCEXPONENT) - 1.0f);
-        float CalibratedAirspeed_tmp;
+        CalibratedAirspeed = CalibratedAirspeed * pitotmeterConfig()->pitot_noise_lpf + CalibratedAirspeed_tmp * (1.0f - pitotmeterConfig()->pitot_noise_lpf); // additional LPF to reduce baro noise
-        CalibratedAirspeed_tmp = pitotmeterConfig->pitot_scale * CASFACTOR * sqrtf(powf(fabsf(pitotPressure - pitotPressureZero) / P0 + 1.0f, CCEXPONENT) - 1.0f);
-        CalibratedAirspeed = CalibratedAirspeed * pitotmeterConfig->pitot_noise_lpf + CalibratedAirspeed_tmp * (1.0f - pitotmeterConfig->pitot_noise_lpf); // additional LPF to reduce baro noise
         float TrueAirspeed = CalibratedAirspeed * TASFACTOR * sqrtf(pitotTemperature);
 
         pitot.airSpeed = TrueAirspeed*100;

src/main/sensors/pitotmeter.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "config/parameter_group.h"
+
 #include "drivers/pitotmeter.h"
 
 typedef enum {
@@ -30,12 +32,14 @@ typedef enum {
 #define PITOT_SAMPLE_COUNT_MAX   48
 
 typedef struct pitotmeterConfig_s {
-    uint8_t use_median_filtering;           // Use 3-point median filtering
     uint8_t pitot_hardware;                 // Pitotmeter hardware to use
+    uint8_t use_median_filtering;           // Use 3-point median filtering
     float pitot_noise_lpf;                  // additional LPF to reduce pitot noise
     float pitot_scale;                      // scale value
 } pitotmeterConfig_t;
 
+PG_DECLARE(pitotmeterConfig_t, pitotmeterConfig);
+
 typedef struct pito_s {
     pitotDev_t dev;
     int32_t airSpeed;
@@ -43,8 +47,7 @@ typedef struct pito_s {
 
 extern pitot_t pitot;
 
-bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse);
+bool pitotInit(void);
-void usePitotmeterConfig(pitotmeterConfig_t *pitotmeterConfigToUse);
 bool isPitotCalibrationComplete(void);
 void pitotSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 uint32_t pitotUpdate(void);

src/main/target/AIRHEROF3/target.h

@@ -51,6 +51,11 @@
 #define BARO
 #define USE_BARO_SPI_BMP280
 
+#define MAG
+#define USE_MAG_AK8963
+#define USE_MAG_AK8975
+#define USE_MAG_HMC5883
+
 #define USE_UART1
 #define USE_UART2
 #define USE_UART3

src/main/target/AIRHEROF3/target.mk

@@ -7,6 +7,8 @@ TARGET_SRC = \
             drivers/accgyro_spi_mpu6500.c \
             drivers/barometer_bmp280.c \
             drivers/barometer_spi_bmp280.c \
+            drivers/compass_ak8963.c \
+            drivers/compass_ak8975.c \
             drivers/compass_hmc5883l.c \
             drivers/compass_mag3110.c \
             drivers/light_ws2811strip.c \