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) |