WebSVN - NaviCtrl - Diff - Rev 329 and 330

 #define SPI_COMMAND_INDEX 0
 s32 Kalman_K = 32;
 s32 Kalman_MaxDrift = 5 * 16;
+s32 Kalman_MaxFusion = 64;
-s32 Kalman_MaxFusion = 64;
+s32 Kalman_Kompass = 32;
 s32 ToFcGpsZ = 0;
 u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN};
 s32 ToFC_AltitudeSetpoint = 0;
 u8  FromFC_VarioCharacter = ' ';
 u8 DisableFC_Sticks = 0;
 u8 NC_GPS_ModeCharacter = ' ';
 u8 FCCalibActive = 0;
+u8 FC_is_Calibrated = 0;
 SPI_Version_t FC_Version;
                         case  SPI_NCCMD_KALMAN:  // wird am häufigsten betätigt
                                 CalcHeadFree();
                                 ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
                                 ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
                                 ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
-                                ToFlightCtrl.Param.Byte[3]      = (u8) SerialLinkOkay;
+                                ToFlightCtrl.Param.Byte[3]      = (u8) Kalman_Kompass;
                                 ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ;
                                 ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C;
                                 ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S;
                 if(DisableFC_Sticks) ToFlightCtrl.Param.Byte[7] = FLAG_GPS_AID; else ToFlightCtrl.Param.Byte[7] = 0x00;
                                 if(CAM_Orientation.UpdateMask & CAM_UPDATE_AZIMUTH)
                                 ToFlightCtrl.Param.Byte[4] = Version_HW;
                                 ToFlightCtrl.Param.Byte[5] = DebugOut.Status[0];
                                 ToFlightCtrl.Param.Byte[6] = DebugOut.Status[1];
                                 ToFlightCtrl.Param.Byte[7] = ErrorCode;
                                 ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter;
+                                ToFlightCtrl.Param.Byte[9] = SerialLinkOkay;
                                 break;
                         case SPI_NCCMD_GPSINFO:
                                 ToFlightCtrl.Param.Byte[0] = GPSData.Flags;
+                                }
                                 else
+                                {
                                         ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint;
+                                }
-//                              DebugOut.Analog[25] = (s16)ToFlightCtrl.Param.Byte[9];
-//                              DebugOut.Analog[20] = ToFlightCtrl.Param.sInt[5];
                                 break;
-                        case SPI_NCCMD_HOTT_INFO:
-                                switch(hott_index++)
-                                {
 /*
 typedef struct
+{
   unsigned char StartByte;  //0         // 0x7C
   unsigned char Packet_ID;  //1         // 0x89  - Vario ID
   unsigned char NullByte;       // 0x00
   unsigned char NullByte1;      // 0x00
   unsigned char EndByte;                // 0x7D
 } GPSPacket_t;
 */
+/*
+typedef struct
+{
+  unsigned char StartByte;      //0 0x7C
+  unsigned char Packet_ID;      //1 HOTT_GENERAL_PACKET_ID
+  unsigned char WarnBeep;       //2 Anzahl der Töne 0..36
+  unsigned char VoltageCell1;   //3 208 = 4,16V  (Voltage * 50 = Wert)
+  unsigned char VoltageCell2;   //4
+  unsigned char VoltageCell3;   //5
+  unsigned char VoltageCell4;   //6
+  unsigned char VoltageCell5;   //7
+  unsigned char VoltageCell6;   //8
+  unsigned int  Battery1;               //9   51  = 5,1V
+  unsigned int  Battery2;               //11  51  = 5,1V
+  unsigned char Temperature1;   //13  44 = 24°C, 0 = -20°C
+  unsigned char Temperature2;   //14  44 = 24°C, 0 = -20°C
+  unsigned char FuelPercent;    //15
+    signed int  FuelCapacity;   //16
+  unsigned int  Rpm;
+  unsigned int  Altitude;
+  unsigned int  m_sec;              // 3000 = 0
+  unsigned char m_3sec;             // 120 = 0
+  unsigned int  Current;                // 1 = 0.1A
+  unsigned int  InputVoltage;   // 66  = 6,6V
+  unsigned int  Capacity;               // 1  = 10mAh
+  unsigned char NullByte1;      // 0x00
+  unsigned char NullByte2;      // 0x00
+  unsigned char EndByte;                // 0x7D
+} HoTTGeneral_t;
+*/
+                        case SPI_NCCMD_HOTT_INFO:
+                                switch(hott_index++)
+                                {
                                 case 0:
                                         //Dezimalgrad            --> Grad mit Dezimalminuten     --> Grad, Minuten, Sekunden
                                         //53.285788 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; // index
                                         i2 /= 10;
                                         ToFlightCtrl.Param.Byte[6]  = i1 % 256;
                                         ToFlightCtrl.Param.Byte[7]  = i1 / 256;
                                         ToFlightCtrl.Param.Byte[8]  = i2 % 256;
                                         ToFlightCtrl.Param.Byte[9]  = i2 / 256;
                                         break;
                                 case 1:
                                         ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
                                         ToFlightCtrl.Param.Byte[0] = 11;        // index
                                         ToFlightCtrl.Param.Byte[1] = 8-1;       // how many
                                         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; // index
+                                        ToFlightCtrl.Param.Byte[1] = 2; // how many
+                                        ToFlightCtrl.Param.Byte[2] = EarthMagneticField / (5 * 2);
+                                        ToFlightCtrl.Param.Byte[3] = EarthMagneticInclination / 2;
                                         hott_index = 0;
                                         break;
-                                   default:
+                                 default:
-                                                ToFlightCtrl.Param.Byte[0] = 255;
+                                        ToFlightCtrl.Param.Byte[0] = 255;
-                                                hott_index = 0;
+                                        hott_index = 0;
-                                                break;
+                                        break;
+                                }
                                 break;
                         default:
                                 break;
 // 0 = 0,1
                                 FC.StatusFlags |= FromFlightCtrl.Param.Byte[8];
                                 if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive)
+                                {
                                         HeadFreeStartAngle = Compass_Heading * 10;
                                         Compass_Init();
-                                        FCCalibActive = 1;
+                                        FCCalibActive = 10;
+                                }
                                 else
+                                {
-                                        FCCalibActive = 0;
+                                        if(FCCalibActive) if(--FCCalibActive == 0) FC_is_Calibrated = 1;
+                                }
                                 if(FC.StatusFlags & FC_STATUS_START)
+                             {
                                    if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else
                                    HeadFreeStartAngle = FromFlightCtrl.GyroHeading;

Subversion Repositories NaviCtrl

(root)/tags/V0.28m/spi_slave.c @ 402 - Rev 329 → 330