394,6 → 394,7 |
{ |
static u32 beep_rythm; |
static u32 GPSDataTimeout = 0; |
float compassheading, sin_h, cos_h; |
|
// pointer to current target position |
static GPS_Pos_t * pTargetPositionOld = NULL; |
403,6 → 404,16 |
static s32 OperatingRadiusOld = -1; |
static u32 WPTime = 0; |
|
|
|
// get current heading from the FC gyro compass heading |
if(abs(FC.StickYaw) > 20 || FromFlightCtrl.GyroHeading > 3600) compassheading = (float)I2C_Heading.Heading * M_PI_180; |
else compassheading = ((float)FromFlightCtrl.GyroHeading * M_PI_180) / 10.0; |
|
sin_h = sin(compassheading); |
cos_h = cos(compassheading); |
|
|
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Check for new data from GPS-receiver |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
472,12 → 483,23 |
if(GPS_IsManuallyControlled()) |
{ |
GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active |
GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
GPS_pTargetPosition = NULL; |
GPS_TargetRadius = 0; |
} |
else |
{ |
#define PH_MOVE_THRESHOLD 8 |
if( ((abs(FC.StickNick) > PH_MOVE_THRESHOLD) || (abs(FC.StickRoll) > PH_MOVE_THRESHOLD) ) && (FC.RC_Quality > 150)) |
{ // indirect control by moving the hold position with rc sticks |
// rc sticks have a range of +/-127 counts |
// GPS Coordinates are in 1-E7 deg, i.e. 1 counts is 1E-7°/360°*40E+8cm = 1.11 cm |
// normal manual activtion threshold is 8 counts that results in a change of the hold position |
// of min 8*1.11cm * 5Hz = 8.88 = 44.4cm/s an max 127*1.11cm *5Hz = 705cm/s |
GPS_HoldPosition.Latitude += (s32)(cos_h*FC.StickNick + sin_h*FC.StickRoll); |
GPS_HoldPosition.Longitude += (s32)((sin_h*FC.StickNick - cos_h*FC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) ); |
GPS_HoldPosition.Status = NEWDATA; |
} |
GPS_pTargetPosition = &GPS_HoldPosition; |
GPS_TargetRadius = 100; // 1 meter |
} |