Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Added pitotmeterConfig parameter group

Browse source
This commit is contained in:
Martin Budden 2017-01-02 00:26:43 +00:00
parent 8ff1096863
commit 88813c777c
12 changed files with 78 additions and 57 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/config/config_master.h
View file

@ -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

3
src/main/config/parameter_group_ids.h
View file

@ -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

20
src/main/fc/config.c
View file

@ -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)

5
src/main/fc/fc_init.c
View file

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

8
src/main/fc/fc_msp.c
View file

@ -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;

29
src/main/io/serial_cli.c
View file

@ -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;
}

13
src/main/sensors/initialisation.c
View file

@ -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(
pitotmeterConfig_t *pitotConfig)
bool sensorsAutodetect(void)
{
bool eepromUpdatePending = false;
@ -64,9 +63,7 @@ bool sensorsAutodetect(
#endif
#ifdef PITOT
pitotDetect(&pitot.dev, pitotConfig->pitot_hardware);
#else
UNUSED(pitotConfig);
pitotInit();
#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) {
pitotConfig->pitot_hardware = detectedSensors[SENSOR_INDEX_PITOT];
#ifdef PITOT
if (pitotmeterConfig()->pitot_hardware == PITOT_AUTODETECT) {
pitotmeterConfig()->pitot_hardware = detectedSensors[SENSOR_INDEX_PITOT];
eepromUpdatePending = true;
}
#endif
if (eepromUpdatePending) {
writeEEPROM();

3
src/main/sensors/initialisation.h
View file

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

34
src/main/sensors/pitotmeter.c
View file

@ -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 {
float CalibratedAirspeed_tmp;
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
} else {
const float 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;

9
src/main/sensors/pitotmeter.h
View file

@ -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);
void usePitotmeterConfig(pitotmeterConfig_t *pitotmeterConfigToUse);
bool pitotInit(void);
bool isPitotCalibrationComplete(void);
void pitotSetCalibrationCycles(uint16_t calibrationCyclesRequired);
uint32_t pitotUpdate(void);

5
src/main/target/AIRHEROF3/target.h
View file

@ -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

2
src/main/target/AIRHEROF3/target.mk
View file

@ -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 \