Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 843 → Rev 844

/branches/V0.68d Code Redesign killagreg/GPS.c
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
/branches/V0.68d Code Redesign killagreg/fc.c
1025,11 → 1025,12
if((ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) || (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
{
static uint8_t updCompass = 0;
int16_t w,v;
 
if (!updCompass--)
{
updCompass = 49; // update only at 2ms*50 = 100ms (10Hz)
// get current compass heading (angule between MK head and magnetic north)
// get current compass heading (angle between MK head and magnetic north)
#ifdef USE_MM3
CompassHeading = MM3_Heading();
#endif
1040,7 → 1041,11
else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; // calc course deviation
}
 
if ((MaxStickPitch < 75) && (MaxStickRoll < 75))
// get maximum attitude angle
w = abs(IntegralPitch/512);
v = abs(IntegralRoll /512);
if(v > w) w = v;
if (w < 25)
{
if(UpdateCompassCourse)
{
1048,7 → 1053,9
CompassCourse = CompassHeading;
CompassOffCourse = 0;
}
Reading_IntegralGyroYaw += (CompassOffCourse * FCParam.CompassYawEffect) / 16;
w = (w * FCParam.CompassYawEffect) / 64;
w = FCParam.CompassYawEffect - w;
if(w > 0) Reading_IntegralGyroYaw += (CompassOffCourse * w) / 32;
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1060,7 → 1067,7
GPS_P_Factor = FCParam.UserParam5;
GPS_D_Factor = FCParam.UserParam6;
if(EmergencyLanding) GPS_Main(230); // enables Comming Home
else GPS_Main(Poti3);
else GPS_Main(Poti3); // behavior controlled by Poti3
}
else
{
/branches/V0.68d Code Redesign killagreg/gps.h
6,9 → 6,9
extern int16_t GPS_Pitch;
extern int16_t GPS_Roll;
 
extern int32_t GPS_P_Factor;
extern int32_t GPS_I_Factor;
extern int32_t GPS_D_Factor;
extern uint8_t GPS_P_Factor;
extern uint8_t GPS_I_Factor;
extern uint8_t GPS_D_Factor;
 
extern void GPS_Main(uint8_t ctrl);
extern void GPS_SetHomePosition(void);