WebSVN - FlightCtrl - Diff - Rev 1645 and 1775 - /branches/dongfang_FC

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + 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
+#ifndef SPI2X
 #define SPI2X  SPI2X0
-//#endif
+#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
-  {
+typedef enum {
-    SPI_SYNC1,
+  SPI_SYNC1,
-    SPI_SYNC2,
+  SPI_SYNC2,
-    SPI_DATA
+  SPI_DATA
-  } SPI_RXState_t;
 } SPI_RXState_t;
 // data exchange packets to and From NaviCtrl
+ToNaviCtrl_t                    toNaviCtrl;
-ToNaviCtrl_t                    ToNaviCtrl;
-FromNaviCtrl_t                  FromNaviCtrl;
+FromNaviCtrl_t                  fromNaviCtrl;
+SPI_VersionInfo_t               SPI_VersionInfo;
 SPI_VersionInfo_t SPI_VersionInfo;
 // rx packet buffer
-#define SPI_RXBUFFER_LEN sizeof(FromNaviCtrl)
+#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)
+#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
+  SPI_TxBuffer = (uint8_t *) &toNaviCtrl; // set pointer to tx-buffer
   SPITransferCompleted = 1;
   // initialize data packet to NaviControl
-  ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
+  toNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
-  ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
+  toNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
-  ToNaviCtrl.Command = SPI_CMD_USER;
+  toNaviCtrl.Command = SPI_CMD_USER;
-  ToNaviCtrl.IntegralNick = 0;
+  toNaviCtrl.IntegralPitch = 0;
-  ToNaviCtrl.IntegralRoll = 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 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
+  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.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.GyroHeading   = (int16_t)((10 * yawGyroHeading)   / GYRO_DEG_FACTOR_YAW);   // convert to multiple of 0.1°
-  ToNaviCtrl.GyroNick = rate_PID[PITCH]; // TODO: Which one should it be??
+  toNaviCtrl.GyroPitch = rate_ATT[PITCH];
-  ToNaviCtrl.GyroRoll = rate_PID[ROLL];
+  toNaviCtrl.GyroRoll = rate_ATT[ROLL];
-  ToNaviCtrl.GyroYaw =  yawRate;
+  toNaviCtrl.GyroYaw  = yawRate;
-  ToNaviCtrl.AccNick =  0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
+  toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) / GYRO_DEG_FACTOR_PITCHROLL; // 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°
+  toNaviCtrl.AccRoll  = (10 * getAngleEstimateFromAcc(ROLL))  / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1°
-  // naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0;
+  // TODO: What are these little bastards?
+  averageAcc[PITCH] = averageAcc[ROLL] = averageAccCount = 0;
-  switch(ToNaviCtrl.Command) {
+  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[i] = dynamicParams.UserParams[i];
     }
-    ToNaviCtrl.Param.Byte[8] = MKFlags;
+    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[9] = (uint8_t)UBat;
-    ToNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning;
+    toNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning;
-    ToNaviCtrl.Param.Byte[11] = getActiveParamSet();
+    toNaviCtrl.Param.Byte[11] = getActiveParamSet();
     break;
   case SPI_CMD_PARAMETER1:
-    ToNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl;     // Parameters for the Naviboard
+    toNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl;     // Parameters for the Naviboard
-    ToNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain;
+    toNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain;
-    ToNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP;
+    toNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP;
-    ToNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI;
+    toNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI;
-    ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD;
+    toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD;
-    ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC;
+    toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC;
-    ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat;
+    toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat;
-    ToNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold;
+    toNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold;
-    ToNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius;
+    toNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius;
-    ToNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection;
+    toNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection;
-    ToNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation;
+    toNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation;
-    ToNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation;
+    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;
+    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;
+    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;
+    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[3] = (int8_t) tmp;
-    ToNaviCtrl.Param.Byte[4] = (uint8_t) variables[0];
+    toNaviCtrl.Param.Byte[4] = (uint8_t) variables[0];
-    ToNaviCtrl.Param.Byte[5] = (uint8_t) variables[1];
+    toNaviCtrl.Param.Byte[5] = (uint8_t) variables[1];
-    ToNaviCtrl.Param.Byte[6] = (uint8_t) variables[2];
+    toNaviCtrl.Param.Byte[6] = (uint8_t) variables[2];
-    ToNaviCtrl.Param.Byte[7] = (uint8_t) variables[3];
+    toNaviCtrl.Param.Byte[7] = (uint8_t) variables[3];
-    ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
+    toNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
     break;
   case SPI_CMD_MISC:
-    ToNaviCtrl.Param.Byte[0] = compassCalState;
+    toNaviCtrl.Param.Byte[0] = compassCalState;
-    if(compassCalState > 4)
+    if(compassCalState > 4) { // jump from 5 to 0
-      { // jump from 5 to 0
-        compassCalState  = 0;
+          compassCalState  = 0;
-      }
+    }
-    ToNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime;
+    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.Int[1]  = 0; //readingHeight; // at address of Byte 2 and 3
-    ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit;
+    toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit;
-    ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit;
+    toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit;
-    ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit;
+    toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit;
     break;
   case SPI_CMD_VERSION:
-    ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major;
+    toNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major;
-    ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor;
+    toNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor;
-    ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch;
+    toNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch;
-    ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible;
+    toNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible;
-    ToNaviCtrl.Param.Byte[4] = BoardRelease;
+    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)) {
+    if(abs(fromNaviCtrl.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) {
-      GPSStickPitch     = FromNaviCtrl.GPSStickNick;
+      GPSStickPitch     = fromNaviCtrl.GPSStickPitch;
-      GPSStickRoll      = FromNaviCtrl.GPSStickRoll;
+      GPSStickRoll      = fromNaviCtrl.GPSStickRoll;
       NCDataOkay = 250;
+    }
     // update compass readings
-    if(FromNaviCtrl.CompassHeading <= 360) {
+    if(fromNaviCtrl.CompassHeading <= 360) {
-      compassHeading = FromNaviCtrl.CompassHeading;
+      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;
+    if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = fromNaviCtrl.BeepTime;
-    switch (FromNaviCtrl.Command) {
+    switch (fromNaviCtrl.Command) {
     case SPI_KALMAN:
-      dynamicParams.KalmanK = FromNaviCtrl.Param.Byte[0];
+      dynamicParams.KalmanK             = fromNaviCtrl.Param.Byte[0];
-      dynamicParams.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1];
+      dynamicParams.KalmanMaxFusion = fromNaviCtrl.Param.Byte[1];
-      dynamicParams.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2];
+      dynamicParams.KalmanMaxDrift      = fromNaviCtrl.Param.Byte[2];
-      NCSerialDataOkay = FromNaviCtrl.Param.Byte[3];
-      //DebugOut.Analog[29] = NCSerialDataOkay;
+      NCSerialDataOkay                          = fromNaviCtrl.Param.Byte[3];
       break;
     default:
       break;
+    }
-  }
-  else // no valid data from NaviCtrl
+  } 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++];
+      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
+      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
+      toNaviCtrl.Chksum = toNaviCtrl.Sync1; // init checksum
-      SPDR = ToNaviCtrl.Sync1; // send first byte
+      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 (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) {   // last byte transfered is the checksum of the packet
-        if (rxdata == rxchksum) // checksum matching?
+        if (rxdata == rxchksum) { // checksum matching?
-          {
             // copy SPI_RxBuffer -> FromFlightCtrl
-            uint8_t *ptr = (uint8_t *)&FromNaviCtrl;
+            uint8_t *ptr = (uint8_t *)&fromNaviCtrl;
             cli();
-            memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl));
+            memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(fromNaviCtrl));
             sei();
             SPI_RxDataValid = 1;
-            //DebugOut.Analog[18]++;
-          }
-        else
-          {   // checksum does not match
+          } else {   // checksum does not match
-            //DebugOut.Analog[17]++;
             SPI_RxDataValid = 0; // reset valid flag
+          }
-        SPI_RXState  = SPI_SYNC1; // reset state sync
+          SPI_RXState  = SPI_SYNC1; // reset state sync
-      }
-    else // not all bytes transfered
+    } else { // not all bytes transfered
-      {
-        rxchksum += rxdata; // update checksum
+          rxchksum += rxdata; // update checksum
-      }
+    }
     break;
   }// eof switch(SPI_RXState)
   // if still some bytes left for transmission to NaviCtrl
-  if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN)
+  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
+      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;
+    }
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/spi.c - Rev 1645 → 1775