WebSVN - NaviCtrl - Diff - Rev 152 and 159



                if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
                else
                {
                        if(Parameter.NaviGpsModeControl <  50)
                        {
                                if(FlightMode_Old == GPS_FLIGHT_MODE_FREE) SetDelay(SWITCH_DELAY);
                                if(CheckDelay(SwitchDelay))
                                {
                                        GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
                                        NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH);
                                        NCFlags |= NC_FLAG_FREE;
                                }
                        }

                                                GPS_CopyPosition(&GPS_HomePosition, &(NaviData.HomePosition));
                                        }
                                        GPS_pWaypoint = WPList_Begin(); // go to start of waypoint list, return NULL of the list is empty
                                        if(GPS_pWaypoint != NULL) // if new WP exist
                                        {   // update WP hold time stamp immediately!
                                                WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp
                                                NCFlags &= ~NC_FLAG_TARGET_REACHED;
                                        }
                                }

 

                                                        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)) );
                                                        }*/
                                                        // set target position
                                                        GPS_pTargetPosition = &GPS_HoldPosition;
                                                        GPS_TargetRadius = 100; // 1 meter
                                                }
                                                break;

                                                                if(GPS_pWaypoint->Position.Status == INVALID) // should never happen
                                                                {
                                                                        GPS_pWaypoint = WPList_Next(); // goto to next WP
                                                                        if(GPS_pWaypoint != NULL) // if new WP exist
                                                                        {   // update WP hold time stamp immediately!
                                                                                WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp
                                                                                NCFlags &= ~NC_FLAG_TARGET_REACHED;
                                                                        }
                                                                        BeepTime = 255;
                                                                }
                                                                else // waypoint position is valid

                                                                                {
                                                                                        GPS_pWaypoint = WPList_Next(); // goto to next waypoint, return NULL if end of list has been reached
                                                                                        if(GPS_pWaypoint == NULL) GPS_pWaypoint = WPList_End(); // goto last WP if next one not exist
                                                                                        if(GPS_pWaypoint != NULL) // if new WP exist
                                                                                        {   // update WP hold time stamp immediately!
                                                                                                WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp
                                                                                                NCFlags &= ~NC_FLAG_TARGET_REACHED;
                                                                                        }
                                                                                }
                                                                        } // EOF target reached
                                                                        else
                                                                        {
                                                                                WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp
                                                                        }
                                                                }
                                                        }
                                                        else // pointer to waypoint does not exist
                                                        {
                                                                // try to catch the first waypoint from the list
                                                                GPS_pWaypoint = WPList_Begin();
                                                                if(GPS_pWaypoint != NULL) // if new WP exist
                                                                {   // update WP hold time stamp immediately!
                                                                        WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp
                                                                        NCFlags &= ~NC_FLAG_TARGET_REACHED;
                                                                }
                                                        }
                                                        // EOF waypoint trigger logic

 
                                                        if(GPS_pWaypoint != NULL) // Waypoint exist
                                                        {
                                                                // possible new data have been put into wp-list
                                                                if(GPS_pWaypoint->Position.Status == NEWDATA)
                                                                {
                                                                        WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // update hold time stamp
                                                                        NCFlags &= ~NC_FLAG_TARGET_REACHED;
                                                                }
                                                                // update the hold position
                                                                GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
                                                                // set target to the waypoint
                                                                GPS_pTargetPosition = &(GPS_pWaypoint->Position);
                                                                // update target radius

                                                                GPS_TargetRadius = (s32)(GPS_pWaypoint->ToleranceRadius) * 100L;
 
                                                        }

Subversion Repositories NaviCtrl

(root)/tags/V2.06b/GPS.c @ 647 - Rev 152 → 159

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 81.11cm 5Hz = 8.88 = 44.4cm/s an max 1271.11cm 5Hz = 705cm/s	518	// of min 81.11cm 5Hz = 8.88 = 44.4cm/s an max 1271.11cm 5Hz = 705cm/s
519	GPS_HoldPosition.Latitude += (s32)(cos_hFC.StickNick + sin_hFC.StickRoll);	519	GPS_HoldPosition.Latitude += (s32)(cos_hFC.StickNick + sin_hFC.StickRoll);
520	GPS_HoldPosition.Longitude += (s32)((sin_hFC.StickNick - cos_hFC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) );	520	GPS_HoldPosition.Longitude += (s32)((sin_hFC.StickNick - cos_hFC.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	}