WebSVN - FlightCtrl - Blame - Rev 1078 - /branches/V0.71h Code Redesign killagreg/spi.c

Rev	Author	Line No.	Line
304	ingob	1	// ######################## SPI - FlightCtrl ###################
886	killagreg	2	#include <avr/io.h>
		3	#include <avr/interrupt.h>
		4	#include <string.h>
		5	#include <stdlib.h>
		6	#include "_Settings.h"
		7	#include "spi.h"
		8	#include "fc.h"
		9	#include "rc.h"
		10	#include "eeprom.h"
		11	#include "uart.h"
		12	#include "timer0.h"
936	killagreg	13	#include "analog.h"
304	ingob	14
886	killagreg	15	#define SPI_TXSYNCBYTE1 0xAA
		16	#define SPI_TXSYNCBYTE2 0x83
		17	#define SPI_RXSYNCBYTE1 0x81
		18	#define SPI_RXSYNCBYTE2 0x55
304	ingob	19
886	killagreg	20	typedef enum
		21	{
		22	SPI_SYNC1,
		23	SPI_SYNC2,
		24	SPI_DATA
		25	} SPI_RXState_t;
708	ingob	26
606	ingob	27
886	killagreg	28	// data exchange packets to and From NaviCtrl
1078	killagreg	29	ToNaviCtrl_t ToNaviCtrl;
		30	FromNaviCtrl_t FromNaviCtrl;
		31	FromNaviCtrl_Value_t FromNaviCtrl_Value;
708	ingob	32
1078	killagreg	33	SPI_VersionInfo_t SPI_VersionInfo;
		34
886	killagreg	35	// rx packet buffer
		36	#define SPI_RXBUFFER_LEN sizeof(FromNaviCtrl)
		37	uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN];
		38	uint8_t SPI_RxBufferIndex = 0;
		39	uint8_t SPI_RxBuffer_Request = 0;
720	ingob	40
886	killagreg	41	// tx packet buffer
		42	#define SPI_TXBUFFER_LEN sizeof(ToNaviCtrl)
		43	uint8_t *SPI_TxBuffer;
		44	uint8_t SPI_TxBufferIndex = 0;
823	ingob	45
886	killagreg	46	uint8_t SPITransferCompleted, SPI_ChkSum;
		47	uint8_t SPI_RxDataValid;
		48
1078	killagreg	49	uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION };
886	killagreg	50	uint8_t SPI_CommandCounter = 0;
		51
		52	/*********************************************/
		53	/* Initialize SPI interface to NaviCtrl */
		54	/*********************************************/
304	ingob	55	void SPI_MasterInit(void)
		56	{
886	killagreg	57	DDR_SPI \|= (1<<DD_MOSI)\|(1<<DD_SCK); // Set MOSI and SCK output, all others input
		58	SLAVE_SELECT_DDR_PORT \|= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port
		59
		60	SPCR = (1<<SPE)\|(1<<MSTR)\|(1<<SPR1)\|(0<<SPR0)\|(0<<SPIE); // Enable SPI, Master, set clock rate fck/64
		61	SPSR = 0;//(1<<SPI2X);
		62
		63	SLAVE_SELECT_PORT \|= (1 << SPI_SLAVE_SELECT); // Deselect Slave
		64
		65	SPI_TxBuffer = (uint8_t *) &ToNaviCtrl; // set pointer to tx-buffer
		66	SPITransferCompleted = 1;
		67	// initialize data packet to NaviControl
		68	ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
		69	ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
		70
		71	ToNaviCtrl.Command = SPI_CMD_USER;
911	killagreg	72	ToNaviCtrl.IntegralNick = 0;
886	killagreg	73	ToNaviCtrl.IntegralRoll = 0;
1078	killagreg	74
886	killagreg	75	SPI_RxDataValid = 0;
1078	killagreg	76
		77	SPI_VersionInfo.Major = VERSION_MAJOR;
		78	SPI_VersionInfo.Minor = VERSION_MINOR;
		79	SPI_VersionInfo.Patch = VERSION_PATCH;
		80	SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE;
304	ingob	81	}
		82
886	killagreg	83
		84	/**********************************************************/
		85	/* Update Data transferd by the SPI from/to NaviCtrl */
		86	/**********************************************************/
		87	void UpdateSPI_Buffer(void)
304	ingob	88	{
886	killagreg	89	int16_t tmp;
		90	cli(); // stop all interrupts to avoid writing of new data during update of that packet.
823	ingob	91
886	killagreg	92	// update content of packet to NaviCtrl
911	killagreg	93	ToNaviCtrl.IntegralNick = (int16_t) (IntegralNick / 108);
886	killagreg	94	ToNaviCtrl.IntegralRoll = (int16_t) (IntegralRoll / 108);
		95	ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
911	killagreg	96	ToNaviCtrl.GyroNick = Reading_GyroNick;
886	killagreg	97	ToNaviCtrl.GyroRoll = Reading_GyroRoll;
		98	ToNaviCtrl.GyroYaw = Reading_GyroYaw;
911	killagreg	99	ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc);
1078	killagreg	100	ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc);
911	killagreg	101	NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0;
691	ingob	102
886	killagreg	103	switch(ToNaviCtrl.Command)
		104	{
		105	case SPI_CMD_USER:
		106	ToNaviCtrl.Param.Byte[0] = FCParam.UserParam1;
		107	ToNaviCtrl.Param.Byte[1] = FCParam.UserParam2;
		108	ToNaviCtrl.Param.Byte[2] = FCParam.UserParam3;
		109	ToNaviCtrl.Param.Byte[3] = FCParam.UserParam4;
		110	ToNaviCtrl.Param.Byte[4] = FCParam.UserParam5;
		111	ToNaviCtrl.Param.Byte[5] = FCParam.UserParam6;
		112	ToNaviCtrl.Param.Byte[6] = FCParam.UserParam7;
		113	ToNaviCtrl.Param.Byte[7] = FCParam.UserParam8;
936	killagreg	114	ToNaviCtrl.Param.Byte[8] = MKFlags;
		115	MKFlags &= ~(MKFLAG_CALIBRATE \| MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting
		116	ToNaviCtrl.Param.Byte[9] = (uint8_t)UBat;
		117	ToNaviCtrl.Param.Byte[10] = ParamSet.LowVoltageWarning;
		118	ToNaviCtrl.Param.Byte[11] = GetActiveParamSet();
886	killagreg	119	break;
		120
936	killagreg	121	case SPI_CMD_PARAMETER1:
1078	killagreg	122	ToNaviCtrl.Param.Byte[0] = ParamSet.NaviGpsModeControl; // Parameters for the Naviboard
		123	ToNaviCtrl.Param.Byte[1] = ParamSet.NaviGpsGain;
		124	ToNaviCtrl.Param.Byte[2] = ParamSet.NaviGpsP;
		125	ToNaviCtrl.Param.Byte[3] = ParamSet.NaviGpsI;
		126	ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsD;
		127	ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsACC;
936	killagreg	128	ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsMinSat;
		129	ToNaviCtrl.Param.Byte[7] = ParamSet.NaviStickThreshold;
1078	killagreg	130	ToNaviCtrl.Param.Byte[8] = ParamSet.NaviOperatingRadius;
		131	ToNaviCtrl.Param.Byte[9] = ParamSet.NaviWindCorrection;
		132	ToNaviCtrl.Param.Byte[10] = ParamSet.NaviSpeedCompensation;
		133	ToNaviCtrl.Param.Byte[11] = ParamSet.NaviAngleLimitation;
936	killagreg	134	break;
		135
		136
886	killagreg	137	case SPI_CMD_STICK:
911	killagreg	138	tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
886	killagreg	139	ToNaviCtrl.Param.Byte[0] = (int8_t) tmp;
		140	tmp = PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
		141	ToNaviCtrl.Param.Byte[1] = (int8_t) tmp;
		142	tmp = PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
		143	ToNaviCtrl.Param.Byte[2] = (int8_t) tmp;
911	killagreg	144	tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
886	killagreg	145	ToNaviCtrl.Param.Byte[3] = (int8_t) tmp;
		146	ToNaviCtrl.Param.Byte[4] = (uint8_t) Poti1;
		147	ToNaviCtrl.Param.Byte[5] = (uint8_t) Poti2;
		148	ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3;
		149	ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4;
		150	ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
		151	break;
		152
1078	killagreg	153	case SPI_CMD_MISC:
		154	ToNaviCtrl.Param.Byte[0] = CompassCalState;
		155	if(CompassCalState > 4)
		156	{ // jump from 5 to 0
		157	CompassCalState = 0;
886	killagreg	158	}
1078	killagreg	159	ToNaviCtrl.Param.Int[1] = ReadingHeight;
886	killagreg	160	break;
1078	killagreg	161
		162	case SPI_CMD_VERSION:
		163	ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major;
		164	ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor;
		165	ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch;
		166	ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible;
		167	break;
		168
		169	default:
		170	break;
886	killagreg	171	}
		172
		173
		174	sei(); // enable all interrupts
		175
		176	// analyze content of packet from NaviCtrl if valid
		177	if (SPI_RxDataValid)
		178	{
		179	// update gps controls
911	killagreg	180	if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
886	killagreg	181	{
936	killagreg	182	GPS_Nick = FromNaviCtrl.GPS_Nick;
		183	GPS_Roll = FromNaviCtrl.GPS_Roll;
886	killagreg	184	}
		185	// update compass readings
		186	if(FromNaviCtrl.CompassHeading <= 360)
		187	{
		188	CompassHeading = FromNaviCtrl.CompassHeading;
		189	}
		190	if(CompassHeading < 0) CompassOffCourse = 0;
		191	else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
		192	// NaviCtrl wants to beep?
		193	if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime;
		194
		195	switch (FromNaviCtrl.Command)
		196	{
1078	killagreg	197	case SPI_KALMAN:
		198	FromNaviCtrl_Value.Kalman_K = FromNaviCtrl.Param.Byte[0];
		199	FromNaviCtrl_Value.Kalman_MaxFusion = FromNaviCtrl.Param.Byte[1];
		200	FromNaviCtrl_Value.Kalman_MaxDrift = FromNaviCtrl.Param.Byte[2];
		201	break;
		202
		203
886	killagreg	204	case SPI_CMD_OSD_DATA:
1078	killagreg	205	FromNaviCtrl_Value.OsdBar = FromNaviCtrl.Param.Byte[0];
		206	FromNaviCtrl_Value.Distance = FromNaviCtrl.Param.Int[1];
886	killagreg	207	break;
		208
		209	case SPI_CMD_GPS_POS:
1078	killagreg	210	//FromFlightCtrl.Param.Long[0] = GPS_Data.Longitude;
		211	//FromFlightCtrl.Param.Long[1] = GPS_Data.Latitude;
886	killagreg	212	break;
		213
		214	case SPI_CMD_GPS_TARGET:
1078	killagreg	215	//FromFlightCtrl.Param.Long[0] = GPS_Data.TargetLongitude;
		216	//FromFlightCtrl.Param.Long[1] = GPS_Data.TargetLatitude;
886	killagreg	217	break;
		218
		219	default:
		220	break;
		221	}
		222	}
		223	else // no valid data from NaviCtrl
		224	{
		225	// disable GPS control
911	killagreg	226	GPS_Nick = 0;
886	killagreg	227	GPS_Roll = 0;
		228	}
304	ingob	229	}
		230
886	killagreg	231
		232
		233	/*********************************************/
		234	/* Start Transmission of packet to NaviCtrl */
		235	/*********************************************/
		236	void SPI_StartTransmitPacket(void)
304	ingob	237	{
708	ingob	238
886	killagreg	239	if (!SPITransferCompleted) return; // return immediately if transfer is in progress
		240	else // transmission was completed
		241	{
		242	SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave
691	ingob	243
886	killagreg	244	// cyclic commands
		245	ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
		246	if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
		247
936	killagreg	248	SPITransferCompleted = 0; // transfer is in progress
886	killagreg	249	UpdateSPI_Buffer(); // update data in ToNaviCtrl
		250
		251	SPI_TxBufferIndex = 1; //proceed with 2nd byte
		252
		253	// -- Debug-Output ---
		254	//----
		255	asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
		256	asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
		257	asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
		258	ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum
		259	SPDR = ToNaviCtrl.Sync1; // send first byte
		260	}
304	ingob	261	}
		262
886	killagreg	263	//------------------------------------------------------
		264	// This is the spi data transfer between FlightCtrl and NaviCtrl
		265	// Every time this routine is called within the mainloop one byte of the packet to
		266	// the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered
691	ingob	267
886	killagreg	268	void SPI_TransmitByte(void)
304	ingob	269	{
886	killagreg	270	static SPI_RXState_t SPI_RXState = SPI_SYNC1;
		271	uint8_t rxdata;
		272	static uint8_t rxchksum;
823	ingob	273
886	killagreg	274	if (SPITransferCompleted) return; // return immediatly if transfer was completed
		275	if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending
		276	SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop
823	ingob	277
886	killagreg	278	SLAVE_SELECT_PORT \|= (1 << SPI_SLAVE_SELECT); // DeselectSlave
823	ingob	279
886	killagreg	280	rxdata = SPDR; // save spi data register
823	ingob	281
886	killagreg	282	switch (SPI_RXState)
		283	{
		284	case SPI_SYNC1: // first sync byte
		285	SPI_RxBufferIndex = 0; // set pointer to start of rx buffer
		286	rxchksum = rxdata; // initialize checksum
		287	if (rxdata == SPI_RXSYNCBYTE1 )
		288	{ // 1st Syncbyte found
		289	SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte
		290	}
		291	break;
823	ingob	292
886	killagreg	293	case SPI_SYNC2: // second sync byte
		294	if (rxdata == SPI_RXSYNCBYTE2)
		295	{ // 2nd Syncbyte found
		296	rxchksum += rxdata; // update checksum
		297	SPI_RXState = SPI_DATA; // trigger to state for second sync byte
		298	}
		299	else // 2nd Syncbyte not found
		300	{
		301	SPI_RXState = SPI_SYNC1; // jump back to 1st sync byte
		302	}
		303	break;
823	ingob	304
886	killagreg	305	case SPI_DATA: // data bytes
		306	SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata; // copy data byte to spi buffer
		307	// if all bytes are received of a packet from the NaviCtrl
		308	if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)
		309	{ // last byte transfered is the checksum of the packet
		310	if (rxdata == rxchksum) // checksum matching?
		311	{
		312	// copy SPI_RxBuffer -> FromFlightCtrl
		313	uint8_t ptr = (uint8_t )&FromNaviCtrl;
		314	cli();
		315	memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl));
		316	sei();
		317	SPI_RxDataValid = 1;
1078	killagreg	318	//DebugOut.Analog[18]++;
886	killagreg	319	}
		320	else
		321	{ // checksum does not match
1078	killagreg	322	//DebugOut.Analog[17]++;
886	killagreg	323	SPI_RxDataValid = 0; // reset valid flag
		324	}
		325	SPI_RXState = SPI_SYNC1; // reset state sync
		326	}
		327	else // not all bytes transfered
		328	{
		329	rxchksum += rxdata; // update checksum
		330	}
		331	break;
		332	}// eof switch(SPI_RXState)
		333
		334	// if still some bytes left for transmission to NaviCtrl
		335	if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN)
		336	{
		337	SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave
		338	asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
		339	asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
		340	asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
		341
		342	SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte
		343	ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent
		344	SPI_TxBufferIndex++;
		345	}
		346	else
		347	{
		348	//Transfer of all bytes of the packet to NaviCtrl completed
		349	SPITransferCompleted = 1;
		350	}
304	ingob	351	}
		352
		353
		354

Subversion Repositories FlightCtrl

(root)/branches/V0.71h Code Redesign killagreg/spi.c @ 1204 - Rev 1078