WebSVN - NaviCtrl - Diff - Rev 73 and 74



                        // If GPS signal condition is sufficient for a reliable position measurement
                        if(GPS_IsSignalOK())
                        {
                                // update home deviation info
                                GPS_CalculateDeviation(&(GPSData.Position), &GPS_HomePosition, &CurrentHomeDeviation);
 
                                // if the MK is starting or the home position is invalid then store the home position
                                if((FC.MKFlags & MKFLAG_START) || (GPS_HomePosition.Status == INVALID))
                                {       // try to update the home position from the current position
                                        if(GPS_CopyPosition(&(GPSData.Position), &GPS_HomePosition))
                                        {

                                        // the GPS control is directed to a target position
                                        // given by a waypoint or by the home position
                                        case GPS_FLIGHT_MODE_WAYPOINT:
                                                NaviData.NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH);
                                                NaviData.NCFlags |= NC_FLAG_CH;
 
                                                if(GPS_IsManuallyControlled()) // the human pilot takes the action
                                                {
                                                        GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active
                                                    GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);  // update hold position
                                                        GPS_pTargetPosition = NULL;     // set target position invalid

                                                                        {
                                                                                GPS_pWaypointOld = GPS_pWaypoint;
                                                                                // reset the arrived bit to break a pending HoldTime of the old WP
                                                                                WPArrived = FALSE;
                                                                        }
 
                                                                        if(CurrentTargetDeviation.Status != INVALID)
                                                                        {       // if the waypoint was not catched and the target area has been reached
                                                                                if(!WPArrived && (CurrentTargetDeviation.Distance < (GPS_pWaypoint->ToleranceRadius * 100)))
                                                                                {
                                                                                        WPArrived = TRUE;

                                                                                        WPArrived = FALSE; // which is not arrived
                                                                                }
                                                                        } // EOF if(WPArrived)
                                                                }
                                                        } // EOF waypoint trigger logic
 
                                                        if(GPS_pWaypoint != NULL) // Waypoint exist
                                                        {
                                                                // update the hold position
                                                                GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
                                                                GPS_pTargetPosition = &(GPS_pWaypoint->Position);

        NaviData.TargetPositionDeviation.Bearing  = CurrentTargetDeviation.Bearing;
        NaviData.HomePositionDeviation.Distance   = CurrentHomeDeviation.Distance;
        NaviData.HomePositionDeviation.Bearing    = CurrentHomeDeviation.Bearing;
        NaviData.UBat = FC.UBat;
        NaviData.GroundSpeed = (u16)GPSData.Speed_Ground;
        NaviData.Heading = (s16)(GPSData.Heading/100000L);
        NaviData.CompassHeading = (s16)FromFlightCtrl.GyroHeading/10; // in deg
        NaviData.AngleNick = FromFlightCtrl.AngleNick / 10;        // in deg
        NaviData.AngleRoll = FromFlightCtrl.AngleRoll / 10;        // in deg
        NaviData.RC_Quality = (u8) FC.RC_Quality;
        NaviData.MKFlags = (u8)FC.MKFlags;
        NaviData.OperatingRadius = Parameter.NaviOperatingRadius;

 
        //+++++++++++++++++++++++++++++++++++++++++++++++++++
  return;

Subversion Repositories NaviCtrl

(root)/tags/V0.24c/GPS.c @ 309 - Rev 73 → 74

Rev 73		Rev 74
Line 405...		Line 405...
405	// If GPS signal condition is sufficient for a reliable position measurement	405	// If GPS signal condition is sufficient for a reliable position measurement
406	if(GPS_IsSignalOK())	406	if(GPS_IsSignalOK())
407	{	407	{
408	// update home deviation info	408	// update home deviation info
409	GPS_CalculateDeviation(&(GPSData.Position), &GPS_HomePosition, &CurrentHomeDeviation);	409	GPS_CalculateDeviation(&(GPSData.Position), &GPS_HomePosition, &CurrentHomeDeviation);
410		410
411	// if the MK is starting or the home position is invalid then store the home position	411	// if the MK is starting or the home position is invalid then store the home position
412	if((FC.MKFlags & MKFLAG_START) \|\| (GPS_HomePosition.Status == INVALID))	412	if((FC.MKFlags & MKFLAG_START) \|\| (GPS_HomePosition.Status == INVALID))
413	{ // try to update the home position from the current position	413	{ // try to update the home position from the current position
414	if(GPS_CopyPosition(&(GPSData.Position), &GPS_HomePosition))	414	if(GPS_CopyPosition(&(GPSData.Position), &GPS_HomePosition))
415	{	415	{
Line 457...		Line 457...
457	// the GPS control is directed to a target position	457	// the GPS control is directed to a target position
458	// given by a waypoint or by the home position	458	// given by a waypoint or by the home position
459	case GPS_FLIGHT_MODE_WAYPOINT:	459	case GPS_FLIGHT_MODE_WAYPOINT:
460	NaviData.NCFlags &= ~(NC_FLAG_FREE \| NC_FLAG_PH);	460	NaviData.NCFlags &= ~(NC_FLAG_FREE \| NC_FLAG_PH);
461	NaviData.NCFlags \|= NC_FLAG_CH;	461	NaviData.NCFlags \|= NC_FLAG_CH;
462		462
463	if(GPS_IsManuallyControlled()) // the human pilot takes the action	463	if(GPS_IsManuallyControlled()) // the human pilot takes the action
464	{	464	{
465	GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active	465	GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active
466	GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); // update hold position	466	GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); // update hold position
467	GPS_pTargetPosition = NULL; // set target position invalid	467	GPS_pTargetPosition = NULL; // set target position invalid
Line 484...		Line 484...
484	{	484	{
485	GPS_pWaypointOld = GPS_pWaypoint;	485	GPS_pWaypointOld = GPS_pWaypoint;
486	// reset the arrived bit to break a pending HoldTime of the old WP	486	// reset the arrived bit to break a pending HoldTime of the old WP
487	WPArrived = FALSE;	487	WPArrived = FALSE;
488	}	488	}
489		489
490	if(CurrentTargetDeviation.Status != INVALID)	490	if(CurrentTargetDeviation.Status != INVALID)
491	{ // if the waypoint was not catched and the target area has been reached	491	{ // if the waypoint was not catched and the target area has been reached
492	if(!WPArrived && (CurrentTargetDeviation.Distance < (GPS_pWaypoint->ToleranceRadius * 100)))	492	if(!WPArrived && (CurrentTargetDeviation.Distance < (GPS_pWaypoint->ToleranceRadius * 100)))
493	{	493	{
494	WPArrived = TRUE;	494	WPArrived = TRUE;
Line 505...		Line 505...
505	WPArrived = FALSE; // which is not arrived	505	WPArrived = FALSE; // which is not arrived
506	}	506	}
507	} // EOF if(WPArrived)	507	} // EOF if(WPArrived)
508	}	508	}
509	} // EOF waypoint trigger logic	509	} // EOF waypoint trigger logic
510		510
511	if(GPS_pWaypoint != NULL) // Waypoint exist	511	if(GPS_pWaypoint != NULL) // Waypoint exist
512	{	512	{
513	// update the hold position	513	// update the hold position
514	GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);	514	GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
515	GPS_pTargetPosition = &(GPS_pWaypoint->Position);	515	GPS_pTargetPosition = &(GPS_pWaypoint->Position);
Line 640...		Line 640...
640	NaviData.TargetPositionDeviation.Bearing = CurrentTargetDeviation.Bearing;	640	NaviData.TargetPositionDeviation.Bearing = CurrentTargetDeviation.Bearing;
641	NaviData.HomePositionDeviation.Distance = CurrentHomeDeviation.Distance;	641	NaviData.HomePositionDeviation.Distance = CurrentHomeDeviation.Distance;
642	NaviData.HomePositionDeviation.Bearing = CurrentHomeDeviation.Bearing;	642	NaviData.HomePositionDeviation.Bearing = CurrentHomeDeviation.Bearing;
643	NaviData.UBat = FC.UBat;	643	NaviData.UBat = FC.UBat;
644	NaviData.GroundSpeed = (u16)GPSData.Speed_Ground;	644	NaviData.GroundSpeed = (u16)GPSData.Speed_Ground;
645	NaviData.Heading = (s16)GPSData.Heading;	645	NaviData.Heading = (s16)(GPSData.Heading/100000L);
646	NaviData.CompassHeading = (s16)FromFlightCtrl.GyroHeading/10; // in deg	646	NaviData.CompassHeading = (s16)FromFlightCtrl.GyroHeading/10; // in deg
647	NaviData.AngleNick = FromFlightCtrl.AngleNick / 10; // in deg	647	NaviData.AngleNick = FromFlightCtrl.AngleNick / 10; // in deg
648	NaviData.AngleRoll = FromFlightCtrl.AngleRoll / 10; // in deg	648	NaviData.AngleRoll = FromFlightCtrl.AngleRoll / 10; // in deg
649	NaviData.RC_Quality = (u8) FC.RC_Quality;	649	NaviData.RC_Quality = (u8) FC.RC_Quality;
650	NaviData.MKFlags = (u8)FC.MKFlags;	650	NaviData.MKFlags = (u8)FC.MKFlags;
651	NaviData.OperatingRadius = Parameter.NaviOperatingRadius;	651	NaviData.OperatingRadius = Parameter.NaviOperatingRadius;
Line 652...		Line 652...
652		652
653	//+++++++++++++++++++++++++++++++++++++++++++++++++++	653	//+++++++++++++++++++++++++++++++++++++++++++++++++++
654	return;	654	return;