Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

Remove sensor_barometer.c's dependency on mw.h/board.h.

Browse source 
This also cleans the code - No magic numbers, clearly defines states,
config variables grouped together, avoiding some global variable use.
This commit is contained in:
Dominic Clifton 2014-04-22 23:46:25 +01:00
parent c08d0ac1b3
commit e2550ee5e1
8 changed files with 120 additions and 62 deletions
Download patch file Download diff file Expand all files Collapse all files

23
src/config.c
View file

@ -14,6 +14,7 @@
#include "statusindicator.h"
#include "sensors_acceleration.h"
#include "sensors_barometer.h"
#include "telemetry_common.h"
#include "gps_common.h"
@ -90,8 +91,11 @@ void activateConfig(void)
gpsUseProfile(&currentProfile.gpsProfile);
gpsUsePIDs(&currentProfile.pidProfile);
useFailsafeConfig(&currentProfile.failsafeConfig);
setAccelerationTrims(&masterConfig.accZero);
#ifdef BARO
useBarometerConfig(&currentProfile.barometerConfig);
#endif
}
void readEEPROM(void)
@ -223,6 +227,14 @@ void resetGpsProfile(gpsProfile_t *gpsProfile)
gpsProfile->ap_mode = 40;
}
void resetBarometerConfig(barometerConfig_t *barometerConfig)
{
barometerConfig->baro_sample_count = 21;
barometerConfig->baro_noise_lpf = 0.6f;
barometerConfig->baro_cf_vel = 0.985f;
barometerConfig->baro_cf_alt = 0.965f;
}
// Default settings
static void resetConf(void)
{
@ -306,15 +318,16 @@ static void resetConf(void)
currentProfile.controlRateConfig.thrExpo8 = 0;
// for (i = 0; i < CHECKBOXITEMS; i++)
// cfg.activate[i] = 0;
resetRollAndPitchTrims(&currentProfile.accelerometerTrims);
currentProfile.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
currentProfile.acc_lpf_factor = 4;
currentProfile.accz_deadband = 40;
currentProfile.accxy_deadband = 40;
currentProfile.baro_tab_size = 21;
currentProfile.baro_noise_lpf = 0.6f;
currentProfile.baro_cf_vel = 0.985f;
currentProfile.baro_cf_alt = 0.965f;
resetBarometerConfig(&currentProfile.barometerConfig);
currentProfile.acc_unarmedcal = 1;
// Radio

8
src/config_profile.h
View file

@ -16,10 +16,10 @@ typedef struct profile_s {
uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
uint8_t accz_deadband; // set the acc deadband for z-Axis, this ignores small accelerations
uint8_t accxy_deadband; // set the acc deadband for xy-Axis
uint8_t baro_tab_size; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise
float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
float baro_cf_alt; // apply CF to use ACC for height estimation
barometerConfig_t barometerConfig;
uint8_t acc_unarmedcal; // turn automatic acc compensation on/off
uint16_t activate[CHECKBOX_ITEM_COUNT]; // activate switches

8
src/flight_imu.c
View file

@ -54,11 +54,7 @@ int32_t vario = 0; // variometer in cm/s
int16_t throttleAngleCorrection = 0; // correction of throttle in lateral wind,
float throttleAngleScale;
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
uint32_t baroPressureSum = 0;
int32_t BaroPID = 0;
int32_t BaroAlt = 0;
float magneticDeclination = 0.0f; // calculated at startup from config
@ -400,7 +396,7 @@ int getEstimatedAltitude(void)
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * currentProfile.baro_cf_alt + (float)BaroAlt * (1.0f - currentProfile.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
accAlt = accAlt * currentProfile.barometerConfig.baro_cf_alt + (float)BaroAlt * (1.0f - currentProfile.barometerConfig.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
EstAlt = accAlt;
vel += vel_acc;
@ -420,7 +416,7 @@ int getEstimatedAltitude(void)
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * currentProfile.baro_cf_vel + baroVel * (1 - currentProfile.baro_cf_vel);
vel = vel * currentProfile.barometerConfig.baro_cf_vel + baroVel * (1 - currentProfile.barometerConfig.baro_cf_vel);
vel_tmp = lrintf(vel);
// set vario

2
src/mw.c
View file

@ -489,7 +489,7 @@ void loop(void)
case 1:
taskOrder++;
#ifdef BARO
if (sensors(SENSOR_BARO) && baroUpdate())
if (sensors(SENSOR_BARO) && baroUpdate(currentTime) != BAROMETER_ACTION_NOT_READY)
break;
#endif
case 2:

2
src/mw.h
View file

@ -7,7 +7,6 @@
/* for VBAT monitoring frequency */
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
#define BARO_TAB_SIZE_MAX 48
#define VERSION 230
@ -20,6 +19,7 @@ enum {
ALIGN_MAG = 2
};
#include "sensors_barometer.h"
#include "serial_common.h"
#include "rc_controls.h"
#include "rx_common.h"

92
src/sensors_barometer.c
View file

@ -1,61 +1,97 @@
#include "board.h"
#include "mw.h"
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#include "drivers/barometer_common.h"
#include "config.h"
#include "sensors_barometer.h"
baro_t baro; // barometer access functions
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
uint32_t baroPressureSum = 0;
int32_t BaroAlt = 0;
#ifdef BARO
static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;
barometerConfig_t *barometerConfig;
void useBarometerConfig(barometerConfig_t *barometerConfigToUse)
{
barometerConfig = barometerConfigToUse;
}
bool isBaroCalibrationComplete(void)
{
return calibratingB == 0;
}
void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
calibratingB = calibrationCyclesRequired;
}
void baroCommon(void)
static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading)
{
static int32_t baroHistTab[BARO_TAB_SIZE_MAX];
static int baroHistIdx;
int indexplus1;
static int32_t barometerSamples[BARO_SAMPLE_COUNT_MAX];
static int currentSampleIndex = 0;
int nextSampleIndex;
indexplus1 = (baroHistIdx + 1);
if (indexplus1 == currentProfile.baro_tab_size)
indexplus1 = 0;
baroHistTab[baroHistIdx] = baroPressure;
baroPressureSum += baroHistTab[baroHistIdx];
baroPressureSum -= baroHistTab[indexplus1];
baroHistIdx = indexplus1;
// store current pressure in barometerSamples
nextSampleIndex = (currentSampleIndex + 1);
if (nextSampleIndex == baroSampleCount) {
nextSampleIndex = 0;
}
barometerSamples[currentSampleIndex] = newPressureReading;
// recalculate pressure total
pressureTotal += barometerSamples[currentSampleIndex];
pressureTotal -= barometerSamples[nextSampleIndex];
currentSampleIndex = nextSampleIndex;
return pressureTotal;
}
typedef enum {
BAROMETER_NEEDS_SAMPLES = 0,
BAROMETER_NEEDS_CALCULATION
} barometerState_e;
int baroUpdate(void)
barometerAction_e baroUpdate(uint32_t currentTime)
{
static uint32_t baroDeadline = 0;
static int state = 0;
static barometerState_e state = BAROMETER_NEEDS_SAMPLES;
if ((int32_t)(currentTime - baroDeadline) < 0)
return 0;
return BAROMETER_ACTION_NOT_READY;
baroDeadline = currentTime;
if (state) {
switch (state) {
case BAROMETER_NEEDS_CALCULATION:
baro.get_up();
baro.start_ut();
baroDeadline += baro.ut_delay;
baro.calculate(&baroPressure, &baroTemperature);
state = 0;
return 2;
} else {
state = BAROMETER_NEEDS_SAMPLES;
return BAROMETER_ACTION_PERFORMED_CALCULATION;
case BAROMETER_NEEDS_SAMPLES:
default:
baro.get_ut();
baro.start_up();
baroCommon();
state = 1;
baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
state = BAROMETER_NEEDS_CALCULATION;
baroDeadline += baro.up_delay;
return 1;
return BAROMETER_ACTION_OBTAINED_SAMPLES;
}
}
@ -65,9 +101,9 @@ int32_t baroCalculateAltitude(void)
// calculates height from ground via baro readings
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (currentProfile.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (barometerConfig->baro_sample_count - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt_tmp -= baroGroundAltitude;
BaroAlt = lrintf((float)BaroAlt * currentProfile.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - currentProfile.baro_noise_lpf)); // additional LPF to reduce baro noise
BaroAlt = lrintf((float)BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise
return BaroAlt;
}
@ -75,14 +111,10 @@ int32_t baroCalculateAltitude(void)
void performBaroCalibrationCycle(void)
{
baroGroundPressure -= baroGroundPressure / 8;
baroGroundPressure += baroPressureSum / (currentProfile.baro_tab_size - 1);
baroGroundPressure += baroPressureSum / (barometerConfig->baro_sample_count - 1);
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
calibratingB--;
}
bool isBaroCalibrationComplete(void)
{
return calibratingB == 0;
}
#endif /* BARO */

25
src/sensors_barometer.h
View file

@ -1,10 +1,27 @@
#pragma once
#define BARO_SAMPLE_COUNT_MAX 48
typedef struct barometerConfig_s {
uint8_t baro_sample_count; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise
float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
float baro_cf_alt; // apply CF to use ACC for height estimation
} barometerConfig_t;
typedef enum {
BAROMETER_ACTION_NOT_READY = 0,
BAROMETER_ACTION_OBTAINED_SAMPLES,
BAROMETER_ACTION_PERFORMED_CALCULATION
} barometerAction_e;
extern int32_t BaroAlt;
#ifdef BARO
void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void baroCommon(void);
int baroUpdate(void);
int32_t baroCalculateAltitude(void);
void useBarometerConfig(barometerConfig_t *barometerConfigToUse);
bool isBaroCalibrationComplete(void);
void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
barometerAction_e baroUpdate(uint32_t currentTime);
int32_t baroCalculateAltitude(void);
void performBaroCalibrationCycle(void);
#endif

8
src/serial_cli.c
View file

@ -196,10 +196,10 @@ const clivalue_t valueTable[] = {
{ "acc_unarmedcal", VAR_UINT8, &currentProfile.acc_unarmedcal, 0, 1 },
{ "acc_trim_pitch", VAR_INT16, &currentProfile.accelerometerTrims.trims.pitch, -300, 300 },
{ "acc_trim_roll", VAR_INT16, &currentProfile.accelerometerTrims.trims.roll, -300, 300 },
{ "baro_tab_size", VAR_UINT8, &currentProfile.baro_tab_size, 0, BARO_TAB_SIZE_MAX },
{ "baro_noise_lpf", VAR_FLOAT, &currentProfile.baro_noise_lpf, 0, 1 },
{ "baro_cf_vel", VAR_FLOAT, &currentProfile.baro_cf_vel, 0, 1 },
{ "baro_cf_alt", VAR_FLOAT, &currentProfile.baro_cf_alt, 0, 1 },
{ "baro_tab_size", VAR_UINT8, &currentProfile.barometerConfig.baro_sample_count, 0, BARO_SAMPLE_COUNT_MAX },
{ "baro_noise_lpf", VAR_FLOAT, &currentProfile.barometerConfig.baro_noise_lpf, 0, 1 },
{ "baro_cf_vel", VAR_FLOAT, &currentProfile.barometerConfig.baro_cf_vel, 0, 1 },
{ "baro_cf_alt", VAR_FLOAT, &currentProfile.barometerConfig.baro_cf_alt, 0, 1 },
{ "mag_declination", VAR_INT16, &currentProfile.mag_declination, -18000, 18000 },
{ "gps_pos_p", VAR_UINT8, &currentProfile.pidProfile.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8, &currentProfile.pidProfile.I8[PIDPOS], 0, 200 },