| 4,7 → 4,7 |
|---|
| #include "ubx.h" |
| #include "mymath.h" |
| #include "timer0.h" |
| //#include "uart.h" |
| #include "uart.h" |
| #include "rc.h" |
| #include "eeprom.h" |
| |
| 16,9 → 16,9 |
|---|
| GPS_FLIGHT_MODE_HOME, |
| } FlightMode_t; |
| |
| #define GPS_STICK_LIMIT 200 // limit of gps stick control to avoid critical flight attitudes |
| #define GPS_STICK_LIMIT 500 // limit of gps stick control to avoid critical flight attitudes |
| #define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral |
| #define GPS_P_LIMIT 100 |
| #define GPS_P_LIMIT 250 |
| |
| |
| typedef struct |
| 149,7 → 149,7 |
|---|
| // then the P part of the controller is deactivated. |
| void GPS_PIDController(GPS_Pos_t *pTargetPos) |
| { |
| int32_t PID_Nick, PID_Roll; |
| static int32_t PID_Nick, PID_Roll; |
| int32_t coscompass, sincompass; |
| int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm |
| int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0, I_North = 0, I_East = 0; |
| 206,15 → 206,15 |
|---|
| GPSPosDevIntegral_East = 0; |
| } |
| |
| //Calculate PID-components of the controller (negative sign for compensation) |
| //Calculate PID-components of the controller |
| |
| // P-Part |
| P_North = -((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512; |
| P_East = -((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/512; |
| P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512; |
| P_East = ((int32_t)FCParam.NaviGpsP * GPSPosDev_East)/512; |
| |
| // I-Part |
| I_North = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048; |
| I_East = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048; |
| I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048; |
| I_East = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048; |
| |
| // combine P- & I-Part |
| PID_North = P_North + I_North; |
| 234,8 → 234,8 |
|---|
| } |
| |
| // D-Part |
| D_North = -((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/ 128; |
| D_East = -((int32_t)FCParam.NaviGpsD * GPSInfo.veleast) / 128; |
| D_North = ((int32_t)FCParam.NaviGpsD * GPSInfo.velnorth)/ 128; |
| D_East = ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast) / 128; |
| |
| // combine PI- and D-Part |
| PID_North += D_North; |
| 261,9 → 261,10 |
|---|
| |
| coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
| sincompass = (int32_t)c_sin_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR); |
| PID_Roll = (coscompass * PID_East - sincompass * PID_North) / 8192; |
| PID_Nick = -1*((sincompass * PID_East + coscompass * PID_North) / 8192); |
| PID_Nick = (PID_Nick + (coscompass * PID_North + sincompass * PID_East) / 8192)/2; |
| PID_Roll = (PID_Roll + (sincompass * PID_North - coscompass * PID_East) / 8192)/2; |
| |
| |
| // limit resulting GPS control vector |
| GPS_LimitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
| |
| 285,7 → 286,7 |
|---|
| void GPS_Main(void) |
| { |
| static uint8_t GPS_P_Delay = 0; |
| uint16_t beep_rythm = 0; |
| static uint16_t beep_rythm = 0; |
| |
| GPS_UpdateParameter(); |
| |
| 292,7 → 293,7 |
|---|
| // store home position if start of flight flag is set |
| if(MKFlags & MKFLAG_CALIBRATE) |
| { |
| GPS_SetCurrPosition(&HomePosition); |
| if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700; |
| } |
| |
| switch(GPSInfo.status) |