From eadaff0bdd3b34790a5f4d4ac81eb81e6794534c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 30 Aug 2017 08:50:40 +0100 Subject: [PATCH] Code tidy and comments update --- src/main/drivers/accgyro/accgyro_mpu.c | 20 +++++----- src/main/fc/fc_rc.c | 53 +++++++++++++------------- src/main/flight/pid.c | 7 ++-- 3 files changed, 40 insertions(+), 40 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index d15d4aa79a..c7a10665bf 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -39,16 +39,16 @@ #include "drivers/system.h" #include "drivers/time.h" -#include "accgyro.h" -#include "accgyro_mpu3050.h" -#include "accgyro_mpu6050.h" -#include "accgyro_mpu6500.h" -#include "accgyro_spi_bmi160.h" -#include "accgyro_spi_icm20689.h" -#include "accgyro_spi_mpu6000.h" -#include "accgyro_spi_mpu6500.h" -#include "accgyro_spi_mpu9250.h" -#include "accgyro_mpu.h" +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_mpu3050.h" +#include "drivers/accgyro/accgyro_mpu6050.h" +#include "drivers/accgyro/accgyro_mpu6500.h" +#include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_icm20689.h" +#include "drivers/accgyro/accgyro_spi_mpu6000.h" +#include "drivers/accgyro/accgyro_spi_mpu6500.h" +#include "drivers/accgyro/accgyro_spi_mpu9250.h" +#include "drivers/accgyro/accgyro_mpu.h" mpuResetFnPtr mpuResetFn; diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index b620aefd16..f0ac7e669b 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -23,14 +23,11 @@ #include "build/debug.h" -#include "common/maths.h" #include "common/axis.h" +#include "common/maths.h" #include "common/utils.h" -#include "common/filter.h" #include "config/feature.h" -#include "config/parameter_group.h" -#include "config/parameter_group_ids.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -40,33 +37,36 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "flight/failsafe.h" +#include "flight/imu.h" +#include "flight/pid.h" #include "rx/rx.h" #include "scheduler/scheduler.h" #include "sensors/battery.h" -#include "flight/failsafe.h" -#include "flight/imu.h" -#include "flight/pid.h" -#include "flight/mixer.h" static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3]; static float throttlePIDAttenuation; -float getSetpointRate(int axis) { +float getSetpointRate(int axis) +{ return setpointRate[axis]; } -float getRcDeflection(int axis) { +float getRcDeflection(int axis) +{ return rcDeflection[axis]; } -float getRcDeflectionAbs(int axis) { +float getRcDeflectionAbs(int axis) +{ return rcDeflectionAbs[axis]; } -float getThrottlePIDAttenuation(void) { +float getThrottlePIDAttenuation(void) +{ return throttlePIDAttenuation; } @@ -75,10 +75,8 @@ static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for void generateThrottleCurve(void) { - uint8_t i; - - for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { - int16_t tmp = 10 * i - currentControlRateProfile->thrMid8; + for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { + const int16_t tmp = 10 * i - currentControlRateProfile->thrMid8; uint8_t y = 1; if (tmp > 0) y = 100 - currentControlRateProfile->thrMid8; @@ -89,7 +87,7 @@ void generateThrottleCurve(void) } } -int16_t rcLookupThrottle(int32_t tmp) +static int16_t rcLookupThrottle(int32_t tmp) { const int32_t tmp2 = tmp / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] @@ -114,6 +112,7 @@ static void calculateSetpointRate(int axis) rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f); } + // scale rcCommandf to range [-1.0, 1.0] float rcCommandf = rcCommand[axis] / 500.0f; rcDeflection[axis] = rcCommandf; const float rcCommandfAbs = ABS(rcCommandf); @@ -196,16 +195,16 @@ void processRcCommand(void) if (rxConfig()->rcInterpolation) { // Set RC refresh rate for sampling and channels to filter switch (rxConfig()->rcInterpolation) { - case(RC_SMOOTHING_AUTO): - rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps - break; - case(RC_SMOOTHING_MANUAL): - rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval; - break; - case(RC_SMOOTHING_OFF): - case(RC_SMOOTHING_DEFAULT): - default: - rxRefreshRate = rxGetRefreshRate(); + case RC_SMOOTHING_AUTO: + rxRefreshRate = currentRxRefreshRate + 1000; // Add slight overhead to prevent ramps + break; + case RC_SMOOTHING_MANUAL: + rxRefreshRate = 1000 * rxConfig()->rcInterpolationInterval; + break; + case RC_SMOOTHING_OFF: + case RC_SMOOTHING_DEFAULT: + default: + rxRefreshRate = rxGetRefreshRate(); } if (isRXDataNew && rxRefreshRate > 0) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 064a2c95ac..d723d0041d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -353,6 +353,7 @@ static float calcHorizonLevelStrength(void) static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) { // calculate error angle and limit the angle to the max inclination + // rcDeflection is in range [-1.0, 1.0] float angle = pidProfile->levelSensitivity * getRcDeflection(axis); #ifdef GPS angle += GPS_angle[axis]; @@ -360,11 +361,11 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit); const float errorAngle = angle - ((attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f); if (FLIGHT_MODE(ANGLE_MODE)) { - // ANGLE mode - control is angle based, so control loop is needed + // ANGLE mode - control is angle based currentPidSetpoint = errorAngle * levelGain; } else { - // HORIZON mode - direct sticks control is applied to rate PID - // mix up angle error to desired AngleRate to add a little auto-level feel + // HORIZON mode - mix of ANGLE and ACRO modes + // mix in errorAngle to currentPidSetpoint to add a little auto-level feel const float horizonLevelStrength = calcHorizonLevelStrength(); currentPidSetpoint = currentPidSetpoint + (errorAngle * horizonGain * horizonLevelStrength); }