WebSVN - FlightCtrl - /branches/V0.72p Code Redesign killagreg/spi.c

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// + from this software without specific prior written permission.
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// + for non-commercial use (directly or indirectly)
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// + with our written permission
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// + clearly linked as origin
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <avr/io.h>
#include <avr/interrupt.h>
#include <string.h>
#include <stdlib.h>
#include "main.h"
#include "spi.h"
#include "fc.h"
#include "rc.h"
#include "eeprom.h"
#include "uart0.h"
#include "timer0.h"
#include "analog.h"

//-----------------------------------------
#define DDR_SPI DDRB
#define DD_SS PB4
#define DD_SCK PB7
#define DD_MOSI PB5
#define DD_MISO PB6

// for compatibility reasons gcc3.x <-> gcc4.x
#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 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;

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.IntegralNick = 0;
ToNaviCtrl.IntegralRoll = 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 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)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
ToNaviCtrl.IntegralRoll = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
ToNaviCtrl.GyroHeading = (int16_t)((10 * YawGyroHeading) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
ToNaviCtrl.GyroNick = GyroNick;
ToNaviCtrl.GyroRoll = GyroRoll;
ToNaviCtrl.GyroYaw = GyroYaw;
ToNaviCtrl.AccNick = ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
ToNaviCtrl.AccRoll = ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
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;
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] = (uint8_t)UBat;
ToNaviCtrl.Param.Byte[10] = ParamSet.LowVoltageWarning;
ToNaviCtrl.Param.Byte[11] = GetActiveParamSet();
break;

case SPI_CMD_PARAMETER1:
ToNaviCtrl.Param.Byte[0] = ParamSet.NaviGpsModeControl; // Parameters for the Naviboard
ToNaviCtrl.Param.Byte[1] = ParamSet.NaviGpsGain;
ToNaviCtrl.Param.Byte[2] = ParamSet.NaviGpsP;
ToNaviCtrl.Param.Byte[3] = ParamSet.NaviGpsI;
ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsD;
ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsACC;
ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsMinSat;
ToNaviCtrl.Param.Byte[7] = ParamSet.NaviStickThreshold;
ToNaviCtrl.Param.Byte[8] = ParamSet.NaviOperatingRadius;
ToNaviCtrl.Param.Byte[9] = ParamSet.NaviWindCorrection;
ToNaviCtrl.Param.Byte[10] = ParamSet.NaviSpeedCompensation;
ToNaviCtrl.Param.Byte[11] = ParamSet.NaviAngleLimitation;
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;
break;

case SPI_CMD_MISC:
ToNaviCtrl.Param.Byte[0] = CompassCalState;
if(CompassCalState > 4)
{ // jump from 5 to 0
CompassCalState = 0;
}
ToNaviCtrl.Param.Byte[1] = ParamSet.NaviPHLoginTime;
ToNaviCtrl.Param.Int[1] = ReadingHeight; // at address of Byte 2 and 3
ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsPLimit;
ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsILimit;
ToNaviCtrl.Param.Byte[6] = ParamSet.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.GPSStickNick) < 512 && abs(FromNaviCtrl.GPSStickRoll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
{
GPSStickNick = FromNaviCtrl.GPSStickNick;
GPSStickRoll = FromNaviCtrl.GPSStickRoll;
}
// 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:
FCParam.KalmanK = FromNaviCtrl.Param.Byte[0];
FCParam.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1];
FCParam.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2];
break;

default:
break;
}
}
else // no valid data from NaviCtrl
{
// disable GPS control
GPSStickNick = 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;
//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;
}
}

Subversion Repositories FlightCtrl

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