From 02e33f7d33a6d9296bef88644d8b0046a50c196a Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 9 Nov 2016 16:56:56 +0100 Subject: [PATCH] PID and Motormix, code optimization --- UAV-ControlSystem/src/Flight/pid.c | 5 +++-- UAV-ControlSystem/src/drivers/motormix.c | 6 +----- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index ad5a3ee..6288df7 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -26,6 +26,7 @@ #define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/ #define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/ #define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/ +#define BAROMETER_SCALE 20 #define RADIO_RANGE 500 /*Radio range input*/ #define BAROMETER_RANGE 3 /*Determines the range of the maximum height (limits the rc input)*/ @@ -113,7 +114,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) break; case PID_ID_BAROMETER: - sensorValues[0] = barometer_GetCurrentAveragedtAltitude()*20; + sensorValues[0] = barometer_GetCurrentAveragedtAltitude()*BAROMETER_SCALE; break; default: @@ -174,7 +175,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) //desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate); - desiredCommand[THROTTLE] = 1*20; + desiredCommand[THROTTLE] = 1*BAROMETER_SCALE; break; default: diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index aff2051..88a1778 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -105,11 +105,7 @@ void mix() int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]; - if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) - { - throttle += 1482; - } - + if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += 1482; /* Mixer Full Scale enabled */ if (flags_IsSet_ID(systemFlags_mixerfullscale_id))