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