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

Subversion Repositories FlightCtrl

(root)/branches/V0.72p Code Redesign killagreg/spi.c @ 1194 - Rev 1179