93,7 → 93,7 |
float P; |
float I; |
float D; |
float A; |
float A; |
float ACC; |
s32 P_Limit; |
s32 I_Limit; |
111,7 → 111,7 |
typedef struct |
{ |
u8 Status; // invalid, newdata, processed |
s32 North; // in cm |
s32 North; // in cm |
s32 East; // in cm |
s32 Bearing; // in deg |
s32 Distance; // in cm |
200,7 → 200,11 |
GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm |
} |
// FlightMode changed? |
if(GPS_Parameter.FlightMode != FlightMode_Old) BeepTime = 100; // beep to indicate that mode has switched |
if(GPS_Parameter.FlightMode != FlightMode_Old) |
{ |
BeepTime = 100; // beep to indicate that mode has been switched |
NCFlags &= ~NC_FLAG_TARGET_REACHED; |
} |
FlightMode_Old = GPS_Parameter.FlightMode; |
} |
|
225,7 → 229,7 |
{ |
NCFlags &= ~NC_FLAG_MANUAL_CONTROL; |
return(0); |
} |
} |
} |
|
//------------------------------------------------------------ |
309,7 → 313,7 |
dist = (s32)hypot(*x,*y); // the length of the vector |
if (dist > limit) |
// if vector length is larger than the given limit |
{ // scale vector compontents so that the length is cut off to limit |
{ // scale vector compontents so that the length is cut off to limit |
*x = (s32)(( (double)(*x) * (double)limit ) / (double)dist); |
*y = (s32)(( (double)(*y) * (double)limit ) / (double)dist); |
dist = limit; |
447,7 → 451,7 |
/* The selected flight mode influences the target position pointer and therefore the behavior */ |
|
// check for current flight mode and set the target pointer GPS_pTargetPosition respectively |
switch(GPS_Parameter.FlightMode) |
switch(GPS_Parameter.FlightMode) |
{ |
// the GPS control is deactived |
case GPS_FLIGHT_MODE_FREE: |
595,7 → 599,7 |
RangedTargetPosition.Longitude = GPS_HomePosition.Longitude; |
RangedTargetPosition.Longitude += (s32)((float)TargetHomeDeviation.East / (1.11194927f * cos(RadiansFromGPS(GPS_HomePosition.Latitude))) ); |
RangedTargetPosition.Altitude = GPS_pTargetPosition->Altitude; |
RangedTargetPosition.Status = NEWDATA; |
RangedTargetPosition.Status = NEWDATA; |
} |
else |
{ // the target is located within the operation radius area |
633,6 → 637,7 |
else // deviation could not be calculated |
{ // do nothing on gps sticks! |
GPS_Neutral(); |
NCFlags &= ~NC_FLAG_TARGET_REACHED; // clear target reached |
} |
|
}// eof if GPSSignal is OK |
646,7 → 651,7 |
else if (GPSData.NumOfSats < GPS_Parameter.MinSat && !(beep_rythm % 5)) BeepTime = 10; |
} |
} |
GPSData.Status = PROCESSED; // mark as processed |
GPSData.Status = PROCESSED; // mark as processed |
break; |
} |
|
678,6 → 683,6 |
NaviData.TopSpeed = (s16)GPSData.Speed_Top; // in cm/s |
NaviData.TargetHoldTime = (u8)(GetDelay(WPTime)/1000); // in s |
//+++++++++++++++++++++++++++++++++++++++++++++++++++ |
return; |
return; |
} |
|