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