Reworked altitude estimation. Made it work when ACC is disabled.
This commit is contained in:
parent
9053feb418
commit
6a03b48e7f
2
Makefile
2
Makefile
|
@ -694,7 +694,7 @@ COMMON_SRC = \
|
||||||
fc/rc_controls.c \
|
fc/rc_controls.c \
|
||||||
fc/runtime_config.c \
|
fc/runtime_config.c \
|
||||||
fc/cli.c \
|
fc/cli.c \
|
||||||
flight/altitudehold.c \
|
flight/altitude.c \
|
||||||
flight/failsafe.c \
|
flight/failsafe.c \
|
||||||
flight/imu.c \
|
flight/imu.c \
|
||||||
flight/mixer.c \
|
flight/mixer.c \
|
||||||
|
|
|
@ -64,5 +64,6 @@ typedef enum {
|
||||||
DEBUG_STACK,
|
DEBUG_STACK,
|
||||||
DEBUG_ESC_SENSOR_RPM,
|
DEBUG_ESC_SENSOR_RPM,
|
||||||
DEBUG_ESC_SENSOR_TMP,
|
DEBUG_ESC_SENSOR_TMP,
|
||||||
|
DEBUG_ALTITUDE,
|
||||||
DEBUG_COUNT
|
DEBUG_COUNT
|
||||||
} debugType_e;
|
} debugType_e;
|
||||||
|
|
|
@ -83,7 +83,7 @@ extern uint8_t __config_end;
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
#include "fc/fc_msp.h"
|
#include "fc/fc_msp.h"
|
||||||
|
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
@ -326,7 +326,8 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
|
||||||
"SCHEDULER",
|
"SCHEDULER",
|
||||||
"STACK",
|
"STACK",
|
||||||
"ESC_SENSOR_RPM",
|
"ESC_SENSOR_RPM",
|
||||||
"ESC_SENSOR_TMP"
|
"ESC_SENSOR_TMP",
|
||||||
|
"ALTITUDE"
|
||||||
};
|
};
|
||||||
|
|
||||||
#ifdef OSD
|
#ifdef OSD
|
||||||
|
|
|
@ -67,7 +67,7 @@
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
|
|
@ -72,7 +72,7 @@
|
||||||
|
|
||||||
#include "telemetry/telemetry.h"
|
#include "telemetry/telemetry.h"
|
||||||
|
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
|
|
@ -62,7 +62,7 @@
|
||||||
#include "fc/rc_adjustments.h"
|
#include "fc/rc_adjustments.h"
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
@ -700,11 +700,11 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
||||||
|
|
||||||
case MSP_ALTITUDE:
|
case MSP_ALTITUDE:
|
||||||
#if defined(BARO) || defined(SONAR)
|
#if defined(BARO) || defined(SONAR)
|
||||||
sbufWriteU32(dst, altitudeHoldGetEstimatedAltitude());
|
sbufWriteU32(dst, getEstimatedAltitude());
|
||||||
#else
|
#else
|
||||||
sbufWriteU32(dst, 0);
|
sbufWriteU32(dst, 0);
|
||||||
#endif
|
#endif
|
||||||
sbufWriteU16(dst, vario);
|
sbufWriteU16(dst, getEstimatedVario());
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_SONAR_ALTITUDE:
|
case MSP_SONAR_ALTITUDE:
|
||||||
|
|
|
@ -49,7 +49,7 @@
|
||||||
#include "fc/cli.h"
|
#include "fc/cli.h"
|
||||||
#include "fc/fc_dispatch.h"
|
#include "fc/fc_dispatch.h"
|
||||||
|
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
|
|
|
@ -34,16 +34,23 @@
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
|
||||||
|
#include "sensors/sensors.h"
|
||||||
#include "sensors/barometer.h"
|
#include "sensors/barometer.h"
|
||||||
#include "sensors/sonar.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_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0);
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig,
|
PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig,
|
||||||
|
@ -55,7 +62,8 @@ uint8_t velocityControl = 0;
|
||||||
int32_t errorVelocityI = 0;
|
int32_t errorVelocityI = 0;
|
||||||
int32_t altHoldThrottleAdjustment = 0;
|
int32_t altHoldThrottleAdjustment = 0;
|
||||||
int32_t AltHold;
|
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;
|
static pidProfile_t *pidProfile;
|
||||||
|
@ -68,7 +76,6 @@ void configureAltitudeHold(pidProfile_t *initialPidProfile)
|
||||||
#if defined(BARO) || defined(SONAR)
|
#if defined(BARO) || defined(SONAR)
|
||||||
|
|
||||||
static int16_t initialThrottleHold;
|
static int16_t initialThrottleHold;
|
||||||
static int32_t EstAlt; // in cm
|
|
||||||
|
|
||||||
// 40hz update rate (20hz LPF on acc)
|
// 40hz update rate (20hz LPF on acc)
|
||||||
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
|
#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;
|
rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband;
|
||||||
} else {
|
} else {
|
||||||
if (isAltHoldChanged) {
|
if (isAltHoldChanged) {
|
||||||
AltHold = EstAlt;
|
AltHold = estimatedAltitude;
|
||||||
isAltHoldChanged = 0;
|
isAltHoldChanged = 0;
|
||||||
}
|
}
|
||||||
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, PWM_RANGE_MIN, PWM_RANGE_MAX);
|
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||||
|
@ -100,7 +107,7 @@ static void applyMultirotorAltHold(void)
|
||||||
velocityControl = 1;
|
velocityControl = 1;
|
||||||
isAltHoldChanged = 1;
|
isAltHoldChanged = 1;
|
||||||
} else if (isAltHoldChanged) {
|
} else if (isAltHoldChanged) {
|
||||||
AltHold = EstAlt;
|
AltHold = estimatedAltitude;
|
||||||
velocityControl = 0;
|
velocityControl = 0;
|
||||||
isAltHoldChanged = 0;
|
isAltHoldChanged = 0;
|
||||||
}
|
}
|
||||||
|
@ -136,7 +143,7 @@ void updateAltHoldState(void)
|
||||||
|
|
||||||
if (!FLIGHT_MODE(BARO_MODE)) {
|
if (!FLIGHT_MODE(BARO_MODE)) {
|
||||||
ENABLE_FLIGHT_MODE(BARO_MODE);
|
ENABLE_FLIGHT_MODE(BARO_MODE);
|
||||||
AltHold = EstAlt;
|
AltHold = estimatedAltitude;
|
||||||
initialThrottleHold = rcData[THROTTLE];
|
initialThrottleHold = rcData[THROTTLE];
|
||||||
errorVelocityI = 0;
|
errorVelocityI = 0;
|
||||||
altHoldThrottleAdjustment = 0;
|
altHoldThrottleAdjustment = 0;
|
||||||
|
@ -153,7 +160,7 @@ void updateSonarAltHoldState(void)
|
||||||
|
|
||||||
if (!FLIGHT_MODE(SONAR_MODE)) {
|
if (!FLIGHT_MODE(SONAR_MODE)) {
|
||||||
ENABLE_FLIGHT_MODE(SONAR_MODE);
|
ENABLE_FLIGHT_MODE(SONAR_MODE);
|
||||||
AltHold = EstAlt;
|
AltHold = estimatedAltitude;
|
||||||
initialThrottleHold = rcData[THROTTLE];
|
initialThrottleHold = rcData[THROTTLE];
|
||||||
errorVelocityI = 0;
|
errorVelocityI = 0;
|
||||||
altHoldThrottleAdjustment = 0;
|
altHoldThrottleAdjustment = 0;
|
||||||
|
@ -178,7 +185,7 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
|
||||||
// Altitude P-Controller
|
// Altitude P-Controller
|
||||||
|
|
||||||
if (!velocityControl) {
|
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
|
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
|
setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
|
||||||
} else {
|
} else {
|
||||||
|
@ -203,122 +210,104 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
|
||||||
|
|
||||||
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
|
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
|
||||||
{
|
{
|
||||||
static timeUs_t previousTimeUs;
|
static timeUs_t previousTimeUs = 0;
|
||||||
uint32_t dTime;
|
const uint32_t dTime = currentTimeUs - previousTimeUs;
|
||||||
int32_t baroVel;
|
if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
|
||||||
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)
|
|
||||||
return;
|
return;
|
||||||
|
}
|
||||||
previousTimeUs = currentTimeUs;
|
previousTimeUs = currentTimeUs;
|
||||||
|
|
||||||
|
static float vel = 0.0f;
|
||||||
|
static float accAlt = 0.0f;
|
||||||
|
|
||||||
|
int32_t baroAlt = 0;
|
||||||
#ifdef BARO
|
#ifdef BARO
|
||||||
if (!isBaroCalibrationComplete()) {
|
if (sensors(SENSOR_BARO)) {
|
||||||
performBaroCalibrationCycle();
|
if (!isBaroCalibrationComplete()) {
|
||||||
vel = 0;
|
performBaroCalibrationCycle();
|
||||||
accAlt = 0;
|
vel = 0;
|
||||||
}
|
accAlt = 0;
|
||||||
|
} else {
|
||||||
baro.BaroAlt = baroCalculateAltitude();
|
baroAlt = baroCalculateAltitude();
|
||||||
#else
|
estimatedAltitude = baroAlt;
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
|
#ifdef SONAR
|
||||||
|
if (sensors(SENSOR_SONAR)) {
|
||||||
// Integrator - velocity, cm/sec
|
int32_t sonarAlt = sonarRead();
|
||||||
if (accSumCount) {
|
sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle());
|
||||||
accZ_tmp = (float)accSum[2] / (float)accSumCount;
|
if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
|
||||||
} else {
|
// SONAR in range, so use complementary filter
|
||||||
accZ_tmp = 0;
|
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
|
#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();
|
imuResetAccelerationSum();
|
||||||
|
|
||||||
|
int32_t baroVel = 0;
|
||||||
#ifdef BARO
|
#ifdef BARO
|
||||||
if (!isBaroCalibrationComplete()) {
|
if (sensors(SENSOR_BARO)) {
|
||||||
return;
|
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
|
#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).
|
// 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
|
// 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 = vel * barometerConfig()->baro_cf_vel + baroVel * (1.0f - barometerConfig()->baro_cf_vel);
|
||||||
vel_tmp = lrintf(vel);
|
int32_t vel_tmp = lrintf(vel);
|
||||||
|
|
||||||
// set vario
|
// 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);
|
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
|
||||||
|
|
||||||
accZ_old = accZ_tmp;
|
accZ_old = accZ_tmp;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
int32_t altitudeHoldGetEstimatedAltitude(void)
|
int32_t getEstimatedAltitude(void)
|
||||||
{
|
{
|
||||||
return EstAlt;
|
return estimatedAltitude;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
int32_t getEstimatedVario(void)
|
||||||
|
{
|
||||||
|
return estimatedVario;
|
||||||
|
}
|
|
@ -20,7 +20,6 @@
|
||||||
#include "common/time.h"
|
#include "common/time.h"
|
||||||
|
|
||||||
extern int32_t AltHold;
|
extern int32_t AltHold;
|
||||||
extern int32_t vario;
|
|
||||||
|
|
||||||
typedef struct airplaneConfig_s {
|
typedef struct airplaneConfig_s {
|
||||||
bool fixedwing_althold_reversed; // false for negative pitch/althold gain. later check if need more than just sign
|
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);
|
PG_DECLARE(airplaneConfig_t, airplaneConfig);
|
||||||
|
|
||||||
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
|
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
|
||||||
|
int32_t getEstimatedAltitude(void);
|
||||||
|
int32_t getEstimatedVario(void);
|
||||||
|
|
||||||
struct pidProfile_s;
|
struct pidProfile_s;
|
||||||
void configureAltitudeHold(struct pidProfile_s *initialPidProfile);
|
void configureAltitudeHold(struct pidProfile_s *initialPidProfile);
|
||||||
|
@ -36,5 +37,3 @@ void configureAltitudeHold(struct pidProfile_s *initialPidProfile);
|
||||||
void applyAltHold(void);
|
void applyAltHold(void);
|
||||||
void updateAltHoldState(void);
|
void updateAltHoldState(void);
|
||||||
void updateSonarAltHoldState(void);
|
void updateSonarAltHoldState(void);
|
||||||
|
|
||||||
int32_t altitudeHoldGetEstimatedAltitude(void);
|
|
|
@ -67,7 +67,6 @@ typedef struct imuRuntimeConfig_s {
|
||||||
void imuConfigure(uint16_t throttle_correction_angle);
|
void imuConfigure(uint16_t throttle_correction_angle);
|
||||||
|
|
||||||
float getCosTiltAngle(void);
|
float getCosTiltAngle(void);
|
||||||
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
|
|
||||||
void imuUpdateAttitude(timeUs_t currentTimeUs);
|
void imuUpdateAttitude(timeUs_t currentTimeUs);
|
||||||
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
||||||
|
|
||||||
|
|
|
@ -256,7 +256,7 @@ static void osdDrawSingleElement(uint8_t item)
|
||||||
|
|
||||||
case OSD_ALTITUDE:
|
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());
|
sprintf(buff, "%c%d.%01d%c", alt < 0 ? '-' : ' ', abs(alt / 100), abs((alt % 100) / 10), osdGetAltitudeSymbol());
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -619,7 +619,7 @@ void osdUpdateAlarms(void)
|
||||||
// This is overdone?
|
// This is overdone?
|
||||||
// uint16_t *itemPos = osdConfig()->item_pos;
|
// uint16_t *itemPos = osdConfig()->item_pos;
|
||||||
|
|
||||||
int32_t alt = osdGetAltitude(baro.BaroAlt) / 100;
|
int32_t alt = osdGetAltitude(getEstimatedAltitude()) / 100;
|
||||||
statRssi = rssi * 100 / 1024;
|
statRssi = rssi * 100 / 1024;
|
||||||
|
|
||||||
if (statRssi < osdConfig()->rssi_alarm)
|
if (statRssi < osdConfig()->rssi_alarm)
|
||||||
|
@ -699,8 +699,8 @@ static void osdUpdateStats(void)
|
||||||
if (stats.min_rssi > statRssi)
|
if (stats.min_rssi > statRssi)
|
||||||
stats.min_rssi = statRssi;
|
stats.min_rssi = statRssi;
|
||||||
|
|
||||||
if (stats.max_altitude < baro.BaroAlt)
|
if (stats.max_altitude < getEstimatedAltitude())
|
||||||
stats.max_altitude = baro.BaroAlt;
|
stats.max_altitude = getEstimatedAltitude();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void osdGetBlackboxStatusString(char * buff, uint8_t len)
|
static void osdGetBlackboxStatusString(char * buff, uint8_t len)
|
||||||
|
|
|
@ -57,6 +57,7 @@
|
||||||
|
|
||||||
#ifdef TELEMETRY
|
#ifdef TELEMETRY
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
#include "flight/altitude.h"
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
#include "sensors/battery.h"
|
#include "sensors/battery.h"
|
||||||
#include "sensors/barometer.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)) {
|
if((jetiExBusRequestFrame[EXBUS_HEADER_DATA_ID] == EXBUS_EX_REQUEST) && (calcCRC16(jetiExBusRequestFrame, jetiExBusRequestFrame[EXBUS_HEADER_MSG_LEN]) == 0)) {
|
||||||
jetiExSensors[EX_VOLTAGE].value = getBatteryVoltage();
|
jetiExSensors[EX_VOLTAGE].value = getBatteryVoltage();
|
||||||
jetiExSensors[EX_CURRENT].value = getAmperage();
|
jetiExSensors[EX_CURRENT].value = getAmperage();
|
||||||
jetiExSensors[EX_ALTITUDE].value = baro.BaroAlt;
|
jetiExSensors[EX_ALTITUDE].value = getEstimatedAltitude();
|
||||||
jetiExSensors[EX_CAPACITY].value = getMAhDrawn();
|
jetiExSensors[EX_CAPACITY].value = getMAhDrawn();
|
||||||
jetiExSensors[EX_FRAMES_LOST].value = framesLost;
|
jetiExSensors[EX_FRAMES_LOST].value = framesLost;
|
||||||
jetiExSensors[EX_TIME_DIFF].value = timeDiff;
|
jetiExSensors[EX_TIME_DIFF].value = timeDiff;
|
||||||
|
|
|
@ -60,7 +60,7 @@
|
||||||
|
|
||||||
#include "telemetry/telemetry.h"
|
#include "telemetry/telemetry.h"
|
||||||
|
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
|
|
@ -57,7 +57,7 @@
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
|
||||||
|
@ -175,9 +175,9 @@ static void sendAccel(void)
|
||||||
static void sendBaro(void)
|
static void sendBaro(void)
|
||||||
{
|
{
|
||||||
sendDataHead(ID_ALTITUDE_BP);
|
sendDataHead(ID_ALTITUDE_BP);
|
||||||
serialize16(baro.BaroAlt / 100);
|
serialize16(getEstimatedAltitude() / 100);
|
||||||
sendDataHead(ID_ALTITUDE_AP);
|
sendDataHead(ID_ALTITUDE_AP);
|
||||||
serialize16(ABS(baro.BaroAlt % 100));
|
serialize16(ABS(getEstimatedAltitude() % 100));
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef GPS
|
#ifdef GPS
|
||||||
|
@ -356,7 +356,7 @@ static void sendGPSLatLong(void)
|
||||||
static void sendVario(void)
|
static void sendVario(void)
|
||||||
{
|
{
|
||||||
sendDataHead(ID_VERT_SPEED);
|
sendDataHead(ID_VERT_SPEED);
|
||||||
serialize16(vario);
|
serialize16(getEstimatedVario());
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -84,7 +84,7 @@
|
||||||
#include "flight/navigation.h"
|
#include "flight/navigation.h"
|
||||||
#include "io/gps.h"
|
#include "io/gps.h"
|
||||||
|
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
|
|
||||||
#include "telemetry/telemetry.h"
|
#include "telemetry/telemetry.h"
|
||||||
#include "telemetry/hott.h"
|
#include "telemetry/hott.h"
|
||||||
|
@ -210,7 +210,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
|
||||||
|
|
||||||
uint16_t altitude = GPS_altitude;
|
uint16_t altitude = GPS_altitude;
|
||||||
if (!STATE(GPS_FIX)) {
|
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
|
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)
|
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_L = hottEamAltitude & 0x00FF;
|
||||||
hottEAMMessage->altitude_H = hottEamAltitude >> 8;
|
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)
|
static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage)
|
||||||
{
|
{
|
||||||
|
int32_t vario = getEstimatedVario();
|
||||||
hottEAMMessage->climbrate_L = (30000 + vario) & 0x00FF;
|
hottEAMMessage->climbrate_L = (30000 + vario) & 0x00FF;
|
||||||
hottEAMMessage->climbrate_H = (30000 + vario) >> 8;
|
hottEAMMessage->climbrate_H = (30000 + vario) >> 8;
|
||||||
hottEAMMessage->climbrate3s = 120 + (vario / 100);
|
hottEAMMessage->climbrate3s = 120 + (vario / 100);
|
||||||
|
|
|
@ -69,7 +69,7 @@
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
#include "flight/navigation.h"
|
#include "flight/navigation.h"
|
||||||
|
|
||||||
#include "telemetry/telemetry.h"
|
#include "telemetry/telemetry.h"
|
||||||
|
@ -144,7 +144,7 @@ static void ltm_gframe(void)
|
||||||
ltm_serialise_8((uint8_t)(GPS_speed / 100));
|
ltm_serialise_8((uint8_t)(GPS_speed / 100));
|
||||||
|
|
||||||
#if defined(BARO) || defined(SONAR)
|
#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
|
#else
|
||||||
ltm_alt = GPS_altitude * 100;
|
ltm_alt = GPS_altitude * 100;
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -50,7 +50,7 @@
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/navigation.h"
|
#include "flight/navigation.h"
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
|
|
||||||
#include "io/serial.h"
|
#include "io/serial.h"
|
||||||
#include "io/gimbal.h"
|
#include "io/gimbal.h"
|
||||||
|
@ -331,7 +331,7 @@ void mavlinkSendPosition(void)
|
||||||
GPS_altitude * 1000,
|
GPS_altitude * 1000,
|
||||||
// relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
|
// relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
|
||||||
#if defined(BARO) || defined(SONAR)
|
#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
|
#else
|
||||||
GPS_altitude * 1000,
|
GPS_altitude * 1000,
|
||||||
#endif
|
#endif
|
||||||
|
@ -400,7 +400,7 @@ void mavlinkSendHUDAndHeartbeat(void)
|
||||||
#if defined(BARO) || defined(SONAR)
|
#if defined(BARO) || defined(SONAR)
|
||||||
if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
|
if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
|
||||||
// Baro or sonar generally is a better estimate of altitude than GPS MSL altitude
|
// 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)
|
#if defined(GPS)
|
||||||
else if (sensors(SENSOR_GPS)) {
|
else if (sensors(SENSOR_GPS)) {
|
||||||
|
|
|
@ -32,7 +32,7 @@
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
|
@ -638,7 +638,7 @@ void handleSmartPortTelemetry(void)
|
||||||
//case FSSP_DATAID_RPM :
|
//case FSSP_DATAID_RPM :
|
||||||
case FSSP_DATAID_ALTITUDE :
|
case FSSP_DATAID_ALTITUDE :
|
||||||
if (sensors(SENSOR_BARO)) {
|
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;
|
smartPortHasRequest = 0;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
@ -675,7 +675,7 @@ void handleSmartPortTelemetry(void)
|
||||||
//case FSSP_DATAID_CAP_USED :
|
//case FSSP_DATAID_CAP_USED :
|
||||||
case FSSP_DATAID_VARIO :
|
case FSSP_DATAID_VARIO :
|
||||||
if (sensors(SENSOR_BARO)) {
|
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;
|
smartPortHasRequest = 0;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -25,7 +25,7 @@ alignsensor_unittest_SRC := \
|
||||||
|
|
||||||
|
|
||||||
altitude_hold_unittest_SRC := \
|
altitude_hold_unittest_SRC := \
|
||||||
$(USER_DIR)/flight/altitudehold.c
|
$(USER_DIR)/flight/altitude.c
|
||||||
|
|
||||||
|
|
||||||
baro_bmp085_unittest_SRC := \
|
baro_bmp085_unittest_SRC := \
|
||||||
|
@ -66,7 +66,7 @@ flight_failsafe_unittest_SRC := \
|
||||||
|
|
||||||
flight_imu_unittest_SRC := \
|
flight_imu_unittest_SRC := \
|
||||||
$(USER_DIR)/flight/imu.c \
|
$(USER_DIR)/flight/imu.c \
|
||||||
$(USER_DIR)/flight/altitudehold.c \
|
$(USER_DIR)/flight/altitude.c \
|
||||||
$(USER_DIR)/common/maths.c
|
$(USER_DIR)/common/maths.c
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -45,7 +45,7 @@ extern "C" {
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitude.h"
|
||||||
|
|
||||||
#include "config/runtime_config.h"
|
#include "config/runtime_config.h"
|
||||||
|
|
||||||
|
|
|
@ -183,7 +183,8 @@ uint32_t fixedMillis = 0;
|
||||||
|
|
||||||
baro_t baro;
|
baro_t baro;
|
||||||
|
|
||||||
int32_t vario = 0;
|
uint32_t getEstimatedAltitude() { return 0; }
|
||||||
|
uint32_t getEstimatedVario() { return 0; }
|
||||||
|
|
||||||
uint32_t millis(void) {
|
uint32_t millis(void) {
|
||||||
return fixedMillis;
|
return fixedMillis;
|
||||||
|
|
Loading…
Reference in New Issue