WebSVN - NaviCtrl - Diff - Rev 487 and 489

 // + 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
 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)
+{
                                         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)
+                                {
+                                                }
-                                         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;
                         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];
                                 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.04a/spi_slave.c @ 868 - Rev 487 → 489