Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + 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;

    }

}