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

Subversion Repositories FlightCtrl

(root)/branches/V0.70d CRK HexaLotte/spi.c - Rev 957