diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 75ced4a5f..716d83e49 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -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) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 0aa043e38..07f552267 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -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); diff --git a/src/main/config/config.c b/src/main/config/config.c index b90e49e5e..16dca4403 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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); diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 92a59fcdf..097c8ec23 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -19,6 +19,9 @@ #include #include #include +#include + +#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 + diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 252c12f78..f4dfee0a9 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -15,8 +15,9 @@ * along with Cleanflight. If not, see . */ +void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig); -void updateAltHold(void); +void applyAltHold(void); void updateAltHoldState(void); void updateSonarAltHoldState(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 90eb488e3..f525d5166 100755 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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 */ diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 32febf044..6fc93375c 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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); diff --git a/src/main/mw.c b/src/main/mw.c index 790bba241..f2f407d57 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -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