Rev 128 | Rev 130 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 128 | Rev 129 | ||
|---|---|---|---|
| Line 392... | Line 392... | ||
| 392 | //------------------------------------------------------------ |
392 | //------------------------------------------------------------ |
| 393 | void GPS_Navigation(void) |
393 | void GPS_Navigation(void) |
| 394 | { |
394 | { |
| 395 | static u32 beep_rythm; |
395 | static u32 beep_rythm; |
| 396 | static u32 GPSDataTimeout = 0; |
396 | static u32 GPSDataTimeout = 0; |
| - | 397 | float compassheading, sin_h, cos_h; |
|
| Line 397... | Line 398... | ||
| 397 | 398 | ||
| 398 | // pointer to current target position |
399 | // pointer to current target position |
| 399 | static GPS_Pos_t * pTargetPositionOld = NULL; |
400 | static GPS_Pos_t * pTargetPositionOld = NULL; |
| Line 400... | Line 401... | ||
| 400 | static Waypoint_t* GPS_pWaypointOld = NULL; |
401 | static Waypoint_t* GPS_pWaypointOld = NULL; |
| 401 | 402 | ||
| 402 | static GPS_Pos_t RangedTargetPosition = {0,0,0, INVALID}; // the limited target position, this is derived from the target position with repect to the operating radius |
403 | static GPS_Pos_t RangedTargetPosition = {0,0,0, INVALID}; // the limited target position, this is derived from the target position with repect to the operating radius |
| Line -... | Line 404... | ||
| - | 404 | static s32 OperatingRadiusOld = -1; |
|
| - | 405 | static u32 WPTime = 0; |
|
| - | 406 | ||
| - | 407 | ||
| - | 408 | ||
| - | 409 | // get current heading from the FC gyro compass heading |
|
| - | 410 | if(abs(FC.StickYaw) > 20 || FromFlightCtrl.GyroHeading > 3600) compassheading = (float)I2C_Heading.Heading * M_PI_180; |
|
| - | 411 | else compassheading = ((float)FromFlightCtrl.GyroHeading * M_PI_180) / 10.0; |
|
| - | 412 | ||
| - | 413 | sin_h = sin(compassheading); |
|
| 403 | static s32 OperatingRadiusOld = -1; |
414 | cos_h = cos(compassheading); |
| 404 | static u32 WPTime = 0; |
415 | |
| 405 | 416 | ||
| 406 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
417 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 407 | //+ Check for new data from GPS-receiver |
418 | //+ Check for new data from GPS-receiver |
| Line 470... | Line 481... | ||
| 470 | case GPS_FLIGHT_MODE_AID: |
481 | case GPS_FLIGHT_MODE_AID: |
| Line 471... | Line 482... | ||
| 471 | 482 | ||
| 472 | if(GPS_IsManuallyControlled()) |
483 | if(GPS_IsManuallyControlled()) |
| 473 | { |
484 | { |
| 474 | GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active |
485 | GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active |
| 475 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
486 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
| 476 | GPS_pTargetPosition = NULL; |
487 | GPS_pTargetPosition = NULL; |
| 477 | GPS_TargetRadius = 0; |
488 | GPS_TargetRadius = 0; |
| 478 | } |
489 | } |
| 479 | else |
490 | else |
| - | 491 | { |
|
| - | 492 | #define PH_MOVE_THRESHOLD 8 |
|
| - | 493 | if( ((abs(FC.StickNick) > PH_MOVE_THRESHOLD) || (abs(FC.StickRoll) > PH_MOVE_THRESHOLD) ) && (FC.RC_Quality > 150)) |
|
| - | 494 | { // indirect control by moving the hold position with rc sticks |
|
| - | 495 | // rc sticks have a range of +/-127 counts |
|
| - | 496 | // GPS Coordinates are in 1-E7 deg, i.e. 1 counts is 1E-7°/360°*40E+8cm = 1.11 cm |
|
| - | 497 | // normal manual activtion threshold is 8 counts that results in a change of the hold position |
|
| - | 498 | // of min 8*1.11cm * 5Hz = 8.88 = 44.4cm/s an max 127*1.11cm *5Hz = 705cm/s |
|
| - | 499 | GPS_HoldPosition.Latitude += (s32)(cos_h*FC.StickNick + sin_h*FC.StickRoll); |
|
| - | 500 | GPS_HoldPosition.Longitude += (s32)((sin_h*FC.StickNick - cos_h*FC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) ); |
|
| - | 501 | GPS_HoldPosition.Status = NEWDATA; |
|
| 480 | { |
502 | } |
| 481 | GPS_pTargetPosition = &GPS_HoldPosition; |
503 | GPS_pTargetPosition = &GPS_HoldPosition; |
| 482 | GPS_TargetRadius = 100; // 1 meter |
504 | GPS_TargetRadius = 100; // 1 meter |
| 483 | } |
505 | } |