WebSVN - NaviCtrl - Diff - Rev 462 and 463



u8 BL_MinOfMaxPWM;  // indication if all BL-controllers run on full power
u32 FC_I2C_ErrorConter;
SPI_Version_t FC_Version;
s16 POI_KameraNick = 0;
u8 NC_Wait_for_LED = 0;
s16 GyroCompassCorrected = 0; // corrected with the magnetic declination

 
//--------------------------------------------------------------
void SSP0_IRQHandler(void)
{

        {
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
                ToFlightCtrl.CompassHeading = Compass_Heading;
                DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading - GeoMagDec) % 3600;
                if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
                ToFlightCtrl.MagVecX = MagVector.X;
                ToFlightCtrl.MagVecY = MagVector.Y;
                ToFlightCtrl.MagVecZ = MagVector.Z;
//              ToFlightCtrl.NCStatus = 0;

                                        //53.28 5788 7.4847269   --> N53° 17.14728 E7° 29.08362  --> N53° 17' 8.837" E7° 29' 5.017" 
                                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
                                        ToFlightCtrl.Param.Byte[0] = 3+3;       // index  +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
                                        ToFlightCtrl.Param.Byte[1] = 9-1;       // how many
                                        //-----------------------------
                                        ToFlightCtrl.Param.Byte[2] = GyroCompassCorrected / 20;//NaviData.HomePositionDeviation.Bearing / 2; 
                                        i1 = GPSData.Speed_Ground; // in cm/sec
                                        i1 *= 36;
                                        i1 /= 1000;
                                        ToFlightCtrl.Param.Byte[3] = i1 % 256;
                                        ToFlightCtrl.Param.Byte[4] = i1 / 256;

                                        ClearFCStatusFlags = 0;
                                }
                                FC.StatusFlags |= FromFlightCtrl.Param.Byte[8];
                                if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive)
                                {
                                        HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600;
                                        Compass_Init();
                                        FCCalibActive = 10;
                                        FC_is_Calibrated = 0;
                                }
                                else
                                {
                                        if(FCCalibActive) if(--FCCalibActive == 0) FC_is_Calibrated = 1;
                                }
                                if(FC.StatusFlags & FC_STATUS_START)
                             {
                                   if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else
                                   HeadFreeStartAngle = GyroCompassCorrected;
                                 }

 
                                 if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
                                  {

                                         if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK))       // nur bei ausreichender Distance -> 20m
                                         {
                                          HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 -  Parameter.OrientationAngle * 150) % 3600; // in 0.1°
                                         }
                                         else                                                                                            // Ansonsten die aktuelle Richtung übernehmen
                                          HeadFreeStartAngle = GyroCompassCorrected; // in 0.1°   
                                        }
                                }

 
                                Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];

                                else DebugOut.StatusRed &= ~AMPEL_FC;
                                break;
                        default:
                                break;
                }
 
                DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
                DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
                DebugOut.Analog[2] = FromFlightCtrl.AccNick;
                DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
                DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg

Rev 462	Rev 463
Line 121...	Line 121...
121	u8 BL_MinOfMaxPWM; // indication if all BL-controllers run on full power	121	u8 BL_MinOfMaxPWM; // indication if all BL-controllers run on full power
122	u32 FC_I2C_ErrorConter;	122	u32 FC_I2C_ErrorConter;
123	SPI_Version_t FC_Version;	123	SPI_Version_t FC_Version;
124	s16 POI_KameraNick = 0;	124	s16 POI_KameraNick = 0;
125	u8 NC_Wait_for_LED = 0;	125	u8 NC_Wait_for_LED = 0;
-		126	s16 GyroCompassCorrected = 0; // corrected with the magnetic declination
Line 126...	Line 127...
126		127
127	//--------------------------------------------------------------	128	//--------------------------------------------------------------
128	void SSP0_IRQHandler(void)	129	void SSP0_IRQHandler(void)
129	{	130	{
Line 304...	Line 305...
304	{	305	{
305	// avoid sending data via SPI during the update of the ToFlightCtrl structure	306	// avoid sending data via SPI during the update of the ToFlightCtrl structure
306	VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt	307	VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
307	ToFlightCtrl.CompassHeading = Compass_Heading;	308	ToFlightCtrl.CompassHeading = Compass_Heading;
308	DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;	309	DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
-		310	GyroCompassCorrected = (3600 + FromFlightCtrl.GyroHeading - GeoMagDec) % 3600;
309	if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;	311	if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
310	ToFlightCtrl.MagVecX = MagVector.X;	312	ToFlightCtrl.MagVecX = MagVector.X;
311	ToFlightCtrl.MagVecY = MagVector.Y;	313	ToFlightCtrl.MagVecY = MagVector.Y;
312	ToFlightCtrl.MagVecZ = MagVector.Z;	314	ToFlightCtrl.MagVecZ = MagVector.Z;
313	// ToFlightCtrl.NCStatus = 0;	315	// ToFlightCtrl.NCStatus = 0;
Line 453...	Line 455...
453	//53.28 5788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017"	455	//53.28 5788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017"
454	ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;	456	ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
455	ToFlightCtrl.Param.Byte[0] = 3+3; // index +3, weil bei HoTT V4 3 Bytes eingeschoben wurden	457	ToFlightCtrl.Param.Byte[0] = 3+3; // index +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
456	ToFlightCtrl.Param.Byte[1] = 9-1; // how many	458	ToFlightCtrl.Param.Byte[1] = 9-1; // how many
457	//-----------------------------	459	//-----------------------------
458	ToFlightCtrl.Param.Byte[2] = FromFlightCtrl.GyroHeading / 20;//NaviData.HomePositionDeviation.Bearing / 2;	460	ToFlightCtrl.Param.Byte[2] = GyroCompassCorrected / 20;//NaviData.HomePositionDeviation.Bearing / 2;
459	i1 = GPSData.Speed_Ground; // in cm/sec	461	i1 = GPSData.Speed_Ground; // in cm/sec
460	i1 *= 36;	462	i1 *= 36;
461	i1 /= 1000;	463	i1 /= 1000;
462	ToFlightCtrl.Param.Byte[3] = i1 % 256;	464	ToFlightCtrl.Param.Byte[3] = i1 % 256;
463	ToFlightCtrl.Param.Byte[4] = i1 / 256;	465	ToFlightCtrl.Param.Byte[4] = i1 / 256;
Line 573...	Line 575...
573	ClearFCStatusFlags = 0;	575	ClearFCStatusFlags = 0;
574	}	576	}
575	FC.StatusFlags \|= FromFlightCtrl.Param.Byte[8];	577	FC.StatusFlags \|= FromFlightCtrl.Param.Byte[8];
576	if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive)	578	if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive)
577	{	579	{
578	HeadFreeStartAngle = Compass_Heading * 10;	580	HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600;
579	Compass_Init();	581	Compass_Init();
580	FCCalibActive = 10;	582	FCCalibActive = 10;
581	FC_is_Calibrated = 0;	583	FC_is_Calibrated = 0;
582	}	584	}
583	else	585	else
584	{	586	{
585	if(FCCalibActive) if(--FCCalibActive == 0) FC_is_Calibrated = 1;	587	if(FCCalibActive) if(--FCCalibActive == 0) FC_is_Calibrated = 1;
586	}	588	}
587	if(FC.StatusFlags & FC_STATUS_START)	589	if(FC.StatusFlags & FC_STATUS_START)
588	{	590	{
589	if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else	591	if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else
590	HeadFreeStartAngle = FromFlightCtrl.GyroHeading;	592	HeadFreeStartAngle = GyroCompassCorrected;
591	}	593	}
Line 592...	Line 594...
592		594
593	if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))	595	if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
594	{	596	{
Line 597...	Line 599...
597	if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK)) // nur bei ausreichender Distance -> 20m	599	if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK)) // nur bei ausreichender Distance -> 20m
598	{	600	{
599	HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 - Parameter.OrientationAngle * 150) % 3600; // in 0.1°	601	HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 - Parameter.OrientationAngle * 150) % 3600; // in 0.1°
600	}	602	}
601	else // Ansonsten die aktuelle Richtung übernehmen	603	else // Ansonsten die aktuelle Richtung übernehmen
602	HeadFreeStartAngle = (3600 + FromFlightCtrl.GyroHeading /+ Parameter.OrientationAngle 150*/) % 3600; // in 0.1°	604	HeadFreeStartAngle = GyroCompassCorrected; // in 0.1°
603	}	605	}
604	}	606	}
Line 605...	Line 607...
605		607
606	Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];	608	Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
Line 736...	Line 738...
736	else DebugOut.StatusRed &= ~AMPEL_FC;	738	else DebugOut.StatusRed &= ~AMPEL_FC;
737	break;	739	break;
738	default:	740	default:
739	break;	741	break;
740	}	742	}
741		-
742	DebugOut.Analog[0] = FromFlightCtrl.AngleNick;	743	DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
743	DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;	744	DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
744	DebugOut.Analog[2] = FromFlightCtrl.AccNick;	745	DebugOut.Analog[2] = FromFlightCtrl.AccNick;
745	DebugOut.Analog[3] = FromFlightCtrl.AccRoll;	746	DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
746	DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg	747	DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg

Subversion Repositories NaviCtrl

(root)/tags/V2.18a/spi_slave.c @ 857 - Rev 462 → 463