WebSVN - NaviCtrl - Diff - Rev 417 and 419



s32 Kalman_MaxFusion = 64;
s32 Kalman_Kompass = 32;
s32 ToFcGpsZ = 0;
u8 CompassCalState = 0;

 
u8 SPI_CommandSequence[] = { SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN, SPI_NCCMD_VERSION };
u8 SPI_CommandCounter = 0;
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
s32 HeadFreeStartAngle = 0;
s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde

        // set the pointer to the checksum byte in the tx buffer
        Ptr_TxChksum = (u8 *) &(((ToFlightCtrl_t *) &(SPI_TxBuffer[2]))->Chksum);

 
        ToFlightCtrl.GPSStick.Nick = 0;
        ToFlightCtrl.GPSStick.Roll = 0;

//      ToFlightCtrl.GPSStick.Yaw = 0;
 

        VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0);

 
//------------------------------------------------------
void SPI0_UpdateBuffer(void)
{
        static u32 timeout = 0;
        static u8 counter = 50,hott_index = 0, last_error_code = 0;
        s16 tmp;

        s32 i1,i2;
       

                // 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;
                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;
                // cycle spi commands
                if(ErrorCode != last_error_code) ToFlightCtrl.Command = SPI_NCCMD_VERSION;
                else ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
                // restart command cycle at the end
                if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;

 
                last_error_code = ErrorCode;
 
#define FLAG_GPS_AID 0x01
                switch (ToFlightCtrl.Command)
                {
                        case  SPI_NCCMD_KALMAN:  // wird am häufigsten betätigt

                        case SPI_MISC:
                                ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5;
                                ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination;
                                ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic;
                                ToFlightCtrl.Param.Byte[3] = SpeakHoTT;
                                ToFlightCtrl.Param.Byte[4] = NaviData.WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1
                                ToFlightCtrl.Param.Byte[5] = NaviData.WaypointNumber; // number of stored waypoints
 
                                ToFlightCtrl.Param.Int[3] =  NaviData.TargetPositionDeviation.Distance / 10;
                                ToFlightCtrl.Param.Byte[8] = NaviData.TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached
                                ToFlightCtrl.Param.Byte[9] = 0;
                                ToFlightCtrl.Param.Byte[10] = 0;
                                ToFlightCtrl.Param.Byte[11] = 0;
//DebugOut.Analog[16] = SpeakHoTT;
                                SpeakHoTT = 0;

                                ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
                                ToFlightCtrl.Param.Int[2]  = NaviData.HomePositionDeviation.Distance; // dm   //4&5
                                ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing;  // deg  //6&7
                                if(FC_WP_EventChannel > 254) FC_WP_EventChannel = 254;
                                ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 127);
                                FC_WP_EventChannel = 0; // the GPS-Routine will set it again
                                if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp))
                                {
                                        ToFlightCtrl.Param.Byte[9] = (u8)tmp;
                                }
                                else

                                CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);
                                CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
                                CHK_POTI_MM(Parameter.NaviAccCompensation,FromFlightCtrl.Param.Byte[10],0,255);
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
                                break;
                        case SPI_FCCMD_PARAMETER2:
                                CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255);
                                break;
                        case SPI_FCCMD_STICK:
                                FC.StickGas     = FromFlightCtrl.Param.sByte[0];
                                FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
                                FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
                                FC.StickNick    = FromFlightCtrl.Param.sByte[3];

                                ServoParams.RollControl = FromFlightCtrl.Param.Byte[6];
                                ServoParams.RollComp    = FromFlightCtrl.Param.Byte[7];
                                ServoParams.RollMin             = FromFlightCtrl.Param.Byte[8];
                                ServoParams.RollMax             = FromFlightCtrl.Param.Byte[9];
                                BL_MinOfMaxPWM = FromFlightCtrl.Param.Byte[10];
                                FC_I2C_ErrorConter = FromFlightCtrl.Param.Byte[11];
                                break;

 
                        case SPI_FCCMD_VERSION:
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];

if(Parameter.User8 < 100) FC.StatusFlags = 0;
else
if(Parameter.User8 < 150) FC.StatusFlags = FC_STATUS_START;
else FC.StatusFlags = FC_STATUS_FLY | FC_STATUS_MOTOR_RUN;
BL_MinOfMaxPWM = 255;
NaviData.FCStatusFlags = FC.StatusFlags;
*/
//+++++++++++++++++++++++++++++++++++++++++++++++++++

 

Rev 417	Rev 419
Line 101...	Line 101...
101	s32 Kalman_MaxFusion = 64;	101	s32 Kalman_MaxFusion = 64;
102	s32 Kalman_Kompass = 32;	102	s32 Kalman_Kompass = 32;
103	s32 ToFcGpsZ = 0;	103	s32 ToFcGpsZ = 0;
104	u8 CompassCalState = 0;	104	u8 CompassCalState = 0;
Line 105...	Line 105...
105		105
106	u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN };	106	u8 SPI_CommandSequence[] = { SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN, SPI_NCCMD_VERSION };
107	u8 SPI_CommandCounter = 0;	107	u8 SPI_CommandCounter = 0;
108	s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;	108	s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
109	s32 HeadFreeStartAngle = 0;	109	s32 HeadFreeStartAngle = 0;
110	s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde	110	s16 FC_WP_EventChannel = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
Line 268...	Line 268...
268	// set the pointer to the checksum byte in the tx buffer	268	// set the pointer to the checksum byte in the tx buffer
269	Ptr_TxChksum = (u8 ) &(((ToFlightCtrl_t ) &(SPI_TxBuffer[2]))->Chksum);	269	Ptr_TxChksum = (u8 ) &(((ToFlightCtrl_t ) &(SPI_TxBuffer[2]))->Chksum);
Line 270...	Line 270...
270		270
271	ToFlightCtrl.GPSStick.Nick = 0;	271	ToFlightCtrl.GPSStick.Nick = 0;
272	ToFlightCtrl.GPSStick.Roll = 0;	272	ToFlightCtrl.GPSStick.Roll = 0;
Line 273...	Line 273...
273	ToFlightCtrl.GPSStick.Yaw = 0;	273	// ToFlightCtrl.GPSStick.Yaw = 0;
274		274
Line 275...	Line 275...
275	VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0);	275	VIC_Config(SSP0_ITLine, VIC_IRQ, PRIORITY_SPI0);
Line 283...	Line 283...
283		283
284	//------------------------------------------------------	284	//------------------------------------------------------
285	void SPI0_UpdateBuffer(void)	285	void SPI0_UpdateBuffer(void)
286	{	286	{
287	static u32 timeout = 0;	287	static u32 timeout = 0;
288	static u8 counter = 50,hott_index = 0;	288	static u8 counter = 50,hott_index = 0, last_error_code = 0;
289	s16 tmp;	289	s16 tmp;
Line 290...	Line 290...
290	s32 i1,i2;	290	s32 i1,i2;
291		291
Line 295...	Line 295...
295	// avoid sending data via SPI during the update of the ToFlightCtrl structure	295	// avoid sending data via SPI during the update of the ToFlightCtrl structure
296	VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt	296	VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
297	ToFlightCtrl.CompassHeading = Compass_Heading;	297	ToFlightCtrl.CompassHeading = Compass_Heading;
298	DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;	298	DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
299	if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;	299	if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
300	// ToFlightCtrl.MagVecX = MagVector.X;	300	ToFlightCtrl.MagVecX = MagVector.X;
301	// ToFlightCtrl.MagVecY = MagVector.Y;	301	ToFlightCtrl.MagVecY = MagVector.Y;
302	ToFlightCtrl.MagVecZ = MagVector.Z;	302	ToFlightCtrl.MagVecZ = MagVector.Z;
303	ToFlightCtrl.NCStatus = 0;	303	// ToFlightCtrl.NCStatus = 0;
304	// cycle spi commands	304	// cycle spi commands
-		305	if(ErrorCode != last_error_code) ToFlightCtrl.Command = SPI_NCCMD_VERSION;
305	ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];	306	else ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
306	// restart command cycle at the end	307	// restart command cycle at the end
307	if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;	308	if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
Line -...	Line 309...
-		309
-		310	last_error_code = ErrorCode;
308		311
309	#define FLAG_GPS_AID 0x01	312	#define FLAG_GPS_AID 0x01
310	switch (ToFlightCtrl.Command)	313	switch (ToFlightCtrl.Command)
311	{	314	{
312	case SPI_NCCMD_KALMAN: // wird am häufigsten betätigt	315	case SPI_NCCMD_KALMAN: // wird am häufigsten betätigt
Line 362...	Line 365...
362	case SPI_MISC:	365	case SPI_MISC:
363	ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5;	366	ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5;
364	ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination;	367	ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination;
365	ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic;	368	ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic;
366	ToFlightCtrl.Param.Byte[3] = SpeakHoTT;	369	ToFlightCtrl.Param.Byte[3] = SpeakHoTT;
367	ToFlightCtrl.Param.Byte[4] = 0;	370	ToFlightCtrl.Param.Byte[4] = NaviData.WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1
368	ToFlightCtrl.Param.Byte[5] = 0;	371	ToFlightCtrl.Param.Byte[5] = NaviData.WaypointNumber; // number of stored waypoints
369	ToFlightCtrl.Param.Byte[6] = 0;	-
370	ToFlightCtrl.Param.Byte[7] = 0;	372	ToFlightCtrl.Param.Int[3] = NaviData.TargetPositionDeviation.Distance / 10;
371	ToFlightCtrl.Param.Byte[8] = 0;	373	ToFlightCtrl.Param.Byte[8] = NaviData.TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached
372	ToFlightCtrl.Param.Byte[9] = 0;	374	ToFlightCtrl.Param.Byte[9] = 0;
373	ToFlightCtrl.Param.Byte[10] = 0;	375	ToFlightCtrl.Param.Byte[10] = 0;
374	ToFlightCtrl.Param.Byte[11] = 0;	376	ToFlightCtrl.Param.Byte[11] = 0;
375	//DebugOut.Analog[16] = SpeakHoTT;	377	//DebugOut.Analog[16] = SpeakHoTT;
376	SpeakHoTT = 0;	378	SpeakHoTT = 0;
Line 383...	Line 385...
383	ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s	385	ToFlightCtrl.Param.Byte[3] = GPSData.Speed_Ground / 100; // m/s
384	ToFlightCtrl.Param.Int[2] = NaviData.HomePositionDeviation.Distance; // dm //4&5	386	ToFlightCtrl.Param.Int[2] = NaviData.HomePositionDeviation.Distance; // dm //4&5
385	ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing; // deg //6&7	387	ToFlightCtrl.Param.sInt[3] = NaviData.HomePositionDeviation.Bearing; // deg //6&7
386	if(FC_WP_EventChannel > 254) FC_WP_EventChannel = 254;	388	if(FC_WP_EventChannel > 254) FC_WP_EventChannel = 254;
387	ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 127);	389	ToFlightCtrl.Param.Byte[8] = (s8)(FC_WP_EventChannel - 127);
-		390	FC_WP_EventChannel = 0; // the GPS-Routine will set it again
388	if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp))	391	if(NCRARAM_STATE_VALID == NCParams_GetValue(NCPARAMS_ALTITUDE_RATE, &tmp))
389	{	392	{
390	ToFlightCtrl.Param.Byte[9] = (u8)tmp;	393	ToFlightCtrl.Param.Byte[9] = (u8)tmp;
391	}	394	}
392	else	395	else
Line 580...	Line 583...
580	CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);	583	CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);
581	CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);	584	CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
582	CHK_POTI_MM(Parameter.NaviAccCompensation,FromFlightCtrl.Param.Byte[10],0,255);	585	CHK_POTI_MM(Parameter.NaviAccCompensation,FromFlightCtrl.Param.Byte[10],0,255);
583	CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);	586	CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
584	break;	587	break;
-		588	case SPI_FCCMD_PARAMETER2:
-		589	CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255);
585		590	break;
586	case SPI_FCCMD_STICK:	591	case SPI_FCCMD_STICK:
587	FC.StickGas = FromFlightCtrl.Param.sByte[0];	592	FC.StickGas = FromFlightCtrl.Param.sByte[0];
588	FC.StickYaw = FromFlightCtrl.Param.sByte[1];	593	FC.StickYaw = FromFlightCtrl.Param.sByte[1];
589	FC.StickRoll = FromFlightCtrl.Param.sByte[2];	594	FC.StickRoll = FromFlightCtrl.Param.sByte[2];
590	FC.StickNick = FromFlightCtrl.Param.sByte[3];	595	FC.StickNick = FromFlightCtrl.Param.sByte[3];
Line 632...	Line 637...
632	ServoParams.RollControl = FromFlightCtrl.Param.Byte[6];	637	ServoParams.RollControl = FromFlightCtrl.Param.Byte[6];
633	ServoParams.RollComp = FromFlightCtrl.Param.Byte[7];	638	ServoParams.RollComp = FromFlightCtrl.Param.Byte[7];
634	ServoParams.RollMin = FromFlightCtrl.Param.Byte[8];	639	ServoParams.RollMin = FromFlightCtrl.Param.Byte[8];
635	ServoParams.RollMax = FromFlightCtrl.Param.Byte[9];	640	ServoParams.RollMax = FromFlightCtrl.Param.Byte[9];
636	BL_MinOfMaxPWM = FromFlightCtrl.Param.Byte[10];	641	BL_MinOfMaxPWM = FromFlightCtrl.Param.Byte[10];
-		642	FC_I2C_ErrorConter = FromFlightCtrl.Param.Byte[11];
637	break;	643	break;
Line 638...	Line 644...
638		644
639	case SPI_FCCMD_VERSION:	645	case SPI_FCCMD_VERSION:
640	FC_Version.Major = FromFlightCtrl.Param.Byte[0];	646	FC_Version.Major = FromFlightCtrl.Param.Byte[0];
Line 680...	Line 686...
680	if(Parameter.User8 < 100) FC.StatusFlags = 0;	686	if(Parameter.User8 < 100) FC.StatusFlags = 0;
681	else	687	else
682	if(Parameter.User8 < 150) FC.StatusFlags = FC_STATUS_START;	688	if(Parameter.User8 < 150) FC.StatusFlags = FC_STATUS_START;
683	else FC.StatusFlags = FC_STATUS_FLY \| FC_STATUS_MOTOR_RUN;	689	else FC.StatusFlags = FC_STATUS_FLY \| FC_STATUS_MOTOR_RUN;
684	BL_MinOfMaxPWM = 255;	690	BL_MinOfMaxPWM = 255;
-		691	NaviData.FCStatusFlags = FC.StatusFlags;
685	*/	692	*/
686	//+++++++++++++++++++++++++++++++++++++++++++++++++++	693	//+++++++++++++++++++++++++++++++++++++++++++++++++++
Line 687...	Line 694...
687		694

Subversion Repositories NaviCtrl

(root)/tags/V2.04a/spi_slave.c @ 634 - Rev 417 → 419