WebSVN - FlightCtrl - Blame - /branches/V0.69k 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"
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;
911	killagreg	70	ToNaviCtrl.IntegralNick = 0;
886	killagreg	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
911	killagreg	85	ToNaviCtrl.IntegralNick = (int16_t) (IntegralNick / 108);
886	killagreg	86	ToNaviCtrl.IntegralRoll = (int16_t) (IntegralRoll / 108);
		87	ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
911	killagreg	88	ToNaviCtrl.GyroNick = Reading_GyroNick;
886	killagreg	89	ToNaviCtrl.GyroRoll = Reading_GyroRoll;
		90	ToNaviCtrl.GyroYaw = Reading_GyroYaw;
911	killagreg	91	ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc);
886	killagreg	92	ToNaviCtrl.AccRoll = (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc);
911	killagreg	93	NaviCntAcc = 0; NaviAccNick = 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:
911	killagreg	109	tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
886	killagreg	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;
911	killagreg	115	tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
886	killagreg	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;
913	killagreg	122	ToNaviCtrl.Param.Byte[9] = (uint8_t) MotorsOn;
886	killagreg	123	break;
		124
		125	case SPI_CMD_CAL_COMPASS:
		126	if(CompassCalState > 5)
		127	{
		128	CompassCalState = 0;
		129	ToNaviCtrl.Param.Byte[0] = 5;
		130	}
		131	else
		132	{
		133	ToNaviCtrl.Param.Byte[0] = CompassCalState;
		134	}
		135	break;
		136	}
		137
		138
		139	sei(); // enable all interrupts
		140
		141	// analyze content of packet from NaviCtrl if valid
		142	if (SPI_RxDataValid)
		143	{
		144	// update gps controls
911	killagreg	145	if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
886	killagreg	146	{
911	killagreg	147	GPS_Nick = FromNaviCtrl.GPS_Nick;
886	killagreg	148	GPS_Roll = FromNaviCtrl.GPS_Roll;
		149	}
		150	// update compass readings
		151	if(FromNaviCtrl.CompassHeading <= 360)
		152	{
		153	CompassHeading = FromNaviCtrl.CompassHeading;
		154	}
		155	if(CompassHeading < 0) CompassOffCourse = 0;
		156	else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
		157	// NaviCtrl wants to beep?
		158	if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime;
		159
		160	switch (FromNaviCtrl.Command)
		161	{
		162	case SPI_CMD_OSD_DATA:
		163	// ToFlightCtrl.Param.Byte[0] = OsdBar;
		164	// ToFlightCtrl.Param.Int[1] = Distance;
		165	break;
		166
		167	case SPI_CMD_GPS_POS:
		168	// ToFlightCtrl.Param.Long[0] = GPS_Data.Longitude;
		169	// ToFlightCtrl.Param.Long[1] = GPS_Data.Latitude;
		170	break;
		171
		172	case SPI_CMD_GPS_TARGET:
		173	// ToFlightCtrl.Param.Long[0] = GPS_Data.TargetLongitude;
		174	// ToFlightCtrl.Param.Long[1] = GPS_Data.TargetLatitude;
		175	break;
		176
		177	default:
		178	break;
		179	}
		180	}
		181	else // no valid data from NaviCtrl
		182	{
		183	// disable GPS control
911	killagreg	184	GPS_Nick = 0;
886	killagreg	185	GPS_Roll = 0;
		186	}
304	ingob	187	}
		188
886	killagreg	189
		190
		191	/*********************************************/
		192	/* Start Transmission of packet to NaviCtrl */
		193	/*********************************************/
		194	void SPI_StartTransmitPacket(void)
304	ingob	195	{
708	ingob	196
886	killagreg	197	if (!SPITransferCompleted) return; // return immediately if transfer is in progress
		198	else // transmission was completed
		199	{
		200	SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave
691	ingob	201
886	killagreg	202	// cyclic commands
		203	ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
		204	if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
		205
		206	SPITransferCompleted = 0; // tranfer is in progress
		207	UpdateSPI_Buffer(); // update data in ToNaviCtrl
		208
		209	SPI_TxBufferIndex = 1; //proceed with 2nd byte
		210
		211	// -- Debug-Output ---
		212	//----
		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	asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
		216	ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum
		217	SPDR = ToNaviCtrl.Sync1; // send first byte
		218	}
304	ingob	219	}
		220
886	killagreg	221	//------------------------------------------------------
		222	// This is the spi data transfer between FlightCtrl and NaviCtrl
		223	// Every time this routine is called within the mainloop one byte of the packet to
		224	// the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered
691	ingob	225
886	killagreg	226	void SPI_TransmitByte(void)
304	ingob	227	{
886	killagreg	228	static SPI_RXState_t SPI_RXState = SPI_SYNC1;
		229	uint8_t rxdata;
		230	static uint8_t rxchksum;
823	ingob	231
886	killagreg	232	if (SPITransferCompleted) return; // return immediatly if transfer was completed
		233	if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending
		234	SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop
823	ingob	235
886	killagreg	236	SLAVE_SELECT_PORT \|= (1 << SPI_SLAVE_SELECT); // DeselectSlave
823	ingob	237
886	killagreg	238	rxdata = SPDR; // save spi data register
823	ingob	239
886	killagreg	240	switch (SPI_RXState)
		241	{
		242	case SPI_SYNC1: // first sync byte
		243	SPI_RxBufferIndex = 0; // set pointer to start of rx buffer
		244	rxchksum = rxdata; // initialize checksum
		245	if (rxdata == SPI_RXSYNCBYTE1 )
		246	{ // 1st Syncbyte found
		247	SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte
		248	}
		249	break;
823	ingob	250
886	killagreg	251	case SPI_SYNC2: // second sync byte
		252	if (rxdata == SPI_RXSYNCBYTE2)
		253	{ // 2nd Syncbyte found
		254	rxchksum += rxdata; // update checksum
		255	SPI_RXState = SPI_DATA; // trigger to state for second sync byte
		256	}
		257	else // 2nd Syncbyte not found
		258	{
		259	SPI_RXState = SPI_SYNC1; // jump back to 1st sync byte
		260	}
		261	break;
823	ingob	262
886	killagreg	263	case SPI_DATA: // data bytes
		264	SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata; // copy data byte to spi buffer
		265	// if all bytes are received of a packet from the NaviCtrl
		266	if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)
		267	{ // last byte transfered is the checksum of the packet
		268	if (rxdata == rxchksum) // checksum matching?
		269	{
		270	// copy SPI_RxBuffer -> FromFlightCtrl
		271	uint8_t ptr = (uint8_t )&FromNaviCtrl;
		272	cli();
		273	memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl));
		274	sei();
		275	SPI_RxDataValid = 1;
		276	DebugOut.Analog[18]++;
		277	}
		278	else
		279	{ // checksum does not match
		280	DebugOut.Analog[17]++;
		281	SPI_RxDataValid = 0; // reset valid flag
		282	}
		283	SPI_RXState = SPI_SYNC1; // reset state sync
		284	}
		285	else // not all bytes transfered
		286	{
		287	rxchksum += rxdata; // update checksum
		288	}
		289	break;
		290	}// eof switch(SPI_RXState)
		291
		292	// if still some bytes left for transmission to NaviCtrl
		293	if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN)
		294	{
		295	SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave
		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	asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
		299
		300	SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte
		301	ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent
		302	SPI_TxBufferIndex++;
		303	}
		304	else
		305	{
		306	//Transfer of all bytes of the packet to NaviCtrl completed
		307	SPITransferCompleted = 1;
		308	}
304	ingob	309	}
		310
		311
886	killagreg	312	#endif //USE_SPI_COMMUNICATION
304	ingob	313
886	killagreg	314

Subversion Repositories FlightCtrl

(root)/branches/V0.69k Code Redesign killagreg/spi.c @ 960 - Rev