rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2760 from mikeller/reworked_altitude_estimation 

Reworked altitude estimation. Made it work when ACC is disabled.
This commit is contained in:
Michael Keller 2017-04-10 13:00:16 +12:00 committed by GitHub
parent 572c5762d5 396388bf84
commit ce8c5fbd79
21 changed files with 127 additions and 136 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -695,7 +695,7 @@ COMMON_SRC = \
fc/rc_controls.c \
fc/runtime_config.c \
fc/cli.c \
flight/altitudehold.c \
flight/altitude.c \
flight/failsafe.c \
flight/imu.c \
flight/mixer.c \

1
src/main/build/debug.h
View File

@ -64,5 +64,6 @@ typedef enum {
DEBUG_STACK,
DEBUG_ESC_SENSOR_RPM,
DEBUG_ESC_SENSOR_TMP,
DEBUG_ALTITUDE,
DEBUG_COUNT
} debugType_e;

5
src/main/fc/cli.c
View File

@ -83,7 +83,7 @@ extern uint8_t __config_end;
#include "fc/runtime_config.h"
#include "fc/fc_msp.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
@ -326,7 +326,8 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"SCHEDULER",
"STACK",
"ESC_SENSOR_RPM",
"ESC_SENSOR_TMP"
"ESC_SENSOR_TMP",
"ALTITUDE"
};
#ifdef OSD

2
src/main/fc/config.c
View File

@ -67,7 +67,7 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"

2
src/main/fc/fc_core.c
View File

@ -72,7 +72,7 @@
#include "telemetry/telemetry.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"

6
src/main/fc/fc_msp.c
View File

@ -62,7 +62,7 @@
#include "fc/rc_adjustments.h"
#include "fc/runtime_config.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
@ -693,11 +693,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_ALTITUDE:
#if defined(BARO) || defined(SONAR)
sbufWriteU32(dst, altitudeHoldGetEstimatedAltitude());
sbufWriteU32(dst, getEstimatedAltitude());
#else
sbufWriteU32(dst, 0);
#endif
sbufWriteU16(dst, vario);
sbufWriteU16(dst, getEstimatedVario());
break;
case MSP_SONAR_ALTITUDE:

2
src/main/fc/fc_tasks.c
View File

@ -49,7 +49,7 @@
#include "fc/cli.h"
#include "fc/fc_dispatch.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"

184
src/main/flight/altitudehold.c → src/main/flight/altitude.c
View File

@ -34,16 +34,23 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "rx/rx.h"
#include "sensors/sensors.h"
#include "sensors/barometer.h"
#include "sensors/sonar.h"
enum {
DEBUG_ALTITUDE_ACC,
DEBUG_ALTITUDE_VEL,
DEBUG_ALTITUDE_HEIGHT
};
PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0);
PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig,
@ -55,7 +62,8 @@ uint8_t velocityControl = 0;
int32_t errorVelocityI = 0;
int32_t altHoldThrottleAdjustment = 0;
int32_t AltHold;
int32_t vario = 0; // variometer in cm/s
int32_t estimatedVario = 0; // variometer in cm/s
static int32_t estimatedAltitude = 0; // in cm
static pidProfile_t *pidProfile;
@ -68,7 +76,6 @@ void configureAltitudeHold(pidProfile_t *initialPidProfile)
#if defined(BARO) || defined(SONAR)
static int16_t initialThrottleHold;
static int32_t EstAlt; // in cm
// 40hz update rate (20hz LPF on acc)
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
@ -87,7 +94,7 @@ static void applyMultirotorAltHold(void)
rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = EstAlt;
AltHold = estimatedAltitude;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, PWM_RANGE_MIN, PWM_RANGE_MAX);
@ -100,7 +107,7 @@ static void applyMultirotorAltHold(void)
velocityControl = 1;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = EstAlt;
AltHold = estimatedAltitude;
velocityControl = 0;
isAltHoldChanged = 0;
}
@ -136,7 +143,7 @@ void updateAltHoldState(void)
if (!FLIGHT_MODE(BARO_MODE)) {
ENABLE_FLIGHT_MODE(BARO_MODE);
AltHold = EstAlt;
AltHold = estimatedAltitude;
initialThrottleHold = rcData[THROTTLE];
errorVelocityI = 0;
altHoldThrottleAdjustment = 0;
@ -153,7 +160,7 @@ void updateSonarAltHoldState(void)
if (!FLIGHT_MODE(SONAR_MODE)) {
ENABLE_FLIGHT_MODE(SONAR_MODE);
AltHold = EstAlt;
AltHold = estimatedAltitude;
initialThrottleHold = rcData[THROTTLE];
errorVelocityI = 0;
altHoldThrottleAdjustment = 0;
@ -178,7 +185,7 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
// Altitude P-Controller
if (!velocityControl) {
error = constrain(AltHold - EstAlt, -500, 500);
error = constrain(AltHold - estimatedAltitude, -500, 500);
error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
} else {
@ -203,122 +210,103 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
static timeUs_t previousTimeUs;
uint32_t dTime;
int32_t baroVel;
float dt;
float vel_acc;
int32_t vel_tmp;
float accZ_tmp;
static float accZ_old = 0.0f;
static float vel = 0.0f;
static float accAlt = 0.0f;
static int32_t lastBaroAlt;
#ifdef SONAR
int32_t sonarAlt = SONAR_OUT_OF_RANGE;
static int32_t baroAlt_offset = 0;
float sonarTransition;
#endif
dTime = currentTimeUs - previousTimeUs;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
static timeUs_t previousTimeUs = 0;
const uint32_t dTime = currentTimeUs - previousTimeUs;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
return;
}
previousTimeUs = currentTimeUs;
static float vel = 0.0f;
static float accAlt = 0.0f;
int32_t baroAlt = 0;
#ifdef BARO
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
vel = 0;
accAlt = 0;
}
baro.BaroAlt = baroCalculateAltitude();
#else
baro.BaroAlt = 0;
#endif
#ifdef SONAR
sonarAlt = sonarRead();
sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle());
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// just use the SONAR
baroAlt_offset = baro.BaroAlt - sonarAlt;
baro.BaroAlt = sonarAlt;
} else {
baro.BaroAlt -= baroAlt_offset;
if (sonarAlt > 0 && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
baro.BaroAlt = sonarAlt * sonarTransition + baro.BaroAlt * (1.0f - sonarTransition);
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
vel = 0;
accAlt = 0;
} else {
baroAlt = baroCalculateAltitude();
estimatedAltitude = baroAlt;
}
}
#endif
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / (float)accSumCount;
} else {
accZ_tmp = 0;
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
int32_t sonarAlt = sonarCalculateAltitude(sonarRead(), getCosTiltAngle());
if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
sonarAlt = (float)sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition);
estimatedAltitude = sonarAlt;
}
}
vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
#ifdef DEBUG_ALT_HOLD
debug[1] = accSum[2] / accSumCount; // acceleration
debug[2] = vel; // velocity
debug[3] = accAlt; // height
#endif
float accZ_tmp = 0;
#ifdef ACC
if (sensors(SENSOR_ACC)) {
const float dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / accSumCount;
}
const float vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)baroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
estimatedAltitude = accAlt;
}
#endif
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_ACC, accSum[2] / accSumCount);
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_VEL, vel);
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_HEIGHT, accAlt);
imuResetAccelerationSum();
int32_t baroVel = 0;
#ifdef BARO
if (!isBaroCalibrationComplete()) {
return;
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
return;
}
static int32_t lastBaroAlt = 0;
baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = baroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
}
#endif
#ifdef SONAR
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// the sonar has the best range
EstAlt = baro.BaroAlt;
} else {
EstAlt = accAlt;
}
#else
EstAlt = accAlt;
#endif
baroVel = (baro.BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = baro.BaroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
// 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 * barometerConfig()->baro_cf_vel + baroVel * (1.0f - barometerConfig()->baro_cf_vel);
vel_tmp = lrintf(vel);
vel = vel * barometerConfig()->baro_cf_vel + (float)baroVel * (1.0f - barometerConfig()->baro_cf_vel);
int32_t vel_tmp = lrintf(vel);
// set vario
vario = applyDeadband(vel_tmp, 5);
estimatedVario = applyDeadband(vel_tmp, 5);
static float accZ_old = 0.0f;
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
}
#endif
int32_t altitudeHoldGetEstimatedAltitude(void)
int32_t getEstimatedAltitude(void)
{
return EstAlt;
return estimatedAltitude;
}
#endif
int32_t getEstimatedVario(void)
{
return estimatedVario;
}

5
src/main/flight/altitudehold.h → src/main/flight/altitude.h
View File

@ -20,7 +20,6 @@
#include "common/time.h"
extern int32_t AltHold;
extern int32_t vario;
typedef struct airplaneConfig_s {
bool fixedwing_althold_reversed; // false for negative pitch/althold gain. later check if need more than just sign
@ -29,6 +28,8 @@ typedef struct airplaneConfig_s {
PG_DECLARE(airplaneConfig_t, airplaneConfig);
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
int32_t getEstimatedAltitude(void);
int32_t getEstimatedVario(void);
struct pidProfile_s;
void configureAltitudeHold(struct pidProfile_s *initialPidProfile);
@ -36,5 +37,3 @@ void configureAltitudeHold(struct pidProfile_s *initialPidProfile);
void applyAltHold(void);
void updateAltHoldState(void);
void updateSonarAltHoldState(void);
int32_t altitudeHoldGetEstimatedAltitude(void);

1
src/main/flight/imu.h
View File

@ -67,7 +67,6 @@ typedef struct imuRuntimeConfig_s {
void imuConfigure(uint16_t throttle_correction_angle);
float getCosTiltAngle(void);
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
void imuUpdateAttitude(timeUs_t currentTimeUs);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);

8
src/main/io/osd.c
View File

@ -256,7 +256,7 @@ static void osdDrawSingleElement(uint8_t item)
case OSD_ALTITUDE:
{
int32_t alt = osdGetAltitude(baro.BaroAlt);
int32_t alt = osdGetAltitude(getEstimatedAltitude());
sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
break;
}
@ -620,7 +620,7 @@ void osdUpdateAlarms(void)
// This is overdone?
// uint16_t *itemPos = osdConfig()->item_pos;
int32_t alt = osdGetAltitude(baro.BaroAlt) / 100;
int32_t alt = osdGetAltitude(getEstimatedAltitude()) / 100;
statRssi = rssi * 100 / 1024;
if (statRssi < osdConfig()->rssi_alarm)
@ -700,8 +700,8 @@ static void osdUpdateStats(void)
if (stats.min_rssi > statRssi)
stats.min_rssi = statRssi;
if (stats.max_altitude < baro.BaroAlt)
stats.max_altitude = baro.BaroAlt;
if (stats.max_altitude < getEstimatedAltitude())
stats.max_altitude = getEstimatedAltitude();
}
#ifdef BLACKBOX

3
src/main/rx/jetiexbus.c
View File

@ -57,6 +57,7 @@
#ifdef TELEMETRY
#include <string.h>
#include "flight/altitude.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
#include "sensors/barometer.h"
@ -525,7 +526,7 @@ void handleJetiExBusTelemetry(void)
if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
jetiExSensors[EX_VOLTAGE].value = getBatteryVoltage();
jetiExSensors[EX_CURRENT].value = getAmperage();
jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt;
jetiExSensors[EX_ALTITUDE].value = getEstimatedAltitude();
jetiExSensors[EX_CAPACITY].value = getMAhDrawn();
jetiExSensors[EX_FRAMES_LOST].value = framesLost;
jetiExSensors[EX_TIME_DIFF].value = timeDiff;

2
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -60,7 +60,7 @@
#include "telemetry/telemetry.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"

8
src/main/telemetry/frsky.c
View File

@ -57,7 +57,7 @@
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "rx/rx.h"
@ -175,9 +175,9 @@ static void sendAccel(void)
static void sendBaro(void)
{
sendDataHead(ID_ALTITUDE_BP);
serialize16(baro.BaroAlt / 100);
serialize16(getEstimatedAltitude() / 100);
sendDataHead(ID_ALTITUDE_AP);
serialize16(ABS(baro.BaroAlt % 100));
serialize16(ABS(getEstimatedAltitude() % 100));
}
#ifdef GPS
@ -356,7 +356,7 @@ static void sendGPSLatLong(void)
static void sendVario(void)
{
sendDataHead(ID_VERT_SPEED);
serialize16(vario);
serialize16(getEstimatedVario());
}
/*

7
src/main/telemetry/hott.c
View File

@ -84,7 +84,7 @@
#include "flight/navigation.h"
#include "io/gps.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "telemetry/telemetry.h"
#include "telemetry/hott.h"
@ -210,7 +210,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
uint16_t altitude = GPS_altitude;
if (!STATE(GPS_FIX)) {
altitude = baro.BaroAlt / 100;
altitude = getEstimatedAltitude() / 100;
}
const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // GPS_altitude in m ; offset = 500 -> O m
@ -271,7 +271,7 @@ static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMess
static inline void hottEAMUpdateAltitude(HOTT_EAM_MSG_t *hottEAMMessage)
{
const uint16_t hottEamAltitude = (baro.BaroAlt / 100) + HOTT_EAM_OFFSET_HEIGHT;
const uint16_t hottEamAltitude = (getEstimatedAltitude() / 100) + HOTT_EAM_OFFSET_HEIGHT;
hottEAMMessage->altitude_L = hottEamAltitude & 0x00FF;
hottEAMMessage->altitude_H = hottEamAltitude >> 8;
@ -279,6 +279,7 @@ static inline void hottEAMUpdateAltitude(HOTT_EAM_MSG_t *hottEAMMessage)
static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage)
{
int32_t vario = getEstimatedVario();
hottEAMMessage->climbrate_L = (30000 + vario) & 0x00FF;
hottEAMMessage->climbrate_H = (30000 + vario) >> 8;
hottEAMMessage->climbrate3s = 120 + (vario / 100);

4
src/main/telemetry/ltm.c
View File

@ -69,7 +69,7 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "flight/navigation.h"
#include "telemetry/telemetry.h"
@ -144,7 +144,7 @@ static void ltm_gframe(void)
ltm_serialise_8((uint8_t)(GPS_speed / 100));
#if defined(BARO) || defined(SONAR)
ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() : GPS_altitude * 100;
ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : GPS_altitude * 100;
#else
ltm_alt = GPS_altitude * 100;
#endif

6
src/main/telemetry/mavlink.c
View File

@ -50,7 +50,7 @@
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "io/serial.h"
#include "io/gimbal.h"
@ -331,7 +331,7 @@ void mavlinkSendPosition(void)
GPS_altitude * 1000,
// relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
#if defined(BARO) || defined(SONAR)
(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() * 10 : GPS_altitude * 1000,
(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : GPS_altitude * 1000,
#else
GPS_altitude * 1000,
#endif
@ -400,7 +400,7 @@ void mavlinkSendHUDAndHeartbeat(void)
#if defined(BARO) || defined(SONAR)
if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
// Baro or sonar generally is a better estimate of altitude than GPS MSL altitude
mavAltitude = altitudeHoldGetEstimatedAltitude() / 100.0;
mavAltitude = getEstimatedAltitude() / 100.0;
}
#if defined(GPS)
else if (sensors(SENSOR_GPS)) {

6
src/main/telemetry/smartport.c
View File

@ -32,7 +32,7 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/mixer.h"
@ -638,7 +638,7 @@ void handleSmartPortTelemetry(void)
//case FSSP_DATAID_RPM :
case FSSP_DATAID_ALTITUDE :
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, baro.BaroAlt); // unknown given unit, requested 100 = 1 meter
smartPortSendPackage(id, getEstimatedAltitude()); // unknown given unit, requested 100 = 1 meter
smartPortHasRequest = 0;
}
break;
@ -675,7 +675,7 @@ void handleSmartPortTelemetry(void)
//case FSSP_DATAID_CAP_USED :
case FSSP_DATAID_VARIO :
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s
smartPortSendPackage(id, getEstimatedVario()); // unknown given unit but requested in 100 = 1m/s
smartPortHasRequest = 0;
}
break;

4
src/test/Makefile
View File

@ -25,7 +25,7 @@ alignsensor_unittest_SRC := \
altitude_hold_unittest_SRC := \
$(USER_DIR)/flight/altitudehold.c
$(USER_DIR)/flight/altitude.c
baro_bmp085_unittest_SRC := \
@ -66,7 +66,7 @@ flight_failsafe_unittest_SRC := \
flight_imu_unittest_SRC := \
$(USER_DIR)/flight/imu.c \
$(USER_DIR)/flight/altitudehold.c \
$(USER_DIR)/flight/altitude.c \
$(USER_DIR)/common/maths.c

2
src/test/unit/altitude_hold_unittest.cc.txt
View File

@ -45,7 +45,7 @@ extern "C" {
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/altitudehold.h"
#include "flight/altitude.h"
#include "config/runtime_config.h"

3
src/test/unit/telemetry_hott_unittest.cc
View File

@ -183,7 +183,8 @@ uint32_t fixedMillis = 0;
baro_t baro;
int32_t vario = 0;
uint32_t getEstimatedAltitude() { return 0; }
uint32_t getEstimatedVario() { return 0; }
uint32_t millis(void) {
return fixedMillis;