// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 example: 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"
//-----------------------------------------
// 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;
}
}