0,0 → 1,421 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 "spi.h" |
#include "rc.h" |
#include "eeprom.h" |
#include "uart0.h" |
#include "timer0.h" |
#include "analog.h" |
#include "attitude.h" |
#include "GPSControl.h" |
#include "flight.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 = 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; |
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 transferd 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 |
toNaviCtrl.IntegralPitch = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
toNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
toNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
toNaviCtrl.GyroPitch = rate_ATT[PITCH]; |
toNaviCtrl.GyroRoll = rate_ATT[ROLL]; |
toNaviCtrl.GyroYaw = yawRate; |
toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
toNaviCtrl.AccRoll = (10 * getAngleEstimateFromAcc(ROLL)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
|
|
// 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.Byte[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] = (uint8_t)UBat; |
toNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning; |
toNaviCtrl.Param.Byte[11] = getActiveParamSet(); |
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; |
} |
} |