Subversion Repositories FlightCtrl

Rev

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