philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Fixed abs function for float values

Browse Source 
This could cause the anti windup to work. Needs testing.
This commit is contained in:
Jonas Holmberg 2016-11-01 14:05:52 +01:00
parent 4accab0660
commit fbae0cf452
3 changed files with 18 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
UAV-ControlSystem/inc/utilities.h
View File

@ -25,6 +25,8 @@
#define maxStringSize_CLI 100 //Max sting size used for the messages in the CLI
#define ABS_FLOAT(x) (((x) < 0)? -(x): (x))
typedef char typeString[maxStringSize_CLI];
typedef struct typeStringArr { char val[maxStringSize_CLI]; } typeStringArr;
@ -81,6 +83,8 @@ uint32_t accumulate(uint32_t list[], int length);
***********************************************************************/
void Error_Handler(void);
uint8_t reverse(uint8_t byte);
int16_t constrain(int16_t value, int16_t min, int16_t max);

27
UAV-ControlSystem/src/Flight/pid.c
View File

@ -19,6 +19,7 @@
#include <math.h>
#include "drivers/failsafe_toggles.h"
#include "drivers/motormix.h"
#include "utilities.h"
#define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/
@ -317,19 +318,19 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
// Anti windup protection
// if (motorLimitReached)
// {
// ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
// }
// else
// {
// pidProfileBuff->ITermLimit[axis] = abs(ITerm);
// }
if (motorLimitReached)
{
ITerm = constrainf(ITerm, -pidProfileBuff->ITermLimit[axis], pidProfileBuff->ITermLimit[axis]);
}
else
{
pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm);
}
if (motorLimitReached)
{
ITerm = pidProfileBuff->lastITerm[axis];
}
// if (motorLimitReached)
// {
// ITerm = pidProfileBuff->lastITerm[axis];
// }
pidProfileBuff->lastITerm[axis] = ITerm;
@ -645,6 +646,4 @@ void pidEproom(void)
PidProfile[PID_ID_GYRO].I[YAW] = 40;
}

1
UAV-ControlSystem/src/main.c
View File

@ -124,6 +124,7 @@ int main(void)
//Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler
initScheduler();
while (1)
{
//Run the scheduler, responsible for distributing all the work of the running system