Rev 152 | Rev 171 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 152 | Rev 159 | ||
---|---|---|---|
Line 166... | Line 166... | ||
166 | if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
166 | if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
167 | else |
167 | else |
168 | { |
168 | { |
169 | if(Parameter.NaviGpsModeControl < 50) |
169 | if(Parameter.NaviGpsModeControl < 50) |
170 | { |
170 | { |
171 | if(FlightMode_Old == GPS_FLIGHT_MODE_FREE) SetDelay(SWITCH_DELAY); |
171 | if(FlightMode_Old == GPS_FLIGHT_MODE_FREE) SetDelay(SWITCH_DELAY); |
172 | if(CheckDelay(SwitchDelay)) |
172 | if(CheckDelay(SwitchDelay)) |
173 | { |
173 | { |
174 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
174 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
175 | NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH); |
175 | NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH); |
176 | NCFlags |= NC_FLAG_FREE; |
176 | NCFlags |= NC_FLAG_FREE; |
177 | } |
177 | } |
178 | } |
178 | } |
Line 473... | Line 473... | ||
473 | GPS_CopyPosition(&GPS_HomePosition, &(NaviData.HomePosition)); |
473 | GPS_CopyPosition(&GPS_HomePosition, &(NaviData.HomePosition)); |
474 | } |
474 | } |
475 | GPS_pWaypoint = WPList_Begin(); // go to start of waypoint list, return NULL of the list is empty |
475 | GPS_pWaypoint = WPList_Begin(); // go to start of waypoint list, return NULL of the list is empty |
476 | if(GPS_pWaypoint != NULL) // if new WP exist |
476 | if(GPS_pWaypoint != NULL) // if new WP exist |
477 | { // update WP hold time stamp immediately! |
477 | { // update WP hold time stamp immediately! |
478 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
478 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
479 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
479 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
480 | } |
480 | } |
481 | } |
481 | } |
Line 482... | Line 482... | ||
482 | 482 | ||
Line 506... | Line 506... | ||
506 | GPS_pTargetPosition = NULL; |
506 | GPS_pTargetPosition = NULL; |
507 | GPS_TargetRadius = 0; |
507 | GPS_TargetRadius = 0; |
508 | } |
508 | } |
509 | else |
509 | else |
510 | { |
510 | { |
511 | /* |
511 | /* |
512 | #define PH_MOVE_THRESHOLD 8 |
512 | #define PH_MOVE_THRESHOLD 8 |
513 | if( ((abs(FC.StickNick) > PH_MOVE_THRESHOLD) || (abs(FC.StickRoll) > PH_MOVE_THRESHOLD) ) && (FC.RC_Quality > 150)) |
513 | if( ((abs(FC.StickNick) > PH_MOVE_THRESHOLD) || (abs(FC.StickRoll) > PH_MOVE_THRESHOLD) ) && (FC.RC_Quality > 150)) |
514 | { // indirect control by moving the hold position with rc sticks |
514 | { // indirect control by moving the hold position with rc sticks |
515 | // rc sticks have a range of +/-127 counts |
515 | // rc sticks have a range of +/-127 counts |
516 | // GPS Coordinates are in 1-E7 deg, i.e. 1 counts is 1E-7°/360°*40E+8cm = 1.11 cm |
516 | // GPS Coordinates are in 1-E7 deg, i.e. 1 counts is 1E-7°/360°*40E+8cm = 1.11 cm |
517 | // normal manual activtion threshold is 8 counts that results in a change of the hold position |
517 | // normal manual activtion threshold is 8 counts that results in a change of the hold position |
518 | // of min 8*1.11cm * 5Hz = 8.88 = 44.4cm/s an max 127*1.11cm *5Hz = 705cm/s |
518 | // of min 8*1.11cm * 5Hz = 8.88 = 44.4cm/s an max 127*1.11cm *5Hz = 705cm/s |
519 | GPS_HoldPosition.Latitude += (s32)(cos_h*FC.StickNick + sin_h*FC.StickRoll); |
519 | GPS_HoldPosition.Latitude += (s32)(cos_h*FC.StickNick + sin_h*FC.StickRoll); |
520 | GPS_HoldPosition.Longitude += (s32)((sin_h*FC.StickNick - cos_h*FC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) ); |
520 | GPS_HoldPosition.Longitude += (s32)((sin_h*FC.StickNick - cos_h*FC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) ); |
521 | }*/ |
521 | }*/ |
522 | // set target position |
522 | // set target position |
523 | GPS_pTargetPosition = &GPS_HoldPosition; |
523 | GPS_pTargetPosition = &GPS_HoldPosition; |
524 | GPS_TargetRadius = 100; // 1 meter |
524 | GPS_TargetRadius = 100; // 1 meter |
525 | } |
525 | } |
526 | break; |
526 | break; |
Line 544... | Line 544... | ||
544 | if(GPS_pWaypoint->Position.Status == INVALID) // should never happen |
544 | if(GPS_pWaypoint->Position.Status == INVALID) // should never happen |
545 | { |
545 | { |
546 | GPS_pWaypoint = WPList_Next(); // goto to next WP |
546 | GPS_pWaypoint = WPList_Next(); // goto to next WP |
547 | if(GPS_pWaypoint != NULL) // if new WP exist |
547 | if(GPS_pWaypoint != NULL) // if new WP exist |
548 | { // update WP hold time stamp immediately! |
548 | { // update WP hold time stamp immediately! |
549 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
549 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
550 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
550 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
551 | } |
551 | } |
552 | BeepTime = 255; |
552 | BeepTime = 255; |
553 | } |
553 | } |
554 | else // waypoint position is valid |
554 | else // waypoint position is valid |
Line 561... | Line 561... | ||
561 | { |
561 | { |
562 | GPS_pWaypoint = WPList_Next(); // goto to next waypoint, return NULL if end of list has been reached |
562 | GPS_pWaypoint = WPList_Next(); // goto to next waypoint, return NULL if end of list has been reached |
563 | if(GPS_pWaypoint == NULL) GPS_pWaypoint = WPList_End(); // goto last WP if next one not exist |
563 | if(GPS_pWaypoint == NULL) GPS_pWaypoint = WPList_End(); // goto last WP if next one not exist |
564 | if(GPS_pWaypoint != NULL) // if new WP exist |
564 | if(GPS_pWaypoint != NULL) // if new WP exist |
565 | { // update WP hold time stamp immediately! |
565 | { // update WP hold time stamp immediately! |
566 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
566 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
567 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
567 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
568 | } |
568 | } |
569 | } |
569 | } |
570 | } // EOF target reached |
570 | } // EOF target reached |
571 | else |
571 | else |
572 | { |
572 | { |
573 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp |
573 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp |
574 | } |
574 | } |
575 | } |
575 | } |
576 | } |
576 | } |
577 | else // pointer to waypoint does not exist |
577 | else // pointer to waypoint does not exist |
578 | { |
578 | { |
579 | // try to catch the first waypoint from the list |
579 | // try to catch the first waypoint from the list |
580 | GPS_pWaypoint = WPList_Begin(); |
580 | GPS_pWaypoint = WPList_Begin(); |
581 | if(GPS_pWaypoint != NULL) // if new WP exist |
581 | if(GPS_pWaypoint != NULL) // if new WP exist |
582 | { // update WP hold time stamp immediately! |
582 | { // update WP hold time stamp immediately! |
583 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
583 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
584 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
584 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
585 | } |
585 | } |
586 | } |
586 | } |
587 | // EOF waypoint trigger logic |
587 | // EOF waypoint trigger logic |
Line 588... | Line 588... | ||
588 | 588 | ||
589 | if(GPS_pWaypoint != NULL) // Waypoint exist |
589 | if(GPS_pWaypoint != NULL) // Waypoint exist |
- | 590 | { |
|
- | 591 | // possible new data have been put into wp-list |
|
- | 592 | if(GPS_pWaypoint->Position.Status == NEWDATA) |
|
- | 593 | { |
|
- | 594 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp |
|
- | 595 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
|
590 | { |
596 | } |
591 | // update the hold position |
597 | // update the hold position |
- | 598 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
|
592 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
599 | // set target to the waypoint |
- | 600 | GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
|
593 | GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
601 | // update target radius |
Line 594... | Line 602... | ||
594 | GPS_TargetRadius = (s32)(GPS_pWaypoint->ToleranceRadius) * 100L; |
602 | GPS_TargetRadius = (s32)(GPS_pWaypoint->ToleranceRadius) * 100L; |
595 | 603 | ||
596 | } |
604 | } |