10,10 → 10,10 |
#define TSK_HOLD 1 |
#define TSK_HOME 2 |
|
#define GPS_STICK_SENSE 12 |
#define GPS_STICK_LIMIT 45 |
#define GPS_D_LIMIT_DIST 500 // for position deviations grater than 500 cm (5m) the D-Part of the PD-Controller is limited |
#define GPS_D_LIMIT 50 // PD-Controntroller D-Limit. |
#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_D_LIMIT_DIST 2000 // for position deviations grater than 500 cm (20m) the D-Part of the PD-Controller is limited |
#define GPS_D_LIMIT 50 // PD-Controntroller D-Limit. |
|
int16_t GPS_Pitch = 0; |
int16_t GPS_Roll = 0; |
75,6 → 75,9 |
DebugOut.Analog[12] = GPSPosDev_North; |
DebugOut.Analog[13] = GPSPosDev_East; |
|
//DebugOut.Analog[12] = GPSInfo.velnorth; |
//DebugOut.Analog[13] = GPSInfo.veleast; |
|
//Calculate PD-components of the controller (negative sign for compensation) |
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
212,6 → 215,10 |
{ |
GPS_PDController(&HomePosition); |
} |
else // manual control |
{ |
GPS_Neutral(); |
} |
} |
else // bad home position |
{ |
222,6 → 229,10 |
{ |
GPS_PDController(&HoldPosition); |
} |
else // manual control or no rteference position |
{ |
GPS_Neutral(); |
} |
} |
break; // eof TSK_HOME |
default: // unhandled task |
235,9 → 246,9 |
GPS_Neutral(); |
if(GPS_Task != TSK_IDLE) |
{ |
satbeep = 2048 - (int16_t)GPSInfo.satnum * 256; // is zero at 8 sats |
satbeep = 1800 - (int16_t)GPSInfo.satnum * 225; // is zero at 8 sats |
if (satbeep < 0) satbeep = 0; |
BeepTime = 50 + (uint16_t)satbeep; |
BeepTime = 50 + (uint16_t)satbeep; // max 1850 * 0.1 ms = |
} |
} |
// set current data as processed to avoid further calculations on the same gps data |