12,14 → 12,14 |
#define TSK_HOLD 1 |
#define TSK_HOME 2 |
|
#define GPS_STICK_SENSE 20 // must be at least in a range where 90% of the trimming does not switch of the GPS function |
#define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes |
#define GPS_STICK_SENSE 15 // must be at least in a range where 90% of the trimming does not switch of the GPS function |
#define GPS_STICK_LIMIT 35 // limit of gps stick control to avoid critical flight attitudes |
#define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
#define MAX_VELOCITY 300 // max ground speed in cm/s during position control |
#define MAX_VELOCITY 700 // max ground speed in cm/s during position control |
|
|
int16_t GPS_Pitch = 0, GPS_Roll = 0; |
int32_t GPS_P_Factor = 0, GPS_I_Factor = 0, GPS_D_Factor = 0; |
uint8_t GPS_P_Factor = 0, GPS_I_Factor = 0, GPS_D_Factor = 0; |
|
|
|
162,12 → 162,12 |
//Calculate PID-components of the controller (negative sign for compensation) |
|
// P-Part |
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
P_North = -((int32_t)GPS_P_Factor * GPSPosDev_North)/2048; |
P_East = -((int32_t)GPS_P_Factor * GPSPosDev_East)/2048; |
|
// I-Part |
I_North = -(GPS_I_Factor * GPSPosDevIntegral_North)/8192; |
I_East = -(GPS_I_Factor * GPSPosDevIntegral_East)/8192; |
I_North = -((int32_t)GPS_I_Factor * GPSPosDevIntegral_North)/8192; |
I_East = -((int32_t)GPS_I_Factor * GPSPosDevIntegral_East)/8192; |
|
// combine P- & I-Part |
PID_North = P_North + I_North; |
174,13 → 174,13 |
PID_East = P_East + I_East; |
|
//limit PI-Part to limit the max velocity |
temp1 = (GPS_D_Factor * MAX_VELOCITY)/512; // the PI-Part limit |
temp1 = ((int32_t)GPS_D_Factor * MAX_VELOCITY)/512; // the PI-Part limit |
temp = (int32_t)c_sqrt(PID_North*PID_North + PID_East*PID_East); // the current PI-Part |
if(temp > temp1) // P-Part limit is reached |
{ |
// normalize P-part components to the P-Part limit |
PID_North = (PID_North * temp1)/temp; |
PID_East = (PID_East * temp1)/temp; |
PID_East = (PID_East * temp1) /temp; |
} |
else // PI-Part under its limit |
{ |
194,8 → 194,8 |
} |
|
// D-Part |
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
D_North = -((int32_t)GPS_D_Factor * GPSInfo.velnorth)/512; |
D_East = -((int32_t)GPS_D_Factor * GPSInfo.veleast)/512; |
|
|
// combine PI- and D-Part |