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

Subversion Repositories FlightCtrl

(root)/branches/V0.69k CRK HexaLotte/spi.c - Rev 903