From b64c71264cd14b0229cafdaf5f05bac3786aabaf Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 16 Jan 2015 01:00:40 +0000 Subject: [PATCH] Renaming getRcStickPosition to getRcStickDeflection and moving to rc_controls.c. --- src/main/flight/flight.c | 18 +++++++++++------- src/main/io/rc_controls.c | 4 ++++ src/main/io/rc_controls.h | 2 ++ src/main/mw.c | 11 ++++------- src/main/mw.h | 2 -- 5 files changed, 21 insertions(+), 16 deletions(-) diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index 18577217e..dbc049b3d 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -57,10 +57,10 @@ static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; static int32_t errorAngleI[2] = { 0, 0 }; static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim); // pid controller function prototype + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii @@ -97,7 +97,7 @@ bool shouldAutotune(void) #endif static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { float RateError, errorAngle, AngleRate, gyroRate; float ITerm,PTerm,DTerm; @@ -114,8 +114,8 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control if (FLIGHT_MODE(HORIZON_MODE)) { // Figure out the raw stick positions - stickPosAil = getRcStickPosition(FD_ROLL); - stickPosEle = getRcStickPosition(FD_PITCH); + stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc); + stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc); if(abs(stickPosAil) > abs(stickPosEle)){ mostDeflectedPos = abs(stickPosAil); @@ -205,8 +205,10 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control } static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, - uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim) + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { + UNUSED(rxConfig); + int axis, prop; int32_t error, errorAngle; int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm; @@ -288,8 +290,10 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa #define GYRO_I_MAX 256 static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, - rollAndPitchTrims_t *angleTrim) + rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig) { + UNUSED(rxConfig); + int32_t errorAngle; int axis; int32_t delta, deltaSum; diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 66db65127..9bd456008 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -517,6 +517,10 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges) } } +int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { + return min(abs(rcData[axis] - midrc), 500); +} + void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse) { uint8_t index; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 745acd468..8441ea90f 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -214,3 +214,5 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig); bool isUsingSticksForArming(void); + +int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); diff --git a/src/main/mw.c b/src/main/mw.c index fbc9cfd7e..9b2a45e0a 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -97,7 +97,8 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m extern uint8_t dynP8[3], dynI8[3], dynD8[3]; extern failsafe_t *failsafe; -typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims); +typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, + uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype extern pidControllerFuncPtr pid_controller; @@ -355,11 +356,6 @@ void mwArm(void) } } -int32_t getRcStickPosition(int32_t axis) { - - return min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500); -} - // Automatic ACC Offset Calibration bool AccInflightCalibrationArmed = false; bool AccInflightCalibrationMeasurementDone = false; @@ -714,7 +710,8 @@ void loop(void) ¤tProfile->pidProfile, currentControlRateProfile, masterConfig.max_angle_inclination, - ¤tProfile->accelerometerTrims + ¤tProfile->accelerometerTrims, + &masterConfig.rxConfig ); mixTable(); diff --git a/src/main/mw.h b/src/main/mw.h index 941efe8b3..aebee40be 100644 --- a/src/main/mw.h +++ b/src/main/mw.h @@ -22,5 +22,3 @@ void handleInflightCalibrationStickPosition(); void mwDisarm(void); void mwArm(void); - -int32_t getRcStickPosition(int32_t axis); \ No newline at end of file -- 2.11.4.GIT