Blame |
Last modification |
View Log
| RSS feed
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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.
IntegralNick = 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.
IntegralNick = (int16_t)((10 * pitchAngle
) / GYRO_DEG_FACTOR_PITCHROLL
); // convert to multiple of 0.1°
ToNaviCtrl.
IntegralRoll = (int16_t)((10 * rollAngle
) / 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.
GyroNick = pitchRate_PID
; // TODO: Which one should it be??
ToNaviCtrl.
GyroRoll = rollRate_PID
;
ToNaviCtrl.
GyroYaw = yawRate
;
ToNaviCtrl.
AccNick = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
ToNaviCtrl.
AccRoll = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
naviCntAcc
= 0; naviAccPitch
= 0; naviAccRoll
= 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;
ToNaviCtrl.
Param.
Int[1] = 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.
GPSStickNick) < 512 && abs(FromNaviCtrl.
GPSStickRoll) < 512 && (staticParams.
GlobalConfig & CFG_GPS_ACTIVE
)) {
GPSStickPitch
= FromNaviCtrl.
GPSStickNick;
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];
//DebugOut.Analog[29] = NCSerialDataOkay;
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;
//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;
}
}