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

Reworked altitude estimation. Made it work when ACC is disabled.

This commit is contained in:
mikeller 2017-03-29 02:01:09 +13:00
parent 9053feb418
commit 6a03b48e7f
21 changed files with 127 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -694,7 +694,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"
@ -700,11 +700,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"

183
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,104 @@ 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 = sonarRead();
sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle());
if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
sonarAlt = 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] / (float)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);
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;
}
@ -619,7 +619,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)
@ -699,8 +699,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();
}
static void osdGetBlackboxStatusString(char * buff, uint8_t len)

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;