#include <avr/io.h>
#include <avr/interrupt.h>
#include <string.h>
#include <stdlib.h>
#include "spi.h"
#include "rc.h"
#include "eeprom.h"
#include "timer0.h"
#include "analog.h"
#include "attitude.h"
#include "flight.h"
//-----------------------------------------
// for compatibility reasons gcc3.x <-> gcc4.x
// dongfang: Lets try to abandon GCC3.
/*
#ifndef SPCR
#define SPCR SPCR0
#endif
#ifndef SPIE
#define SPIE SPIE0
#endif
#ifndef SPE
#define SPE SPE0
#endif
#ifndef DORD
#define DORD DORD0
#endif
#ifndef MSTR
#define MSTR MSTR0
#endif
#ifndef CPOL
#define CPOL CPOL0
#endif
#ifndef CPHA
#define CPHA CPHA0
#endif
#ifndef SPR1
#define SPR1 SPR01
#endif
#ifndef SPR0
#define SPR0 SPR00
#endif
#ifndef SPDR
#define SPDR SPDR0
#endif
#ifndef SPSR
#define SPSR SPSR0
#endif
#ifndef SPIF
#define SPIF SPIF0
#endif
#ifndef WCOL
#define WCOL WCOL0
#endif
#ifndef SPI2X
#define SPI2X SPI2X0
#endif
*/
// -------------------------
#define DDR_SPI DDRB
#define DD_SS PB4
#define DD_SCK PB7
#define DD_MOSI PB5
#define DD_MISO PB6
#define SLAVE_SELECT_DDR_PORT DDRC
#define SLAVE_SELECT_PORT PORTC
#define SPI_SLAVE_SELECT PC5
#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
;
SPI_VersionInfo_t SPI_VersionInfo
;
// 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
= 0;
uint8_t NCDataOkay
= 0;
uint8_t NCSerialDataOkay
= 0;
uint8_t SPI_CommandSequence
[] = { SPI_CMD_USER
, SPI_CMD_STICK
,
SPI_CMD_PARAMETER1
, SPI_CMD_STICK
, SPI_CMD_MISC
, SPI_CMD_VERSION
};
uint8_t SPI_CommandCounter
= 0;
/*********************************************/
/* 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.
integralPitch = 0;
toNaviCtrl.
integralRoll = 0;
naviCtrlData.
serialDataOkay = 0;
//NCSerialDataOkay = 0;
//NCDataOkay = 0;
SPI_RxDataValid
= 0;
//SPI_VersionInfo.Major = VERSION_MAJOR;
//SPI_VersionInfo.Minor = VERSION_MINOR;
//SPI_VersionInfo.Patch = VERSION_PATCH;
//SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE;
}
/**********************************************************/
/* Update Data transfered by the SPI from/to NaviCtrl */
/**********************************************************/
void updateSPI_Buffer
(void) {
uint8_t i
;
int16_t tmp
;
cli
();
// stop all interrupts to avoid writing of new data during update of that packet.
// update content of packet to NaviCtrl
// Scaling?
toNaviCtrl.
integralPitch = (angle
[PITCH
] / (GYRO_DEG_FACTOR_PITCHROLL
/10)); // convert to multiple of 0.1�
// Scaling?
toNaviCtrl.
integralRoll = (angle
[ROLL
] / (GYRO_DEG_FACTOR_PITCHROLL
/10)); // convert to multiple of 0.1�
// Scaling?
toNaviCtrl.
gyroHeading = (yawGyroHeading
/ (GYRO_DEG_FACTOR_YAW
/ 10)); // convert to multiple of 0.1�
// Scaling?
toNaviCtrl.
gyroPitch = rate_ATT
[PITCH
];
// Scaling?
toNaviCtrl.
gyroRoll = rate_ATT
[ROLL
];
// Scaling?
toNaviCtrl.
gyroYaw = yawRate
;
// Scaling?
toNaviCtrl.
accPitch = getAngleEstimateFromAcc
(PITCH
) / (GYRO_DEG_FACTOR_PITCHROLL
/10); // convert to multiple of 0.1�
// Scaling?
toNaviCtrl.
AccRoll = getAngleEstimateFromAcc
(ROLL
) / (GYRO_DEG_FACTOR_PITCHROLL
/10);
// TODO: What are these little bastards?
averageAcc
[PITCH
] = averageAcc
[ROLL
] = averageAccCount
= 0;
switch (toNaviCtrl.
command) {
case SPI_CMD_USER
:
for (i
= 0; i
< sizeof(dynamicParams.
userParams); i
++) {
toNaviCtrl.
param.
asByte[i
] = dynamicParams.
userParams[i
];
}
toNaviCtrl.
Param.
Byte[8] = MKFlags
;
MKFlags
&= ~
(MKFLAG_CALIBRATE
| MKFLAG_START
); // calibrate and start are temporal states that are cleared immediately after transmitting
toNaviCtrl.
Param.
Byte[9] = getActiveParamSet
();
toNaviCtrl.
Param.
Byte[10] = 10; //EE_Parameter.ComingHomeAltitude;
toNaviCtrl.
Param.
Byte[11] = 0; //FC_StatusFlags2;
break;
case SPI_CMD_PARAMETER1
:
toNaviCtrl.
Param.
Byte[0] = staticParams.
NaviGpsModeControl; // Parameters for the Naviboard
toNaviCtrl.
Param.
Byte[1] = staticParams.
NaviGpsGain;
toNaviCtrl.
Param.
Byte[2] = staticParams.
NaviGpsP;
toNaviCtrl.
Param.
Byte[3] = staticParams.
NaviGpsI;
toNaviCtrl.
Param.
Byte[4] = staticParams.
NaviGpsD;
toNaviCtrl.
Param.
Byte[5] = staticParams.
NaviGpsACC;
toNaviCtrl.
Param.
Byte[6] = staticParams.
NaviGpsMinSat;
toNaviCtrl.
Param.
Byte[7] = staticParams.
NaviStickThreshold;
toNaviCtrl.
Param.
Byte[8] = staticParams.
NaviOperatingRadius;
toNaviCtrl.
Param.
Byte[9] = staticParams.
NaviWindCorrection;
toNaviCtrl.
Param.
Byte[10] = staticParams.
NaviSpeedCompensation;
toNaviCtrl.
Param.
Byte[11] = staticParams.
NaviAngleLimitation;
break;
case SPI_CMD_STICK
:
tmp
= PPM_in
[staticParams.
ChannelAssignment[CH_THROTTLE
]];
if (tmp
> 127)
tmp
= 127;
else if (tmp
< -128)
tmp
= -128;
toNaviCtrl.
Param.
Byte[0] = (int8_t) tmp
;
tmp
= PPM_in
[staticParams.
ChannelAssignment[CH_YAW
]];
if (tmp
> 127)
tmp
= 127;
else if (tmp
< -128)
tmp
= -128;
toNaviCtrl.
Param.
Byte[1] = (int8_t) tmp
;
tmp
= PPM_in
[staticParams.
ChannelAssignment[CH_ROLL
]];
if (tmp
> 127)
tmp
= 127;
else if (tmp
< -128)
tmp
= -128;
toNaviCtrl.
Param.
Byte[2] = (int8_t) tmp
;
tmp
= PPM_in
[staticParams.
ChannelAssignment[CH_PITCH
]];
if (tmp
> 127)
tmp
= 127;
else if (tmp
< -128)
tmp
= -128;
toNaviCtrl.
Param.
Byte[3] = (int8_t) tmp
;
toNaviCtrl.
Param.
Byte[4] = (uint8_t) variables
[0];
toNaviCtrl.
Param.
Byte[5] = (uint8_t) variables
[1];
toNaviCtrl.
Param.
Byte[6] = (uint8_t) variables
[2];
toNaviCtrl.
Param.
Byte[7] = (uint8_t) variables
[3];
toNaviCtrl.
Param.
Byte[8] = (uint8_t) RC_Quality
;
break;
case SPI_CMD_MISC
:
toNaviCtrl.
Param.
Byte[0] = compassCalState
;
if (compassCalState
> 4) { // jump from 5 to 0
compassCalState
= 0;
}
toNaviCtrl.
Param.
Byte[1] = staticParams.
NaviPHLoginTime;
// TODO: Height and in the correct scaling...
toNaviCtrl.
Param.
Int[1] = 0; //readingHeight; // at address of Byte 2 and 3
toNaviCtrl.
Param.
Byte[4] = staticParams.
NaviGpsPLimit;
toNaviCtrl.
Param.
Byte[5] = staticParams.
NaviGpsILimit;
toNaviCtrl.
Param.
Byte[6] = staticParams.
NaviGpsDLimit;
break;
case SPI_CMD_VERSION
:
toNaviCtrl.
Param.
Byte[0] = SPI_VersionInfo.
Major;
toNaviCtrl.
Param.
Byte[1] = SPI_VersionInfo.
Minor;
toNaviCtrl.
Param.
Byte[2] = SPI_VersionInfo.
Patch;
toNaviCtrl.
Param.
Byte[3] = SPI_VersionInfo.
Compatible;
toNaviCtrl.
Param.
Byte[4] = boardRelease
;
break;
default:
break;
}
sei
();
// enable all interrupts
// analyze content of packet from NaviCtrl if valid
if (SPI_RxDataValid
) {
// update gps controls
if (abs(fromNaviCtrl.
GPSStickPitch) < 512 && abs(fromNaviCtrl.
GPSStickRoll)
< 512 && (staticParams.
GlobalConfig & CFG_GPS_ACTIVE
)) {
GPSStickPitch
= fromNaviCtrl.
GPSStickPitch;
GPSStickRoll
= fromNaviCtrl.
GPSStickRoll;
NCDataOkay
= 250;
}
// 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_KALMAN
:
dynamicParams.
KalmanK = fromNaviCtrl.
Param.
Byte[0];
dynamicParams.
KalmanMaxFusion = fromNaviCtrl.
Param.
Byte[1];
dynamicParams.
KalmanMaxDrift = fromNaviCtrl.
Param.
Byte[2];
NCSerialDataOkay
= fromNaviCtrl.
Param.
Byte[3];
break;
default:
break;
}
} else { // no valid data from NaviCtrl
// disable GPS control
GPSStickPitch
= 0;
GPSStickRoll
= 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; // transfer 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;
} else { // checksum does not match
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;
}
}