Subversion Repositories FlightCtrl

Rev

Rev 1868 | 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 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;
        }
}