WebSVN - FlightCtrl - Diff - Rev 903 and 916 - /branches/V0.69k CRK HexaLotte/spi.c


// ######################## SPI - FlightCtrl ###################
#include <avr/io.h>
#include <avr/interrupt.h>
#include <string.h>
#include <stdlib.h>
#include "_Settings.h"
#include "spi.h"
#include "fc.h"
#include "rc.h"
#include "eeprom.h"
#include "uart.h"
#include "timer0.h"
 
#define SPI_TXSYNCBYTE1 0xAA
#define SPI_TXSYNCBYTE2 0x83
#define SPI_RXSYNCBYTE1 0x81
#define SPI_RXSYNCBYTE2 0x55
 
typedef enum
{
        SPI_SYNC1,
        SPI_SYNC2,
        SPI_DATA
} SPI_RXState_t;
 
 
// data exchange packets to and From NaviCtrl
ToNaviCtrl_t    ToNaviCtrl;
FromNaviCtrl_t  FromNaviCtrl;
 
// rx packet buffer
#define SPI_RXBUFFER_LEN sizeof(FromNaviCtrl)
uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN];
uint8_t SPI_RxBufferIndex = 0;
uint8_t SPI_RxBuffer_Request = 0;
 
// tx packet buffer
#define SPI_TXBUFFER_LEN sizeof(ToNaviCtrl)
uint8_t *SPI_TxBuffer;
uint8_t SPI_TxBufferIndex = 0;
 
uint8_t SPITransferCompleted, SPI_ChkSum;
uint8_t SPI_RxDataValid;
 
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_CAL_COMPASS };
uint8_t SPI_CommandCounter = 0;
 
#ifdef USE_SPI_COMMUNICATION
 
/*********************************************/
/*  Initialize SPI interface to NaviCtrl     */
/*********************************************/
void SPI_MasterInit(void)
{
        DDR_SPI |= (1<<DD_MOSI)|(1<<DD_SCK);    // Set MOSI and SCK output, all others input
        SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port
 
        SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE);   // Enable SPI, Master, set clock rate fck/64
        SPSR = 0;//(1<<SPI2X);
 
        SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT); // Deselect Slave
 
        SPI_TxBuffer = (uint8_t *) &ToNaviCtrl; // set pointer to tx-buffer
        SPITransferCompleted = 1;
        // initialize data packet to NaviControl
        ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
        ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
 
        ToNaviCtrl.Command = SPI_CMD_USER;
        ToNaviCtrl.IntegralNick = 0;
        ToNaviCtrl.IntegralRoll = 0;
        SPI_RxDataValid = 0;
}
 
 
/**********************************************************/
/*  Update Data transferd by the SPI from/to NaviCtrl     */
/**********************************************************/
void UpdateSPI_Buffer(void)
{
        int16_t tmp;
        cli(); // stop all interrupts to avoid writing of new data during update of that packet.
 
        // update content of packet to NaviCtrl
        ToNaviCtrl.IntegralNick = (int16_t) (IntegralNick / 108);
        ToNaviCtrl.IntegralRoll  = (int16_t) (IntegralRoll  / 108);
        ToNaviCtrl.GyroHeading = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
        ToNaviCtrl.GyroNick = Reading_GyroNick;
        ToNaviCtrl.GyroRoll = Reading_GyroRoll;
        ToNaviCtrl.GyroYaw = Reading_GyroYaw;
        ToNaviCtrl.AccNick = (int16_t) ACC_AMPLIFY * (NaviAccNick / NaviCntAcc);
        ToNaviCtrl.AccRoll =  (int16_t) ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc);
        NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0;
 
        switch(ToNaviCtrl.Command)
        {
                case SPI_CMD_USER:
                        ToNaviCtrl.Param.Byte[0] = FCParam.UserParam1;
                        ToNaviCtrl.Param.Byte[1] = FCParam.UserParam2;
                        ToNaviCtrl.Param.Byte[2] = FCParam.UserParam3;
                        ToNaviCtrl.Param.Byte[3] = FCParam.UserParam4;
                        ToNaviCtrl.Param.Byte[4] = FCParam.UserParam5;
                        ToNaviCtrl.Param.Byte[5] = FCParam.UserParam6;
                        ToNaviCtrl.Param.Byte[6] = FCParam.UserParam7;
                        ToNaviCtrl.Param.Byte[7] = FCParam.UserParam8;
                        break;
 
                case SPI_CMD_STICK:
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]];  if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
                        ToNaviCtrl.Param.Byte[0] = (int8_t) tmp;
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
                        ToNaviCtrl.Param.Byte[1] = (int8_t) tmp;
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
                        ToNaviCtrl.Param.Byte[2] = (int8_t) tmp;
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
                        ToNaviCtrl.Param.Byte[3] = (int8_t) tmp;
                        ToNaviCtrl.Param.Byte[4] = (uint8_t) Poti1;
                        ToNaviCtrl.Param.Byte[5] = (uint8_t) Poti2;
                        ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3;
                        ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4;
                        ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
                        ToNaviCtrl.Param.Byte[9] = (uint8_t) MotorsOn;
                        break;
 
                case SPI_CMD_CAL_COMPASS:
                        if(CompassCalState > 5)
                        {
                                CompassCalState = 0;
                                ToNaviCtrl.Param.Byte[0] = 5;
                        }
                        else
                        {
                                ToNaviCtrl.Param.Byte[0] = CompassCalState;
                        }
                        break;
        }
 
 
        sei(); // enable all interrupts
 
        // analyze content of packet from NaviCtrl if valid
        if (SPI_RxDataValid)
        {
                // update gps controls
                if(abs(FromNaviCtrl.GPS_Nick) < 512 && abs(FromNaviCtrl.GPS_Roll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
                {
                        GPS_Nick = FromNaviCtrl.GPS_Nick;
                        GPS_Roll  = FromNaviCtrl.GPS_Roll;
                }
                // update compass readings
                if(FromNaviCtrl.CompassHeading <= 360)
                {
                        CompassHeading = FromNaviCtrl.CompassHeading;
                }
                if(CompassHeading < 0) CompassOffCourse = 0;
                else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
        // NaviCtrl wants to beep?
                if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime;
 
                switch (FromNaviCtrl.Command)
                {
                        case  SPI_CMD_OSD_DATA:
                                //                                ToFlightCtrl.Param.Byte[0] = OsdBar;
                                //                                ToFlightCtrl.Param.Int[1]  = Distance;
                                break;
 
                        case  SPI_CMD_GPS_POS:
                                //                                ToFlightCtrl.Param.Long[0]  = GPS_Data.Longitude;
                                //                                ToFlightCtrl.Param.Long[1]  = GPS_Data.Latitude;
                                break;
 
                        case  SPI_CMD_GPS_TARGET:
                                //                                ToFlightCtrl.Param.Long[0]  = GPS_Data.TargetLongitude;
                                //                                ToFlightCtrl.Param.Long[1]  = GPS_Data.TargetLatitude;
                                break;
 
                        default:
                                break;
                }
        }
        else // no valid data from NaviCtrl
        {
                // disable GPS control
                GPS_Nick = 0;
                GPS_Roll = 0;
        }
}
 
 
 
/*********************************************/
/*  Start Transmission of packet to NaviCtrl */
/*********************************************/
void SPI_StartTransmitPacket(void)
{
 
        if (!SPITransferCompleted) return; // return immediately if transfer is in progress
        else // transmission was completed
        {
                SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // Select slave
 
                // cyclic commands
                ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
 
                SPITransferCompleted = 0; // tranfer is in progress
                UpdateSPI_Buffer();    // update data in ToNaviCtrl
 
                SPI_TxBufferIndex = 1; //proceed with 2nd byte
 
                // -- Debug-Output ---
                //----
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");            asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");            asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");            asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
                ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum
                SPDR = ToNaviCtrl.Sync1; // send first byte
        }
}
 
//------------------------------------------------------
// This is the spi data transfer between FlightCtrl and NaviCtrl
// Every time this routine is called within the mainloop one byte of the packet to
// the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered
 
void SPI_TransmitByte(void)
{
        static SPI_RXState_t SPI_RXState = SPI_SYNC1;
        uint8_t rxdata;
        static uint8_t rxchksum;
 
        if (SPITransferCompleted) return;  // return immediatly if transfer was completed
        if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending
        SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop
 
        SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT);   // DeselectSlave
 
        rxdata = SPDR; // save spi data register
 
        switch (SPI_RXState)
        {
                case SPI_SYNC1: // first sync byte
                        SPI_RxBufferIndex = 0; // set pointer to start of rx buffer
                        rxchksum = rxdata; // initialize checksum
                        if (rxdata == SPI_RXSYNCBYTE1 )
                        {       // 1st Syncbyte found
                                SPI_RXState  = SPI_SYNC2; // trigger to state for second sync byte
                        }
                        break;
 
                case SPI_SYNC2: // second sync byte
                        if (rxdata == SPI_RXSYNCBYTE2)
                        {       // 2nd Syncbyte found
                                rxchksum += rxdata; // update checksum
                                SPI_RXState  = SPI_DATA;   // trigger to state for second sync byte
                        }
                        else // 2nd Syncbyte not found
                        {
                                SPI_RXState  = SPI_SYNC1; // jump back to 1st sync byte
                        }
                        break;
 
                case SPI_DATA: // data bytes
                        SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata;  // copy data byte to spi buffer
                        // if all bytes are received of a packet from the NaviCtrl
                        if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)
                        {   // last byte transfered is the checksum of the packet
                                if (rxdata == rxchksum) // checksum matching?
                                {
                                        // copy SPI_RxBuffer -> FromFlightCtrl
                                        uint8_t *ptr = (uint8_t *)&FromNaviCtrl;
                                        cli();
                                        memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl));
                                        sei();
                                        SPI_RxDataValid = 1;
                                        DebugOut.Analog[18]++;
                                }
                                else
                                {   // checksum does not match
                                        DebugOut.Analog[17]++;
                                        SPI_RxDataValid = 0; // reset valid flag
                                }
                                SPI_RXState  = SPI_SYNC1; // reset state sync
                        }
                        else // not all bytes transfered
                        {
                                rxchksum += rxdata; // update checksum
                        }
                        break;
        }// eof switch(SPI_RXState)
 
    // if still some bytes left for transmission to NaviCtrl
        if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN)
        {
                SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // SelectSlave
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
 
                SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte
                ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent
                SPI_TxBufferIndex++;
        }
        else
        {
                //Transfer of all bytes of the packet to NaviCtrl completed
                SPITransferCompleted = 1;
        }
}
 
 
#endif //USE_SPI_COMMUNICATION
 
 
 

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

Subversion Repositories FlightCtrl

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