Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Lock altitude task to 100Hz

Browse source 
+ fix baro calibration
+ baro refactoring
This commit is contained in:
KarateBrot 2022-10-20 16:01:24 +02:00
parent c5468981e6
commit f17a2af8c3
10 changed files with 54 additions and 86 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/blackbox/blackbox.c
View file

@ -217,7 +217,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"magADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
#endif
#ifdef USE_BARO
{"BaroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(BARO)},
{"baroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(BARO)},
#endif
#ifdef USE_RANGEFINDER
{"surfaceRaw", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(RANGEFINDER)},
@ -319,7 +319,7 @@ typedef struct blackboxMainState_s {
int32_t amperageLatest;
#ifdef USE_BARO
int32_t BaroAlt;
int32_t baroAlt;
#endif
#ifdef USE_MAG
int16_t magADC[XYZ_AXIS_COUNT];
@ -614,7 +614,7 @@ static void writeIntraframe(void)
#ifdef USE_BARO
if (testBlackboxCondition(CONDITION(BARO))) {
blackboxWriteSignedVB(blackboxCurrent->BaroAlt);
blackboxWriteSignedVB(blackboxCurrent->baroAlt);
}
#endif
@ -762,7 +762,7 @@ static void writeInterframe(void)
#ifdef USE_BARO
if (testBlackboxCondition(CONDITION(BARO))) {
deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt;
deltas[optionalFieldCount++] = blackboxCurrent->baroAlt - blackboxLast->baroAlt;
}
#endif
@ -1101,7 +1101,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->amperageLatest = getAmperageLatest();
#ifdef USE_BARO
blackboxCurrent->BaroAlt = baro.BaroAlt;
blackboxCurrent->baroAlt = baro.altitude;
#endif
#ifdef USE_RANGEFINDER

1
src/main/fc/init.c
View file

@ -98,6 +98,7 @@
#include "flight/gps_rescue.h"
#include "flight/pid.h"
#include "flight/pid_init.h"
#include "flight/position.h"
#include "flight/servos.h"
#include "io/asyncfatfs/asyncfatfs.h"

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

@ -294,23 +294,7 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
#endif
#if defined(USE_BARO) || defined(USE_GPS)
static bool taskAltitudeCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
{
UNUSED(currentTimeUs);
#if defined(USE_BARO)
if (isBaroSampleReady()) {
return true;
}
#endif
if (currentDeltaTimeUs > getTask(TASK_ALTITUDE)->attribute->desiredPeriodUs) {
return true;
}
return false;
}
static void taskCalculateAltitude(timeUs_t currentTimeUs)
static void taskCalculateAltitude(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
calculateEstimatedAltitude();
@ -395,7 +379,7 @@ task_attribute_t task_attributes[TASK_COUNT] = {
#endif
#if defined(USE_BARO) || defined(USE_GPS)
[TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, taskAltitudeCheck, taskCalculateAltitude, TASK_PERIOD_HZ(TASK_ALTITUDE_RATE_HZ), TASK_PRIORITY_LOW),
[TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, NULL, taskCalculateAltitude, TASK_PERIOD_HZ(TASK_ALTITUDE_RATE_HZ), TASK_PRIORITY_LOW),
#endif
#ifdef USE_DASHBOARD

3
src/main/flight/position.h
View file

@ -22,7 +22,8 @@
#include "common/time.h"
#define TASK_ALTITUDE_RATE_HZ 120
#define TASK_ALTITUDE_RATE_HZ 100
typedef struct positionConfig_s {
uint8_t altitude_source;
uint8_t altitude_prefer_baro;

78
src/main/sensors/barometer.c
View file

@ -137,19 +137,13 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
barometerConfig->baro_xclr_tag = IO_TAG(BARO_XCLR_PIN);
}
#define NUM_CALIBRATION_CYCLES 100 // 10 seconds init_delay + 100 * 25 ms = 12.5 seconds before valid baro altitude
#define NUM_GROUND_LEVEL_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
static uint16_t calibrationCycles = 0; // baro calibration = get new ground pressure value
static bool baroCalibrated = false;
static int32_t baroPressure = 0;
static int32_t baroTemperature = 0;
static float baroGroundAltitude = 0.0f;
static float baroAltitudeRaw = 0.0f;
#define CALIBRATING_BARO_CYCLES 100 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
#define SET_GROUND_LEVEL_BARO_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
static bool baroCalibrated = false;
static bool baroReady = false;
static bool baroSampleReady = false;
void baroPreInit(void)
{
@ -296,8 +290,7 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
void baroInit(void)
{
baroDetect(&baro.dev, barometerConfig()->baro_hardware);
baroReady = true;
baroReady = baroDetect(&baro.dev, barometerConfig()->baro_hardware);
}
bool baroIsCalibrated(void)
@ -312,13 +305,15 @@ static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
void baroStartCalibration(void)
{
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
baroSetCalibrationCycles(NUM_CALIBRATION_CYCLES);
baroGroundAltitude = 0;
baroCalibrated = false;
}
void baroSetGroundLevel(void)
{
baroSetCalibrationCycles(SET_GROUND_LEVEL_BARO_CYCLES);
baroSetCalibrationCycles(NUM_GROUND_LEVEL_CYCLES);
baroGroundAltitude = 0;
baroCalibrated = false;
}
@ -338,20 +333,13 @@ bool isBaroReady(void)
return baroReady;
}
bool isBaroSampleReady(void)
{
if (baroSampleReady) {
baroSampleReady = false;
return true;
}
return false;
}
static float pressureToAltitude(const float pressure)
{
return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
}
static void performBaroCalibrationCycle(const float altitude);
uint32_t baroUpdate(timeUs_t currentTimeUs)
{
static timeUs_t baroStateDurationUs[BARO_STATE_COUNT];
@ -415,24 +403,23 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
schedulerIgnoreTaskExecTime();
break;
}
baro.dev.calculate(&baroPressure, &baroTemperature);
baro.baroPressure = baroPressure;
baro.baroTemperature = baroTemperature;
baroAltitudeRaw = pressureToAltitude(baroPressure);
performBaroCalibrationCycle();
// update baro data
baro.dev.calculate(&baro.pressure, &baro.temperature);
baro.altitude = pressureToAltitude(baro.pressure);
if (baroIsCalibrated()) {
baro.BaroAlt = baroAltitudeRaw - baroGroundAltitude;
// zero baro altitude
baro.altitude -= baroGroundAltitude;
} else {
baro.BaroAlt = 0.0f;
// establish stable baroGroundAltitude value to zero baro altitude with
performBaroCalibrationCycle(baro.altitude);
baro.altitude = 0.0f;
}
DEBUG_SET(DEBUG_BARO, 1, baro.baroTemperature);
DEBUG_SET(DEBUG_BARO, 2, baro.BaroAlt);
baroSampleReady = true;
DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f)); // hPa
DEBUG_SET(DEBUG_BARO, 2, baro.temperature); // c°C
DEBUG_SET(DEBUG_BARO, 3, lrintf(baro.altitude)); // cm
if (baro.dev.combined_read) {
state = BARO_STATE_PRESSURE_START;
@ -458,23 +445,22 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
return sleepTime;
}
// baroAltitude samples baro.BaroAlt the ALTITUDE task rate
float getBaroAltitude(void)
{
return baro.BaroAlt;
return baro.altitude;
}
void performBaroCalibrationCycle(void)
static void performBaroCalibrationCycle(const float altitude)
{
static uint16_t cycleCount = 0;
if (!baroCalibrated) {
cycleCount++;
baroGroundAltitude += baroAltitudeRaw;
if (cycleCount == calibrationCycles) {
baroGroundAltitude = baroGroundAltitude / calibrationCycles; // simple average
baroCalibrated = true;
cycleCount = 0;
}
baroGroundAltitude += altitude;
cycleCount++;
if (cycleCount >= calibrationCycles) {
baroGroundAltitude /= cycleCount; // simple average
baroCalibrated = true;
cycleCount = 0;
}
}

13
src/main/sensors/barometer.h
View file

@ -22,7 +22,6 @@
#include "pg/pg.h"
#include "drivers/barometer/barometer.h"
#include "flight/position.h"
typedef enum {
BARO_DEFAULT = 0,
@ -49,15 +48,13 @@ typedef struct barometerConfig_s {
PG_DECLARE(barometerConfig_t, barometerConfig);
// #define TASK_BARO_DENOM 3
// #define TASK_BARO_RATE_HZ (TASK_ALTITUDE_RATE_HZ / TASK_BARO_DENOM)
#define TASK_BARO_RATE_HZ 40
#define TASK_BARO_RATE_HZ 40 // Will be overwritten by the baro device driver
typedef struct baro_s {
baroDev_t dev;
float BaroAlt;
int32_t baroTemperature; // Use temperature for telemetry
int32_t baroPressure; // Use pressure for telemetry
float altitude;
int32_t temperature; // Use temperature for telemetry
int32_t pressure; // Use pressure for telemetry
} baro_t;
extern baro_t baro;
@ -69,6 +66,4 @@ void baroStartCalibration(void);
void baroSetGroundLevel(void);
uint32_t baroUpdate(timeUs_t currentTimeUs);
bool isBaroReady(void);
bool isBaroSampleReady(void);
float getBaroAltitude(void);
void performBaroCalibrationCycle(void);

4
src/main/telemetry/frsky_hub.c
View file

@ -219,9 +219,9 @@ static void sendTemperature1(void)
data = escData->dataAge < ESC_DATA_INVALID ? escData->temperature : 0;
}
#elif defined(USE_BARO)
data = (baro.baroTemperature + 50)/ 100; // Airmamaf
data = lrintf(baro.temperature / 100.0f); // Airmamaf
#else
data = gyroGetTemperature() / 10;
data = lrintf(gyroGetTemperature() / 10.0f);
#endif
frSkyHubWriteFrame(ID_TEMPRATURE1, data);
}

7
src/main/telemetry/ibus_shared.c
View file

@ -32,6 +32,7 @@
#include <stdint.h>
#include <stdlib.h>
#include <limits.h>
#include <math.h>
#include "platform.h"
#include "telemetry/telemetry.h"
@ -219,7 +220,7 @@ static uint16_t getTemperature(void)
uint16_t temperature = gyroGetTemperature() * 10;
#if defined(USE_BARO)
if (sensors(SENSOR_BARO)) {
temperature = (uint16_t) ((baro.baroTemperature + 50) / 10);
temperature = lrintf(baro.temperature / 10.0f);
}
#endif
return temperature + IBUS_TEMPERATURE_OFFSET;
@ -425,10 +426,10 @@ static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length)
#ifdef USE_BARO
case IBUS_SENSOR_TYPE_ALT:
case IBUS_SENSOR_TYPE_ALT_MAX:
value.int32 = baro.BaroAlt;
value.int32 = baro.altitude;
break;
case IBUS_SENSOR_TYPE_PRES:
value.uint32 = baro.baroPressure | (((uint32_t)getTemperature()) << 19);
value.uint32 = baro.pressure | (((uint32_t)getTemperature()) << 19);
break;
#endif
#endif //defined(TELEMETRY_IBUS_EXTENDED)

2
src/test/unit/rx_ibus_unittest.cc
View file

@ -42,7 +42,7 @@ extern "C" {
uint8_t batteryCellCount = 3;
float rcCommand[4] = {0, 0, 0, 0};
int16_t telemTemperature1 = 0;
baro_t baro = { .baroTemperature = 50 };
baro_t baro = { .temperature = 50 };
telemetryConfig_t telemetryConfig_System;
timeUs_t rxFrameTimeUs(void) { return 0; }
}

2
src/test/unit/rx_sumd_unittest.cc
View file

@ -41,7 +41,7 @@ extern "C" {
uint8_t batteryCellCount = 3;
float rcCommand[4] = {0, 0, 0, 0};
int16_t telemTemperature1 = 0;
baro_t baro = { .baroTemperature = 50 };
baro_t baro = { .temperature = 50 };
telemetryConfig_t telemetryConfig_System;
timeUs_t rxFrameTimeUs(void) { return 0; }
}