AltHold cleanup.
* Renamed several methods and variables so they make more sense. * Move more altitude hold related code out of imu.c/h into altitudehold.c/h. * Fixed a unsigned integer being using instead of an signed integer in the throttle calculation code.
This commit is contained in:
Dominic Clifton
parent
7d4abb8a4a
commit
daa823ddba
|
|
@ -20,6 +20,18 @@
|
|||
|
||||
#include "maths.h"
|
||||
|
||||
int32_t applyDeadband(int32_t value, int32_t deadband)
|
||||
{
|
||||
if (abs(value) < deadband) {
|
||||
value = 0;
|
||||
} else if (value > 0) {
|
||||
value -= deadband;
|
||||
} else if (value < 0) {
|
||||
value += deadband;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
int constrain(int amt, int low, int high)
|
||||
{
|
||||
if (amt < low)
|
||||
|
|
|
|||
|
|
@ -40,6 +40,8 @@ typedef struct stdev_t
|
|||
int m_n;
|
||||
} stdev_t;
|
||||
|
||||
int32_t applyDeadband(int32_t value, int32_t deadband);
|
||||
|
||||
int constrain(int amt, int low, int high);
|
||||
float constrainf(float amt, float low, float high);
|
||||
|
||||
|
|
@ -48,4 +50,5 @@ void devPush(stdev_t *dev, float x);
|
|||
float devVariance(stdev_t *dev);
|
||||
float devStandardDeviation(stdev_t *dev);
|
||||
float degreesToRadians(int16_t degrees);
|
||||
|
||||
int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
|
||||
|
|
|
|||
|
|
@ -54,6 +54,7 @@
|
|||
#include "io/ledstrip.h"
|
||||
#include "io/gps.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/altitudehold.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/navigation.h"
|
||||
|
||||
|
|
@ -453,7 +454,8 @@ void activateConfig(void)
|
|||
imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
|
||||
imuRuntimeConfig.small_angle = masterConfig.small_angle;
|
||||
|
||||
configureImu(&imuRuntimeConfig, ¤tProfile->pidProfile, ¤tProfile->barometerConfig, ¤tProfile->accDeadband);
|
||||
configureImu(&imuRuntimeConfig, ¤tProfile->pidProfile, ¤tProfile->accDeadband);
|
||||
configureAltitudeHold(¤tProfile->pidProfile, ¤tProfile->barometerConfig);
|
||||
|
||||
calculateThrottleAngleScale(currentProfile->throttle_correction_angle);
|
||||
calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff);
|
||||
|
|
|
|||
|
|
@ -19,6 +19,9 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
|
@ -42,6 +45,7 @@
|
|||
#include "sensors/battery.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/sonar.h"
|
||||
|
||||
#include "io/escservo.h"
|
||||
#include "io/gimbal.h"
|
||||
|
|
@ -63,9 +67,26 @@
|
|||
#include "config/config_master.h"
|
||||
|
||||
#if defined(BARO) || defined(SONAR)
|
||||
|
||||
barometerConfig_t *barometerConfig;
|
||||
pidProfile_t *pidProfile;
|
||||
|
||||
void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig)
|
||||
{
|
||||
pidProfile = initialPidProfile;
|
||||
barometerConfig = intialBarometerConfig;
|
||||
}
|
||||
|
||||
|
||||
static int16_t initialThrottleHold;
|
||||
|
||||
static void multirotorAltHold(void)
|
||||
|
||||
// 40hz update rate (20hz LPF on acc)
|
||||
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
|
||||
|
||||
#define DEGREES_80_IN_DECIDEGREES 800
|
||||
|
||||
static void applyMultirotorAltHold(void)
|
||||
{
|
||||
static uint8_t isAltHoldChanged = 0;
|
||||
// multirotor alt hold
|
||||
|
|
@ -80,7 +101,7 @@ static void multirotorAltHold(void)
|
|||
AltHold = EstAlt;
|
||||
isAltHoldChanged = 0;
|
||||
}
|
||||
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
|
||||
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
|
||||
}
|
||||
} else {
|
||||
// slow alt changes, mostly used for aerial photography
|
||||
|
|
@ -94,25 +115,25 @@ static void multirotorAltHold(void)
|
|||
velocityControl = 0;
|
||||
isAltHoldChanged = 0;
|
||||
}
|
||||
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
|
||||
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
|
||||
}
|
||||
}
|
||||
|
||||
static void fixedWingAltHold()
|
||||
static void applyFixedWingAltHold()
|
||||
{
|
||||
// handle fixedwing-related althold. UNTESTED! and probably wrong
|
||||
// most likely need to check changes on pitch channel and 'reset' althold similar to
|
||||
// how throttle does it on multirotor
|
||||
|
||||
rcCommand[PITCH] += BaroPID * masterConfig.fixedwing_althold_dir;
|
||||
rcCommand[PITCH] += altHoldThrottleAdjustment * masterConfig.fixedwing_althold_dir;
|
||||
}
|
||||
|
||||
void updateAltHold(void)
|
||||
void applyAltHold(void)
|
||||
{
|
||||
if (STATE(FIXED_WING)) {
|
||||
fixedWingAltHold();
|
||||
applyFixedWingAltHold();
|
||||
} else {
|
||||
multirotorAltHold();
|
||||
applyMultirotorAltHold();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -129,7 +150,7 @@ void updateAltHoldState(void)
|
|||
AltHold = EstAlt;
|
||||
initialThrottleHold = rcCommand[THROTTLE];
|
||||
errorVelocityI = 0;
|
||||
BaroPID = 0;
|
||||
altHoldThrottleAdjustment = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -146,8 +167,163 @@ void updateSonarAltHoldState(void)
|
|||
AltHold = EstAlt;
|
||||
initialThrottleHold = rcCommand[THROTTLE];
|
||||
errorVelocityI = 0;
|
||||
BaroPID = 0; // TODO: Change naming of BaroPID to "AltPID" as this is used by both sonar and baro
|
||||
altHoldThrottleAdjustment = 0;
|
||||
}
|
||||
}
|
||||
|
||||
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
|
||||
{
|
||||
return abs(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
|
||||
}
|
||||
|
||||
int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination)
|
||||
{
|
||||
return max(abs(inclination->values.rollDeciDegrees), abs(inclination->values.pitchDeciDegrees));
|
||||
}
|
||||
|
||||
|
||||
int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
|
||||
{
|
||||
int32_t result = 0;
|
||||
int32_t error;
|
||||
int32_t setVel;
|
||||
|
||||
if (!isThrustFacingDownwards(&inclination)) {
|
||||
return result;
|
||||
}
|
||||
|
||||
// Altitude P-Controller
|
||||
|
||||
if (!velocityControl) {
|
||||
error = constrain(AltHold - EstAlt, -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 {
|
||||
setVel = setVelocity;
|
||||
}
|
||||
// Velocity PID-Controller
|
||||
|
||||
// P
|
||||
error = setVel - vel_tmp;
|
||||
result = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
|
||||
|
||||
// I
|
||||
errorVelocityI += (pidProfile->I8[PIDVEL] * error);
|
||||
errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
|
||||
result += errorVelocityI / 8192; // I in range +/-200
|
||||
|
||||
// D
|
||||
result -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void calculateEstimatedAltitude(uint32_t currentTime)
|
||||
{
|
||||
static uint32_t previousTime;
|
||||
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;
|
||||
|
||||
static int32_t baroAlt_offset = 0;
|
||||
float sonarTransition;
|
||||
|
||||
#ifdef SONAR
|
||||
int16_t tiltAngle;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
dTime = currentTime - previousTime;
|
||||
if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
|
||||
return;
|
||||
|
||||
previousTime = currentTime;
|
||||
|
||||
#ifdef BARO
|
||||
if (!isBaroCalibrationComplete()) {
|
||||
performBaroCalibrationCycle();
|
||||
vel = 0;
|
||||
accAlt = 0;
|
||||
}
|
||||
|
||||
BaroAlt = baroCalculateAltitude();
|
||||
#else
|
||||
BaroAlt = 0;
|
||||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
tiltAngle = calculateTiltAngle(&inclination);
|
||||
sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
|
||||
#endif
|
||||
|
||||
if (sonarAlt > 0 && sonarAlt < 200) {
|
||||
baroAlt_offset = BaroAlt - sonarAlt;
|
||||
BaroAlt = sonarAlt;
|
||||
} else {
|
||||
BaroAlt -= baroAlt_offset;
|
||||
if (sonarAlt > 0) {
|
||||
sonarTransition = (300 - sonarAlt) / 100.0f;
|
||||
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
|
||||
}
|
||||
}
|
||||
|
||||
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
|
||||
|
||||
// Integrator - velocity, cm/sec
|
||||
accZ_tmp = (float)accSum[2] / (float)accSumCount;
|
||||
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;
|
||||
|
||||
#if 0
|
||||
debug[1] = accSum[2] / accSumCount; // acceleration
|
||||
debug[2] = vel; // velocity
|
||||
debug[3] = accAlt; // height
|
||||
#endif
|
||||
|
||||
accSum_reset();
|
||||
|
||||
#ifdef BARO
|
||||
if (!isBaroCalibrationComplete()) {
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (sonarAlt > 0 && sonarAlt < 200) {
|
||||
// the sonar has the best range
|
||||
EstAlt = BaroAlt;
|
||||
} else {
|
||||
EstAlt = accAlt;
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
// 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);
|
||||
|
||||
// set vario
|
||||
vario = applyDeadband(vel_tmp, 5);
|
||||
|
||||
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
|
||||
|
||||
accZ_old = accZ_tmp;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -15,8 +15,9 @@
|
|||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig);
|
||||
|
||||
void updateAltHold(void);
|
||||
void applyAltHold(void);
|
||||
void updateAltHoldState(void);
|
||||
void updateSonarAltHoldState(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -65,7 +65,7 @@ int32_t vario = 0; // variometer in cm/s
|
|||
float throttleAngleScale;
|
||||
float fc_acc;
|
||||
|
||||
int32_t BaroPID = 0;
|
||||
int32_t altHoldThrottleAdjustment = 0;
|
||||
|
||||
float magneticDeclination = 0.0f; // calculated at startup from config
|
||||
float gyroScaleRad;
|
||||
|
|
@ -83,14 +83,12 @@ static void getEstimatedAttitude(void);
|
|||
|
||||
imuRuntimeConfig_t *imuRuntimeConfig;
|
||||
pidProfile_t *pidProfile;
|
||||
barometerConfig_t *barometerConfig;
|
||||
accDeadband_t *accDeadband;
|
||||
|
||||
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, accDeadband_t *initialAccDeadband)
|
||||
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband)
|
||||
{
|
||||
imuRuntimeConfig = initialImuRuntimeConfig;
|
||||
pidProfile = initialPidProfile;
|
||||
barometerConfig = intialBarometerConfig;
|
||||
accDeadband = initialAccDeadband;
|
||||
}
|
||||
|
||||
|
|
@ -203,16 +201,13 @@ void rotateV(struct fp_vector *v, fp_angles_t *delta)
|
|||
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
|
||||
}
|
||||
|
||||
int32_t applyDeadband(int32_t value, int32_t deadband)
|
||||
void accSum_reset(void)
|
||||
{
|
||||
if (abs(value) < deadband) {
|
||||
value = 0;
|
||||
} else if (value > 0) {
|
||||
value -= deadband;
|
||||
} else if (value < 0) {
|
||||
value += deadband;
|
||||
}
|
||||
return value;
|
||||
accSum[0] = 0;
|
||||
accSum[1] = 0;
|
||||
accSum[2] = 0;
|
||||
accSumCount = 0;
|
||||
accTimeSum = 0;
|
||||
}
|
||||
|
||||
// rotate acc into Earth frame and calculate acceleration in it
|
||||
|
|
@ -259,15 +254,6 @@ void acc_calc(uint32_t deltaT)
|
|||
accSumCount++;
|
||||
}
|
||||
|
||||
void accSum_reset(void)
|
||||
{
|
||||
accSum[0] = 0;
|
||||
accSum[1] = 0;
|
||||
accSum[2] = 0;
|
||||
accSumCount = 0;
|
||||
accTimeSum = 0;
|
||||
}
|
||||
|
||||
// baseflight calculation by Luggi09 originates from arducopter
|
||||
static int16_t calculateHeading(t_fp_vector *vec)
|
||||
{
|
||||
|
|
@ -371,165 +357,3 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
|
|||
angle = 900;
|
||||
return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f)));
|
||||
}
|
||||
|
||||
#if defined(BARO) || defined(SONAR)
|
||||
|
||||
// 40hz update rate (20hz LPF on acc)
|
||||
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
|
||||
|
||||
#define DEGREES_80_IN_DECIDEGREES 800
|
||||
|
||||
bool isThrustFacingDownwards(rollAndPitchInclination_t *inclination)
|
||||
{
|
||||
return abs(inclination->values.rollDeciDegrees) < DEGREES_80_IN_DECIDEGREES && abs(inclination->values.pitchDeciDegrees) < DEGREES_80_IN_DECIDEGREES;
|
||||
}
|
||||
|
||||
int16_t calculateTiltAngle(rollAndPitchInclination_t *inclination)
|
||||
{
|
||||
return max(abs(inclination->values.rollDeciDegrees), abs(inclination->values.pitchDeciDegrees));
|
||||
}
|
||||
|
||||
int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
|
||||
{
|
||||
uint32_t newBaroPID = 0;
|
||||
int32_t error;
|
||||
int32_t setVel;
|
||||
|
||||
if (!isThrustFacingDownwards(&inclination)) {
|
||||
return newBaroPID;
|
||||
}
|
||||
|
||||
// Altitude P-Controller
|
||||
|
||||
if (!velocityControl) {
|
||||
error = constrain(AltHold - EstAlt, -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 {
|
||||
setVel = setVelocity;
|
||||
}
|
||||
// Velocity PID-Controller
|
||||
|
||||
// P
|
||||
error = setVel - vel_tmp;
|
||||
newBaroPID = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
|
||||
|
||||
// I
|
||||
errorVelocityI += (pidProfile->I8[PIDVEL] * error);
|
||||
errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
|
||||
newBaroPID += errorVelocityI / 8192; // I in range +/-200
|
||||
|
||||
// D
|
||||
newBaroPID -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);
|
||||
|
||||
return newBaroPID;
|
||||
}
|
||||
|
||||
void calculateEstimatedAltitude(uint32_t currentTime)
|
||||
{
|
||||
static uint32_t previousTime;
|
||||
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;
|
||||
|
||||
static int32_t baroAlt_offset = 0;
|
||||
float sonarTransition;
|
||||
|
||||
#ifdef SONAR
|
||||
int16_t tiltAngle;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
dTime = currentTime - previousTime;
|
||||
if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
|
||||
return;
|
||||
|
||||
previousTime = currentTime;
|
||||
|
||||
#ifdef BARO
|
||||
if (!isBaroCalibrationComplete()) {
|
||||
performBaroCalibrationCycle();
|
||||
vel = 0;
|
||||
accAlt = 0;
|
||||
}
|
||||
|
||||
BaroAlt = baroCalculateAltitude();
|
||||
#else
|
||||
BaroAlt = 0;
|
||||
#endif
|
||||
|
||||
#ifdef SONAR
|
||||
tiltAngle = calculateTiltAngle(&inclination);
|
||||
sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
|
||||
#endif
|
||||
|
||||
if (sonarAlt > 0 && sonarAlt < 200) {
|
||||
baroAlt_offset = BaroAlt - sonarAlt;
|
||||
BaroAlt = sonarAlt;
|
||||
} else {
|
||||
BaroAlt -= baroAlt_offset;
|
||||
if (sonarAlt > 0) {
|
||||
sonarTransition = (300 - sonarAlt) / 100.0f;
|
||||
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
|
||||
}
|
||||
}
|
||||
|
||||
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
|
||||
|
||||
// Integrator - velocity, cm/sec
|
||||
accZ_tmp = (float)accSum[2] / (float)accSumCount;
|
||||
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;
|
||||
|
||||
#if 0
|
||||
debug[1] = accSum[2] / accSumCount; // acceleration
|
||||
debug[2] = vel; // velocity
|
||||
debug[3] = accAlt; // height
|
||||
#endif
|
||||
|
||||
accSum_reset();
|
||||
|
||||
#ifdef BARO
|
||||
if (!isBaroCalibrationComplete()) {
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (sonarAlt > 0 && sonarAlt < 200) {
|
||||
// the sonar has the best range
|
||||
EstAlt = BaroAlt;
|
||||
} else {
|
||||
EstAlt = accAlt;
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
// 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);
|
||||
|
||||
// set vario
|
||||
vario = applyDeadband(vel_tmp, 5);
|
||||
|
||||
BaroPID = calculateBaroPid(vel_tmp, accZ_tmp, accZ_old);
|
||||
|
||||
accZ_old = accZ_tmp;
|
||||
}
|
||||
#endif /* BARO */
|
||||
|
|
|
|||
|
|
@ -20,8 +20,11 @@
|
|||
extern int32_t errorVelocityI;
|
||||
extern uint8_t velocityControl;
|
||||
extern int32_t setVelocity;
|
||||
extern int32_t BaroPID;
|
||||
extern int32_t altHoldThrottleAdjustment;
|
||||
extern int16_t throttleAngleCorrection;
|
||||
extern uint32_t accTimeSum;
|
||||
extern int accSumCount;
|
||||
extern float accVelScale;
|
||||
|
||||
typedef struct imuRuntimeConfig_s {
|
||||
uint8_t acc_lpf_factor;
|
||||
|
|
@ -31,10 +34,12 @@ typedef struct imuRuntimeConfig_s {
|
|||
int8_t small_angle;
|
||||
} imuRuntimeConfig_t;
|
||||
|
||||
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, accDeadband_t *initialAccDeadband);
|
||||
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
|
||||
|
||||
void calculateEstimatedAltitude(uint32_t currentTime);
|
||||
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration);
|
||||
void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
|
||||
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
||||
void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
|
||||
|
||||
void accSum_reset(void);
|
||||
|
|
|
|||
|
|
@ -632,7 +632,7 @@ void loop(void)
|
|||
#if defined(BARO) || defined(SONAR)
|
||||
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
|
||||
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
|
||||
updateAltHold();
|
||||
applyAltHold();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue