WebSVN - NaviCtrl - Diff - Rev 102 and 103



{
        float Gain;
        float P;
        float I;
        float D;
        float A;
        float ACC;
        s32 P_Limit;
        s32 I_Limit;
        s32 D_Limit;
        s32 PID_Limit;

} __attribute__((packed)) GPS_Parameter_t;

 
typedef struct
{
        u8  Status; // invalid, newdata, processed
        s32 North;              // in cm
        s32 East;               // in cm
        s32 Bearing;    // in deg
        s32 Distance;   // in cm
} __attribute__((packed)) GPS_Deviation_t;

                GPS_Parameter.StickThreshold = (s8)Parameter.NaviStickThreshold;
                GPS_Parameter.WindCorrection = (float)Parameter.NaviWindCorrection;
                GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm
        }
        // FlightMode changed?
        if(GPS_Parameter.FlightMode != FlightMode_Old)
        {
                BeepTime = 100; // beep to indicate that mode has been switched
                NCFlags &= ~NC_FLAG_TARGET_REACHED;
        }
        FlightMode_Old = GPS_Parameter.FlightMode;
}

 
//-------------------------------------------------------------

        }
        else
        {
                NCFlags &= ~NC_FLAG_MANUAL_CONTROL;
                return(0);
        }
}

 
//------------------------------------------------------------
// copy GPS position from source position to target position

        s32 dist;

 
        dist = (s32)hypot(*x,*y);       // the length of the vector
        if (dist > limit)
        // if vector length is larger than the given limit
        {       // scale vector compontents so that the length is cut off to limit
                *x = (s32)(( (double)(*x) * (double)limit ) / (double)dist);
                *y = (s32)(( (double)(*y) * (double)limit ) / (double)dist);
                dist = limit;
        }

                                }

 

                                /* The selected flight mode influences the target position pointer and therefore the behavior */
 
                                // check for current flight mode and set the target pointer GPS_pTargetPosition respectively
                                switch(GPS_Parameter.FlightMode)
                                {

                                        // the GPS control is deactived

                                                                RangedTargetPosition.Latitude = GPS_HomePosition.Latitude;
                                                                RangedTargetPosition.Latitude += (s32)((float)TargetHomeDeviation.North / 1.11194927f);
                                                                RangedTargetPosition.Longitude = GPS_HomePosition.Longitude;
                                                                RangedTargetPosition.Longitude += (s32)((float)TargetHomeDeviation.East / (1.11194927f * cos(RadiansFromGPS(GPS_HomePosition.Latitude))) );
                                                                RangedTargetPosition.Altitude = GPS_pTargetPosition->Altitude;
                                                                RangedTargetPosition.Status = NEWDATA;
                                                        }
                                                        else
                                                        {       // the target is located within the operation radius area
                                                                // simple copy the loaction to the ranged target position
                                                                GPS_CopyPosition(GPS_pTargetPosition, &RangedTargetPosition);

                                        GPS_Stick.Yaw  = 0;
                                }
                                else // deviation could not be calculated
                                {   // do nothing on gps sticks!
                                        GPS_Neutral();
                                        NCFlags &= ~NC_FLAG_TARGET_REACHED;     // clear target reached
                                }

 
                        }// eof if GPSSignal is OK
                        else // GPSSignal not OK

                                {
                                        if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;
                                        else if (GPSData.NumOfSats < GPS_Parameter.MinSat && !(beep_rythm % 5)) BeepTime = 10;
                                }
                        }
                        GPSData.Status = PROCESSED; // mark as processed
                        break;
        }

 
        DebugOut.Analog[6] = NCFlags;

        NaviData.NCFlags = NCFlags;
        NaviData.OperatingRadius = Parameter.NaviOperatingRadius;
        NaviData.TopSpeed = (s16)GPSData.Speed_Top;  // in cm/s
        NaviData.TargetHoldTime = (u8)(GetDelay(WPTime)/1000); // in s
        //+++++++++++++++++++++++++++++++++++++++++++++++++++
        return;
}

Rev 102	Rev 103
Line 91...	Line 91...
91	{	91	{
92	float Gain;	92	float Gain;
93	float P;	93	float P;
94	float I;	94	float I;
95	float D;	95	float D;
96	float A;	96	float A;
97	float ACC;	97	float ACC;
98	s32 P_Limit;	98	s32 P_Limit;
99	s32 I_Limit;	99	s32 I_Limit;
100	s32 D_Limit;	100	s32 D_Limit;
101	s32 PID_Limit;	101	s32 PID_Limit;
Line 109...	Line 109...
109	} __attribute__((packed)) GPS_Parameter_t;	109	} __attribute__((packed)) GPS_Parameter_t;
Line 110...	Line 110...
110		110
111	typedef struct	111	typedef struct
112	{	112	{
113	u8 Status; // invalid, newdata, processed	113	u8 Status; // invalid, newdata, processed
114	s32 North; // in cm	114	s32 North; // in cm
115	s32 East; // in cm	115	s32 East; // in cm
116	s32 Bearing; // in deg	116	s32 Bearing; // in deg
117	s32 Distance; // in cm	117	s32 Distance; // in cm
118	} __attribute__((packed)) GPS_Deviation_t;	118	} __attribute__((packed)) GPS_Deviation_t;
Line 198...	Line 198...
198	GPS_Parameter.StickThreshold = (s8)Parameter.NaviStickThreshold;	198	GPS_Parameter.StickThreshold = (s8)Parameter.NaviStickThreshold;
199	GPS_Parameter.WindCorrection = (float)Parameter.NaviWindCorrection;	199	GPS_Parameter.WindCorrection = (float)Parameter.NaviWindCorrection;
200	GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm	200	GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm
201	}	201	}
202	// FlightMode changed?	202	// FlightMode changed?
-		203	if(GPS_Parameter.FlightMode != FlightMode_Old)
-		204	{
203	if(GPS_Parameter.FlightMode != FlightMode_Old) BeepTime = 100; // beep to indicate that mode has switched	205	BeepTime = 100; // beep to indicate that mode has been switched
-		206	NCFlags &= ~NC_FLAG_TARGET_REACHED;
-		207	}
204	FlightMode_Old = GPS_Parameter.FlightMode;	208	FlightMode_Old = GPS_Parameter.FlightMode;
205	}	209	}
Line 206...	Line 210...
206		210
207	//-------------------------------------------------------------	211	//-------------------------------------------------------------
Line 223...	Line 227...
223	}	227	}
224	else	228	else
225	{	229	{
226	NCFlags &= ~NC_FLAG_MANUAL_CONTROL;	230	NCFlags &= ~NC_FLAG_MANUAL_CONTROL;
227	return(0);	231	return(0);
228	}	232	}
229	}	233	}
Line 230...	Line 234...
230		234
231	//------------------------------------------------------------	235	//------------------------------------------------------------
232	// copy GPS position from source position to target position	236	// copy GPS position from source position to target position
Line 307...	Line 311...
307	s32 dist;	311	s32 dist;
Line 308...	Line 312...
308		312
309	dist = (s32)hypot(x,y); // the length of the vector	313	dist = (s32)hypot(x,y); // the length of the vector
310	if (dist > limit)	314	if (dist > limit)
311	// if vector length is larger than the given limit	315	// if vector length is larger than the given limit
312	{ // scale vector compontents so that the length is cut off to limit	316	{ // scale vector compontents so that the length is cut off to limit
313	x = (s32)(( (double)(x) * (double)limit ) / (double)dist);	317	x = (s32)(( (double)(x) * (double)limit ) / (double)dist);
314	y = (s32)(( (double)(y) * (double)limit ) / (double)dist);	318	y = (s32)(( (double)(y) * (double)limit ) / (double)dist);
315	dist = limit;	319	dist = limit;
316	}	320	}
Line 445...	Line 449...
445	}	449	}
Line 446...	Line 450...
446		450
Line 447...	Line 451...
447	/* The selected flight mode influences the target position pointer and therefore the behavior */	451	/* The selected flight mode influences the target position pointer and therefore the behavior */
448		452
449	// check for current flight mode and set the target pointer GPS_pTargetPosition respectively	453	// check for current flight mode and set the target pointer GPS_pTargetPosition respectively
450	switch(GPS_Parameter.FlightMode)	454	switch(GPS_Parameter.FlightMode)
451	{	455	{
Line 452...	Line 456...
452	// the GPS control is deactived	456	// the GPS control is deactived
Line 593...	Line 597...
593	RangedTargetPosition.Latitude = GPS_HomePosition.Latitude;	597	RangedTargetPosition.Latitude = GPS_HomePosition.Latitude;
594	RangedTargetPosition.Latitude += (s32)((float)TargetHomeDeviation.North / 1.11194927f);	598	RangedTargetPosition.Latitude += (s32)((float)TargetHomeDeviation.North / 1.11194927f);
595	RangedTargetPosition.Longitude = GPS_HomePosition.Longitude;	599	RangedTargetPosition.Longitude = GPS_HomePosition.Longitude;
596	RangedTargetPosition.Longitude += (s32)((float)TargetHomeDeviation.East / (1.11194927f * cos(RadiansFromGPS(GPS_HomePosition.Latitude))) );	600	RangedTargetPosition.Longitude += (s32)((float)TargetHomeDeviation.East / (1.11194927f * cos(RadiansFromGPS(GPS_HomePosition.Latitude))) );
597	RangedTargetPosition.Altitude = GPS_pTargetPosition->Altitude;	601	RangedTargetPosition.Altitude = GPS_pTargetPosition->Altitude;
598	RangedTargetPosition.Status = NEWDATA;	602	RangedTargetPosition.Status = NEWDATA;
599	}	603	}
600	else	604	else
601	{ // the target is located within the operation radius area	605	{ // the target is located within the operation radius area
602	// simple copy the loaction to the ranged target position	606	// simple copy the loaction to the ranged target position
603	GPS_CopyPosition(GPS_pTargetPosition, &RangedTargetPosition);	607	GPS_CopyPosition(GPS_pTargetPosition, &RangedTargetPosition);
Line 631...	Line 635...
631	GPS_Stick.Yaw = 0;	635	GPS_Stick.Yaw = 0;
632	}	636	}
633	else // deviation could not be calculated	637	else // deviation could not be calculated
634	{ // do nothing on gps sticks!	638	{ // do nothing on gps sticks!
635	GPS_Neutral();	639	GPS_Neutral();
-		640	NCFlags &= ~NC_FLAG_TARGET_REACHED; // clear target reached
636	}	641	}
Line 637...	Line 642...
637		642
638	}// eof if GPSSignal is OK	643	}// eof if GPSSignal is OK
639	else // GPSSignal not OK	644	else // GPSSignal not OK
Line 644...	Line 649...
644	{	649	{
645	if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;	650	if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;
646	else if (GPSData.NumOfSats < GPS_Parameter.MinSat && !(beep_rythm % 5)) BeepTime = 10;	651	else if (GPSData.NumOfSats < GPS_Parameter.MinSat && !(beep_rythm % 5)) BeepTime = 10;
647	}	652	}
648	}	653	}
649	GPSData.Status = PROCESSED; // mark as processed	654	GPSData.Status = PROCESSED; // mark as processed
650	break;	655	break;
651	}	656	}
Line 652...	Line 657...
652		657
653	DebugOut.Analog[6] = NCFlags;	658	DebugOut.Analog[6] = NCFlags;
Line 676...	Line 681...
676	NaviData.NCFlags = NCFlags;	681	NaviData.NCFlags = NCFlags;
677	NaviData.OperatingRadius = Parameter.NaviOperatingRadius;	682	NaviData.OperatingRadius = Parameter.NaviOperatingRadius;
678	NaviData.TopSpeed = (s16)GPSData.Speed_Top; // in cm/s	683	NaviData.TopSpeed = (s16)GPSData.Speed_Top; // in cm/s
679	NaviData.TargetHoldTime = (u8)(GetDelay(WPTime)/1000); // in s	684	NaviData.TargetHoldTime = (u8)(GetDelay(WPTime)/1000); // in s
680	//+++++++++++++++++++++++++++++++++++++++++++++++++++	685	//+++++++++++++++++++++++++++++++++++++++++++++++++++
681	return;	686	return;
682	}	687	}

Subversion Repositories NaviCtrl

(root)/tags/V0.17e/GPS.c @ 895 - Rev 102 → 103