WebSVN - NaviCtrl - Diff - Rev 487 and 489

 // + www.MikroKopter.com
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Software Nutzungsbedingungen (english version: see below)
 // + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
 // + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
 // + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
 // + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
 // + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
 // + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
 // + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
 // + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
 // + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
 // + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
 // + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
 // + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
 // + des Mitverschuldens offen.
 // + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
 // + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
 // + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
 // + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
 // +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + Software LICENSING TERMS
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
 // + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
 // + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
 // + The Software may only be used with the Licensor's products.
 // + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
 // + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
 // + agreement shall be the property of the Licensor.
 // + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include <string.h>
+#include <math.h>
-#include <math.h>
+#include <stdlib.h>
 #include "91x_lib.h"
 #include "led.h"
 #include "gps.h"
 #include "uart1.h"
 #include "spi_slave.h"
 #include "compass.h"
 #include "timer1.h"
 #include "timer2.h"
 #include "config.h"
-#include "main.h"
-#include "compass.h"
+#include "main.h"
-#include "params.h"
-#include "stdlib.h"
+#include "params.h"
-#include "settings.h"
-#include "ncmag.h"
+#include "settings.h"
 #define SPI_RXSYNCBYTE1 0xAA
 #define SPI_RXSYNCBYTE2 0x83
 FromFlightCtrl_t   FromFlightCtrl;
 ToFlightCtrl_t     ToFlightCtrl;
 #define SPI0_TIMEOUT     2500  //ms
 volatile u32 SPI0_Timeout = 0;
 u8 Logging_FCStatusFlags1 = 0,Logging_FCStatusFlags2 = 0;
 u8 SpeakHoTT = 0;
 // tx packet buffer
 #define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization
 volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN + 10];
 u32 FC_I2C_ErrorConter;
 SPI_Version_t FC_Version;
 s16 POI_KameraNick = 0;
 u8 NC_Wait_for_LED = 0;
 s16 GyroCompassCorrected = 0; // corrected with the magnetic declination
-u8 FromFC_LowVoltageHomeActive = 0;
 //--------------------------------------------------------------
 void SSP0_IRQHandler(void)
+{
 void SPI0_UpdateBuffer(void)
+{
         static u32 timeout = 0;
         static u8 counter = 50,hott_index = 0, last_error_code = 0, enable_injecting = 0;
         static s16 last_wp_event = 0;
         s16 tmp;
         s32 i1,i2;
 /*
         union
+        {
                 unsigned char Byte[4];
                 unsigned int Int[2];
                 unsigned long Long;
         } Temp;
 */
     SPIWatchDog = 3500;    // stop communication to FC after this timeout
         if(SPI_RxBuffer_Request)
+        {
                 // avoid sending data via SPI during the update of the  ToFlightCtrl structure
                 VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
                 ToFlightCtrl.MagVecX = MagVector.X;
                 ToFlightCtrl.MagVecY = MagVector.Y;
                 ToFlightCtrl.MagVecZ = MagVector.Z;
 //              ToFlightCtrl.NCStatus = 0;
                 // cycle spi commands
                 if(ErrorCode != last_error_code && enable_injecting)
+                 {
                   ToFlightCtrl.Command = SPI_NCCMD_VERSION;
                   last_error_code = ErrorCode;
                   enable_injecting = 0;
+                 }
                 else
                 if(FC_WP_EventChannel != last_wp_event && enable_injecting)
+                 {
                   ToFlightCtrl.Command = SPI_NCCMD_GPSINFO;
                   last_wp_event = FC_WP_EventChannel;
                   enable_injecting = 0;
+                 }
                 else
+                 {
                   ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
                 // restart command cycle at the end
                   if(SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
                   if(ToFlightCtrl.Command == SPI_NCCMD_KALMAN) enable_injecting = 1;
                                         POI_KameraNick = tmp;
+                                }
                                 else
+                                {
                                         //if(FC.StatusFlags2 & FC_STATUS2_CAREFREE) // only, if carefree is active
                                         POI_KameraNick = CAM_Orientation.Elevation;
                                         //else ToFlightCtrl.Param.sInt[5] = 0;
+                                }
                                 ToFlightCtrl.Param.sInt[5] = POI_KameraNick;
                                 break;
                         case SPI_NCCMD_HOTT_INFO:
                                 switch(hott_index++)
+                                {
                                 case 0:
                                         //Dezimalgrad            --> Grad mit Dezimalminuten     --> Grad, Minuten, Sekunden
                                         //53.28 5788 7.4847269   --> N53° 17.14728 E7° 29.08362  --> N53° 17' 8.837" E7° 29' 5.017"
                                         ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
                                         ToFlightCtrl.Param.Byte[0] = 3+3;       // index  +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
                                         ToFlightCtrl.Param.Byte[1] = 9-1;       // how many
                                         //-----------------------------
                                         ToFlightCtrl.Param.Byte[2] = GyroCompassCorrected / 20;//NaviData.HomePositionDeviation.Bearing / 2;
                                         i1 = GPSData.Speed_Ground; // in cm/sec
                                         i1 *= 36;
                                         i1 /= 1000;
                                         ToFlightCtrl.Param.Byte[3] = i1 % 256;
                                         ToFlightCtrl.Param.Byte[4] = i1 / 256;
                                         if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5]  = 1; // 1 = S
                                         else ToFlightCtrl.Param.Byte[5]  = 0; // 1 = S
                                         i1 = abs(GPSData.Position.Latitude)/10000000L;
                                         i2 = abs(GPSData.Position.Latitude)%10000000L;
                                         if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;}
                                         i1 *= 100;
                                 // Minuten
                                 i2 *= 6;
                                 i2 /= 10;
                                         i1 += i2 / 100000;
                                         i2  = i2 % 100000;
                                         if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2]  = 1; // 1 = E
                                         else ToFlightCtrl.Param.Byte[2]  = 0; // 1 = S
                                         i1 = abs(GPSData.Position.Longitude)/10000000L;
                                         i2 = abs(GPSData.Position.Longitude)%10000000L;
                                         if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;}
                                         i1 *= 100;
                                 // Minuten
                                 i2 *= 6;
                                 i2 /= 10;
                                         i1 += i2 / 100000;
                                         i2  = i2 % 100000;
                                         ToFlightCtrl.Param.Byte[3]  = i1 % 256;
                                         ToFlightCtrl.Param.Byte[4]  = i1 / 256;
                                         ToFlightCtrl.Param.Byte[5]  = i2 % 256;
                                         ToFlightCtrl.Param.Byte[6]  = i2 / 256;
                                         //-----------------------------
                                         i1 = NaviData.HomePositionDeviation.Distance / 10; // dann in m
                                         ToFlightCtrl.Param.Byte[7]  = i1 % 256;
                                         ToFlightCtrl.Param.Byte[8]  = i1 / 256;
                                         break;
                                  case 2:
                                         ToFlightCtrl.Param.Byte[11] = HOTT_GENERAL_PACKET_ID;
                                         ToFlightCtrl.Param.Byte[0] = 5+3;       // index   // +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
                                         ToFlightCtrl.Param.Byte[1] = 2; // how many
                                         ToFlightCtrl.Param.Byte[2] = EarthMagneticField / (5 * 2);
                                         ToFlightCtrl.Param.Byte[3] = EarthMagneticInclination / 2;
                                         break;
                                  case 3:
                                         ToFlightCtrl.Param.Byte[11] = JETI_GPS_PACKET_ID1;
                                         ToFlightCtrl.Param.Byte[0] = 0; // index
                                         ToFlightCtrl.Param.Byte[1] = 4; // how many
                                         //JetiExData[14].Value =  53 * 0x10000 + 23467;
                                         if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5] = 0x40;
                                         else ToFlightCtrl.Param.Byte[5] = 0x00;
                                         ToFlightCtrl.Param.Byte[4] = abs(GPSData.Position.Latitude)/10000000L;
                                         i2 = abs(GPSData.Position.Latitude)%10000000L;
                                         i2 *= 6;
                                         i2 /= 1000;
                                         ToFlightCtrl.Param.Byte[3] = i2 / 256;
                                         ToFlightCtrl.Param.Byte[2] = i2 % 256;
                                         break;
                                  case 4:
                                         ToFlightCtrl.Param.Byte[11] = JETI_GPS_PACKET_ID2;
                                         ToFlightCtrl.Param.Byte[0] = 0; // index
                                         ToFlightCtrl.Param.Byte[1] = 4; // how many
                                         if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5] = 0x60;
                                         else ToFlightCtrl.Param.Byte[5] = 0x20;
                                         ToFlightCtrl.Param.Byte[4] = abs(GPSData.Position.Longitude)/10000000L;
                                         i2 = abs(GPSData.Position.Longitude)%10000000L;
                                         i2 *= 6;
                                         i2 /= 1000;
                                         ToFlightCtrl.Param.Byte[3] = i2 / 256;
                                         ToFlightCtrl.Param.Byte[2] = i2 % 256;
                         hott_index = 0;
                                         break;
                                  default:
                                         ToFlightCtrl.Param.Byte[0] = 255;
                                         hott_index = 0;
                                         break;
+                                }
                                 break;
                         default:
                                 break;
                                         FCCalibActive = 10;
                                         FC_is_Calibrated = 0;
+                                }
                                 else
+                                {
                                         if(FCCalibActive)
-                                         {
+                                        {
-                                            if(--FCCalibActive == 0)
+                                                if(--FCCalibActive == 0)
-                                                 {
+                                                {
-                                                  FC_is_Calibrated = 1;
+                                                        FC_is_Calibrated = 1;
-                                                  ExtCompassOrientation = GetExtCompassOrientation();
+                                                        Compass_Check();
-if(ExtCompassOrientation != Calibration.Version / 16) NCMAG_IsCalibrated = 0;
-                                                 }
+                                                }
-                                         }
+                                        }
+                                }
+                                if(FC.StatusFlags & FC_STATUS_START)
+                                {
+                                        if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else
+                                        HeadFreeStartAngle = GyroCompassCorrected;
+                                }
-                                if(FC.StatusFlags & FC_STATUS_START)
-                             {
-                                   if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 - GeoMagDec) % 3600; else
-                                   HeadFreeStartAngle = GyroCompassCorrected;
-                                 }
-                                 if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
+                                if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
-                                  {
+                                {
-                                   if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
+                                        if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
-                                    {
+                                        {
-                                         if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK))       // nur bei ausreichender Distance -> 20m
+                                                if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK))        // nur bei ausreichender Distance -> 20m
-                                         {
+                                                {
-                                          HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 -  Parameter.OrientationAngle * 150) % 3600; // in 0.1°
+                                                        HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 -  Parameter.OrientationAngle * 150) % 3600; // in 0.1°
-                                         }
+                                                }
-                                         else                                                                                            // Ansonsten die aktuelle Richtung übernehmen
+                                                else                                                                                             // Ansonsten die aktuelle Richtung übernehmen
-                                          HeadFreeStartAngle = GyroCompassCorrected; // in 0.1°
+                                                HeadFreeStartAngle = GyroCompassCorrected; // in 0.1°
+                                        }
+                                }
                                 Parameter.ActiveSetting = FromFlightCtrl.Param.Byte[9];
                                 FC.BAT_Voltage = FromFlightCtrl.Param.Byte[10];
                                 DebugOut.Analog[7] = FC.BAT_Voltage;
                                 DebugOut.Analog[5] = FC.StatusFlags;
                                 NaviData.FCStatusFlags = FC.StatusFlags;
                                 if(FC.StatusFlags2 & FC_STATUS2_WAIT_FOR_TAKEOFF) NaviData.FCStatusFlags &= ~FC_STATUS_FLY;
                                 FC.StatusFlags2 = FromFlightCtrl.Param.Byte[11];
                                 NaviData.FCStatusFlags2 = (NaviData.FCStatusFlags2 & (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)) | (FC.StatusFlags2 & (0xff - (FC_STATUS2_OUT1_ACTIVE | FC_STATUS2_OUT2_ACTIVE)));
                                 if((!(LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) NaviData.FCStatusFlags2 |= FC_STATUS2_OUT1_ACTIVE;
                                 else
                                 if(((LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) && !(FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE)) NaviData.FCStatusFlags2 &= ~FC_STATUS2_OUT1_ACTIVE;
                                 if((!(LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) && (FC.StatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) NaviData.FCStatusFlags2 |= FC_STATUS2_OUT2_ACTIVE;
                                 else
                                 if(((LastTransmittedFCStatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) && !(FC.StatusFlags2 & FC_STATUS2_OUT2_ACTIVE)) NaviData.FCStatusFlags2 &= ~FC_STATUS2_OUT2_ACTIVE;
                                 break;
                         case SPI_FCCMD_PARAMETER2:
                                 CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255);
                                 if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1];     // will be cleared in the SD-Logging
                 Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[2];
                                 Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3];
-                                FromFC_LowVoltageHomeActive = FromFlightCtrl.Param.Byte[4];
+                                Parameter.FromFC_LowVoltageHomeActive = FromFlightCtrl.Param.Byte[4];
                                 break;
                         case SPI_FCCMD_STICK:
                                 FC.StickGas     = FromFlightCtrl.Param.sByte[0];
                                 FC.StickYaw     = FromFlightCtrl.Param.sByte[1];
                                 FC.StickRoll    = FromFlightCtrl.Param.sByte[2];
                                 break;
                         case SPI_FCCMD_MISC:
                                 if(CompassCalState != FromFlightCtrl.Param.Byte[0])
                                 {       // put only new CompassCalState into queue to send via I2C
 //                                      if(FromFlightCtrl.Param.Byte[0] == CompassCalState+1 || FromFlightCtrl.Param.Byte[0] == 0)
+                                         {
                                            CompassCalState = FromFlightCtrl.Param.Byte[0];
                                            Compass_SetCalState(CompassCalState);
+                                         }
                                 FC_Version.Patch                = FromFlightCtrl.Param.Byte[2];
                                 FC_Version.Compatible   = FromFlightCtrl.Param.Byte[3];
                                 FC_Version.Hardware             = FromFlightCtrl.Param.Byte[4];
                                 FC.Error[0]                     |= FromFlightCtrl.Param.Byte[5];
                                 FC.Error[1]                     |= FromFlightCtrl.Param.Byte[6];
                                 if(FromFlightCtrl.Param.Byte[7] >= 68 && FromFlightCtrl.Param.Byte[7] <= 188)
+                                {
-                                 FC.FromFC_DisableDeclination = 1;
+                                        FC.FromFC_DisableDeclination = 1;
-                                 FC.FromFC_CompassOffset = 10 * (signed char) ((unsigned char) FromFlightCtrl.Param.Byte[7] - 128);
+                                        FC.FromFC_CompassOffset = 10 * (s8) (FromFlightCtrl.Param.Byte[7] - 128);
-                                 GeoMagDec = 0;
+                                        GeoMagDec = 0;
                                 }
                                 else
+                                {
-                                 FC.FromFC_DisableDeclination = 0;
+                                        FC.FromFC_DisableDeclination = 0;
-                                 FC.FromFC_CompassOffset = 10 * (signed char) FromFlightCtrl.Param.Byte[7];
+                                        FC.FromFC_CompassOffset = 10 * FromFlightCtrl.Param.sByte[7];
+                                }
                                 Parameter.GlobalConfig = FromFlightCtrl.Param.Byte[8];
                                 Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
                                 Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
                                 Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
                         SPI0_UpdateBuffer();
                         if (FC_Version.Major != 0xFF)  break;
                 }while (!CheckDelay(timeout));
                 UART1_PutString(".");
                 repeat++;
-//              FCCalibActive = 1;
+                FCCalibActive = 1;
         }while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
         // if we got it
         if (FC_Version.Major != 0xFF)
+        {
-                sprintf(msg, " FC V%d.%02d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
+                sprintf(msg, " FC V%d.%d%c HW:%d.%02d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
                 UART1_PutString(msg);
+        }
         else UART1_PutString("\n\r not found!");
+}
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// + extended Current measurement -> 200 = 20A    201 = 21A    255 = 75A (20+55)
+// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+u16 BL3_Current(u8 who) // in 0,1A
+{
+        if(Motor[who].Current <= 200) return((u16) Motor[who].Current);
+        else
+        {
+                if(Motor_Version[who] & MOTOR_STATE_BL30) return(200 + 10 * (u16) (Motor[who].Current - 200));
+                else return((u16) Motor[who].Current);
+        }

Subversion Repositories NaviCtrl

(root)/tags/V2.14d/spi_slave.c @ 811 - Rev 487 → 489