WebSVN - NaviCtrl - Diff - Rev 199 and 202



s32 Kalman_K = 32;
s32 Kalman_MaxDrift = 5 * 16;
s32 Kalman_MaxFusion = 64;
s32 ToFcGpsZ = 0;

 
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN};

u8 SPI_CommandCounter = 0;

 

        SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
        SSP_InitStructure.SSP_ClockRate = 0;

 
        SSP_Init(SSP0, &SSP_InitStructure);
        SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
 

        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue));
 
        SSP_Cmd(SSP0, ENABLE);

//------------------------------------------------------
void SPI0_UpdateBuffer(void)
{
        static u32 timeout = 0;
        static u8 counter = 50;
 
        if (SPI_RxBuffer_Request)
        {
                // avoid sending data via SPI during the update of the  ToFlightCtrl structure
                VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
                ToFlightCtrl.CompassHeading = I2C_Heading.Heading;

                // restart command cycle at the end
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;

 
                switch (ToFlightCtrl.Command)
                {
                        case  SPI_NCCMD_KALMAN:
                                ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
                                ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
                                ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
                                ToFlightCtrl.Param.Byte[3]      = (u8) SerialLinkOkay;
                                ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;

                                break;
 
                        case SPI_NCCMD_VERSION:
                                ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR;
                                ToFlightCtrl.Param.Byte[1] = VERSION_MINOR;
                                ToFlightCtrl.Param.Byte[2] = VERSION_MINOR;
                                ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE;
                                ToFlightCtrl.Param.Byte[4] = 0;
                                break;
 
                        default:
                                break;

                }
                VIC_ITCmd(SSP0_ITLine, ENABLE);         // enable SPI interrupt
 
 
                switch(FromFlightCtrl.Command)
                {
                        case SPI_FCCMD_USER:
                                Parameter.User1 = FromFlightCtrl.Param.Byte[0];

                                break;

 
#define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; }

#define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); }
 
                        case SPI_FCCMD_PARAMETER1:
                                CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255);
                                CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);
                                CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);

                                CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
                                CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255);
                                CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],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];

                                FC.Poti[2]              = FromFlightCtrl.Param.Byte[6];
                                FC.Poti[3]              = FromFlightCtrl.Param.Byte[7];
                                FC.Poti[4]              = FromFlightCtrl.Param.Byte[8];
                                FC.Poti[5]              = FromFlightCtrl.Param.Byte[9];
                                FC.Poti[6]              = FromFlightCtrl.Param.Byte[10];
                                FC.Poti[7]              = FromFlightCtrl.Param.Byte[11];
                                break;

 
                        case SPI_FCCMD_MISC:
                                if(CompassCalState != FromFlightCtrl.Param.Byte[0])
                                {       // put only new CompassCalState into queue to send via I2C
                                        CompassCalState = FromFlightCtrl.Param.Byte[0];
                                        fifo_put(&CompassCalcStateFiFo, CompassCalState);

                                FC.RC_Quality   = FromFlightCtrl.Param.Byte[9];
                                FC.RC_RSSI              = FromFlightCtrl.Param.Byte[10];
                                NaviData.Gas    = (FC.UBat * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
                                break;

 
                        case SPI_FCCMD_SERVOS:
                                ServoParams.Refresh             = FromFlightCtrl.Param.Byte[0];
                                ServoParams.CompInvert  = FromFlightCtrl.Param.Byte[1];
                                ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];
                                ServoParams.NickComp    = FromFlightCtrl.Param.Byte[3];

                                ServoParams.RollComp    = FromFlightCtrl.Param.Byte[7];
                                ServoParams.RollMin             = FromFlightCtrl.Param.Byte[8];
                                ServoParams.RollMax             = FromFlightCtrl.Param.Byte[9];
                                break;

 
                        case SPI_FCCMD_VERSION:
                                FC_Version.Major                = FromFlightCtrl.Param.Byte[0];
                                FC_Version.Minor                = FromFlightCtrl.Param.Byte[1];
                                FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
                                FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];

                }

 
                // every time we got new data from the FC via SPI call the navigation routine
                GPS_Navigation();
                ClearFCFlags = 1;
 
                if(counter)
                {
                        counter--;                                       // count down to enable servo
                        if(!counter) TIMER2_Init();  // enable Servo Output

                }
 

                Data3D.AngleNick = FromFlightCtrl.AngleNick;            // in 0.1 deg
                Data3D.AngleRoll = FromFlightCtrl.AngleRoll;            // in 0.1 deg
                Data3D.Heading   = FromFlightCtrl.GyroHeading;          // in 0.1 deg
        }       // EOF if(SPI_RxBuffer_Request)
        else // no new SPI data
        {
                if(CheckDelay(timeout) && (counter == 0))
                {
                        TIMER2_Deinit();  // disable Servo Output
                        counter = 50;     // reset counter for enabling Servo Output
                }

Rev 199	Rev 202
Line 96...	Line 96...
96	s32 Kalman_K = 32;	96	s32 Kalman_K = 32;
97	s32 Kalman_MaxDrift = 5 * 16;	97	s32 Kalman_MaxDrift = 5 * 16;
98	s32 Kalman_MaxFusion = 64;	98	s32 Kalman_MaxFusion = 64;
99	s32 ToFcGpsZ = 0;	99	s32 ToFcGpsZ = 0;
Line 100...	Line 100...
100		100
101	u8 SPI_CommandSequence[] = { SPI_KALMAN };	101	u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN, SPI_NCCMD_KALMAN};
Line 102...	Line 102...
102	u8 SPI_CommandCounter = 0;	102	u8 SPI_CommandCounter = 0;
Line 103...	Line 103...
103		103
Line 244...	Line 244...
244	SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;	244	SSP_InitStructure.SSP_CPOL = SSP_CPOL_Low;
245	SSP_InitStructure.SSP_ClockRate = 0;	245	SSP_InitStructure.SSP_ClockRate = 0;
Line 246...	Line 246...
246		246
247	SSP_Init(SSP0, &SSP_InitStructure);	247	SSP_Init(SSP0, &SSP_InitStructure);
248	SSP_ITConfig(SSP0, SSP_IT_RxFifo \| SSP_IT_RxTimeOut, ENABLE);	248	SSP_ITConfig(SSP0, SSP_IT_RxFifo \| SSP_IT_RxTimeOut, ENABLE);
249		249
Line 250...	Line 250...
250	fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue));	250	fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue));
251		251
252	SSP_Cmd(SSP0, ENABLE);	252	SSP_Cmd(SSP0, ENABLE);
Line 267...	Line 267...
267	//------------------------------------------------------	267	//------------------------------------------------------
268	void SPI0_UpdateBuffer(void)	268	void SPI0_UpdateBuffer(void)
269	{	269	{
270	static u32 timeout = 0;	270	static u32 timeout = 0;
271	static u8 counter = 50;	271	static u8 counter = 50;
272		272
273	if (SPI_RxBuffer_Request)	273	if (SPI_RxBuffer_Request)
274	{	274	{
275	// avoid sending data via SPI during the update of the ToFlightCtrl structure	275	// avoid sending data via SPI during the update of the ToFlightCtrl structure
276	VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt	276	VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
277	ToFlightCtrl.CompassHeading = I2C_Heading.Heading;	277	ToFlightCtrl.CompassHeading = I2C_Heading.Heading;
Line 284...	Line 284...
284	// restart command cycle at the end	284	// restart command cycle at the end
285	if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;	285	if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
Line 286...	Line 286...
286		286
287	switch (ToFlightCtrl.Command)	287	switch (ToFlightCtrl.Command)
288	{	288	{
289	case SPI_KALMAN:	289	case SPI_NCCMD_KALMAN:
290	ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;	290	ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
291	ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;	291	ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
292	ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;	292	ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
293	ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay;	293	ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay;
294	ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;	294	ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
Line -...	Line 295...
-		295	break;
-		296
-		297	case SPI_NCCMD_VERSION:
-		298	ToFlightCtrl.Param.Byte[0] = VERSION_MAJOR;
-		299	ToFlightCtrl.Param.Byte[1] = VERSION_MINOR;
-		300	ToFlightCtrl.Param.Byte[2] = VERSION_MINOR;
-		301	ToFlightCtrl.Param.Byte[3] = FC_SPI_COMPATIBLE;
-		302	ToFlightCtrl.Param.Byte[4] = 0;
295	break;	303	break;
296		304
297	default:	305	default:
298	break;	306	break;
Line 299...	Line 307...
299	}	307	}
300	VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt	308	VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt
301		309
302		310
303	switch(FromFlightCtrl.Command)	311	switch(FromFlightCtrl.Command)
304	{	312	{
305	case SPI_CMD_USER:	313	case SPI_FCCMD_USER:
306	Parameter.User1 = FromFlightCtrl.Param.Byte[0];	314	Parameter.User1 = FromFlightCtrl.Param.Byte[0];
Line 324...	Line 332...
324	break;	332	break;
Line 325...	Line 333...
325		333
326	#define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; }	334	#define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; }
Line 327...	Line 335...
327	#define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); }	335	#define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); }
328		336
329	case SPI_CMD_PARAMETER1:	337	case SPI_FCCMD_PARAMETER1:
330	CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255);	338	CHK_POTI_MM(Parameter.NaviGpsModeControl,FromFlightCtrl.Param.Byte[0],0,255);
331	CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);	339	CHK_POTI_MM(Parameter.NaviGpsGain,FromFlightCtrl.Param.Byte[1],0,255);
332	CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);	340	CHK_POTI_MM(Parameter.NaviGpsP,FromFlightCtrl.Param.Byte[2],0,255);
Line 339...	Line 347...
339	CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);	347	CHK_POTI_MM(Parameter.NaviWindCorrection,FromFlightCtrl.Param.Byte[9],0,255);
340	CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255);	348	CHK_POTI_MM(Parameter.NaviSpeedCompensation,FromFlightCtrl.Param.Byte[10],0,255);
341	CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);	349	CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
342	break;	350	break;
Line 343...	Line 351...
343		351
344	case SPI_CMD_STICK:	352	case SPI_FCCMD_STICK:
345	FC.StickGas = FromFlightCtrl.Param.sByte[0];	353	FC.StickGas = FromFlightCtrl.Param.sByte[0];
346	FC.StickYaw = FromFlightCtrl.Param.sByte[1];	354	FC.StickYaw = FromFlightCtrl.Param.sByte[1];
347	FC.StickRoll = FromFlightCtrl.Param.sByte[2];	355	FC.StickRoll = FromFlightCtrl.Param.sByte[2];
348	FC.StickNick = FromFlightCtrl.Param.sByte[3];	356	FC.StickNick = FromFlightCtrl.Param.sByte[3];
Line 351...	Line 359...
351	FC.Poti[2] = FromFlightCtrl.Param.Byte[6];	359	FC.Poti[2] = FromFlightCtrl.Param.Byte[6];
352	FC.Poti[3] = FromFlightCtrl.Param.Byte[7];	360	FC.Poti[3] = FromFlightCtrl.Param.Byte[7];
353	FC.Poti[4] = FromFlightCtrl.Param.Byte[8];	361	FC.Poti[4] = FromFlightCtrl.Param.Byte[8];
354	FC.Poti[5] = FromFlightCtrl.Param.Byte[9];	362	FC.Poti[5] = FromFlightCtrl.Param.Byte[9];
355	FC.Poti[6] = FromFlightCtrl.Param.Byte[10];	363	FC.Poti[6] = FromFlightCtrl.Param.Byte[10];
356	FC.Poti[7] = FromFlightCtrl.Param.Byte[11];	364	FC.Poti[7] = FromFlightCtrl.Param.Byte[11];
357	break;	365	break;
Line 358...	Line 366...
358		366
359	case SPI_CMD_MISC:	367	case SPI_FCCMD_MISC:
360	if(CompassCalState != FromFlightCtrl.Param.Byte[0])	368	if(CompassCalState != FromFlightCtrl.Param.Byte[0])
361	{ // put only new CompassCalState into queue to send via I2C	369	{ // put only new CompassCalState into queue to send via I2C
362	CompassCalState = FromFlightCtrl.Param.Byte[0];	370	CompassCalState = FromFlightCtrl.Param.Byte[0];
363	fifo_put(&CompassCalcStateFiFo, CompassCalState);	371	fifo_put(&CompassCalcStateFiFo, CompassCalState);
Line 372...	Line 380...
372	FC.RC_Quality = FromFlightCtrl.Param.Byte[9];	380	FC.RC_Quality = FromFlightCtrl.Param.Byte[9];
373	FC.RC_RSSI = FromFlightCtrl.Param.Byte[10];	381	FC.RC_RSSI = FromFlightCtrl.Param.Byte[10];
374	NaviData.Gas = (FC.UBat * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;	382	NaviData.Gas = (FC.UBat * (u32) FromFlightCtrl.Param.Byte[11]) / (u32) Parameter.LowVoltageWarning;
375	break;	383	break;
Line 376...	Line 384...
376		384
377	case SPI_CMD_SERVOS:	385	case SPI_FCCMD_SERVOS:
378	ServoParams.Refresh = FromFlightCtrl.Param.Byte[0];	386	ServoParams.Refresh = FromFlightCtrl.Param.Byte[0];
379	ServoParams.CompInvert = FromFlightCtrl.Param.Byte[1];	387	ServoParams.CompInvert = FromFlightCtrl.Param.Byte[1];
380	ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];	388	ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];
381	ServoParams.NickComp = FromFlightCtrl.Param.Byte[3];	389	ServoParams.NickComp = FromFlightCtrl.Param.Byte[3];
Line 385...	Line 393...
385	ServoParams.RollComp = FromFlightCtrl.Param.Byte[7];	393	ServoParams.RollComp = FromFlightCtrl.Param.Byte[7];
386	ServoParams.RollMin = FromFlightCtrl.Param.Byte[8];	394	ServoParams.RollMin = FromFlightCtrl.Param.Byte[8];
387	ServoParams.RollMax = FromFlightCtrl.Param.Byte[9];	395	ServoParams.RollMax = FromFlightCtrl.Param.Byte[9];
388	break;	396	break;
Line 389...	Line 397...
389		397
390	case SPI_CMD_VERSION:	398	case SPI_FCCMD_VERSION:
391	FC_Version.Major = FromFlightCtrl.Param.Byte[0];	399	FC_Version.Major = FromFlightCtrl.Param.Byte[0];
392	FC_Version.Minor = FromFlightCtrl.Param.Byte[1];	400	FC_Version.Minor = FromFlightCtrl.Param.Byte[1];
393	FC_Version.Patch = FromFlightCtrl.Param.Byte[2];	401	FC_Version.Patch = FromFlightCtrl.Param.Byte[2];
394	FC_Version.Compatible = FromFlightCtrl.Param.Byte[3];	402	FC_Version.Compatible = FromFlightCtrl.Param.Byte[3];
Line 400...	Line 408...
400	}	408	}
Line 401...	Line 409...
401		409
402	// every time we got new data from the FC via SPI call the navigation routine	410	// every time we got new data from the FC via SPI call the navigation routine
403	GPS_Navigation();	411	GPS_Navigation();
404	ClearFCFlags = 1;	412	ClearFCFlags = 1;
405		413
406	if(counter)	414	if(counter)
407	{	415	{
408	counter--; // count down to enable servo	416	counter--; // count down to enable servo
409	if(!counter) TIMER2_Init(); // enable Servo Output	417	if(!counter) TIMER2_Init(); // enable Servo Output
Line 410...	Line 418...
410	}	418	}
411		419
Line 420...	Line 428...
420	Data3D.AngleNick = FromFlightCtrl.AngleNick; // in 0.1 deg	428	Data3D.AngleNick = FromFlightCtrl.AngleNick; // in 0.1 deg
421	Data3D.AngleRoll = FromFlightCtrl.AngleRoll; // in 0.1 deg	429	Data3D.AngleRoll = FromFlightCtrl.AngleRoll; // in 0.1 deg
422	Data3D.Heading = FromFlightCtrl.GyroHeading; // in 0.1 deg	430	Data3D.Heading = FromFlightCtrl.GyroHeading; // in 0.1 deg
423	} // EOF if(SPI_RxBuffer_Request)	431	} // EOF if(SPI_RxBuffer_Request)
424	else // no new SPI data	432	else // no new SPI data
425	{	433	{
426	if(CheckDelay(timeout) && (counter == 0))	434	if(CheckDelay(timeout) && (counter == 0))
427	{	435	{
428	TIMER2_Deinit(); // disable Servo Output	436	TIMER2_Deinit(); // disable Servo Output
429	counter = 50; // reset counter for enabling Servo Output	437	counter = 50; // reset counter for enabling Servo Output
430	}	438	}

Subversion Repositories NaviCtrl

(root)/tags/V2.06i/spi_slave.c @ 811 - Rev 199 → 202