| 0,0 → 1,1087 |
|---|
| /*#######################################################################################*/ |
| /* !!! THIS IS NOT FREE SOFTWARE !!! */ |
| /*#######################################################################################*/ |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 |
| // + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt. |
| // + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software. |
| // + #### ENDE DER NUTZUNGSBEDINGUNGEN ####' |
| // + 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. |
| // + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other |
| // + features that can be used to identify the program may not be altered or defaced by the customer. |
| // + The customer shall be responsible for taking reasonable precautions |
| // + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the |
| // + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and |
| // + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product |
| // + liability. However, the Licensor shall be entitled to the defense of contributory negligence. |
| // + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test |
| // + the software for his purpose before any operational usage. The customer will backup his data before using the software. |
| // + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data |
| // + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations. |
| // + *) The territory aspect only refers to the place where the Software is used, not its programmed range. |
| // + #### END OF LICENSING TERMS #### |
| // + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //#define MCLK96MHZ |
| const unsigned long _Main_Crystal = 25000; |
| //#include <stdio.h> |
| #include <string.h> |
| #include "91x_lib.h" |
| #include "led.h" |
| #include "uart0.h" |
| #include "uart1.h" |
| #include "uart2.h" |
| #include "gps.h" |
| #include "i2c.h" |
| #include "compass.h" |
| #include "ncmag.h" |
| #include "timer1.h" |
| #include "timer2.h" |
| #include "analog.h" |
| #include "spi_slave.h" |
| #include "fat16.h" |
| #include "sdc.h" |
| #include "logging.h" |
| #include "params.h" |
| #include "settings.h" |
| #include "config.h" |
| #include "main.h" |
| #include "debug.h" |
| #include "eeprom.h" |
| #include "ssc.h" |
| #include "sdc.h" |
| #include "uart1.h" |
| #include "canbus.h" |
| #include "triggerlog.h" |
| #include "CamCtrl.h" |
| |
| |
| #ifdef FOLLOW_ME |
| u8 TransmitAlsoToFC = 0; |
| #endif |
| u32 TimerCheckError; |
| u32 TimerSecond; |
| u8 ErrorCode = 0; |
| u16 BeepTime; |
| u8 NCFlags = 0; |
| s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg |
| u8 ErrorGpsFixLost = 0; |
| u8 FromFC_LoadWP_List = 0, FromFC_Load_SinglePoint = 0, FromFC_Save_SinglePoint = 0; |
| u8 ToFC_MaxWpListIndex = 4; |
| u8 ClearFCStatusFlags = 0; |
| u8 StopNavigation = 0; |
| volatile u32 PollingTimeout = 10000; |
| Param_t Parameter; |
| Partner_t Partner; |
| volatile FC_t FC; |
| volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero |
| volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero |
| u32 CountGpsProcessedIn5Sec = 50,CountNewGpsDataIn5Sec = 25, FreqGpsNavProcessed = 0, FreqNewGpsData = 0; |
| u8 NewWPL_Name = 0; |
| u32 MaxWP_Radius_in_m = 0; |
| s8 ErrorMSG[25]; |
| s8 PartnerErrorMSG[25] = " --- \0"; |
| u8 OEM_String[OEM_NAME_LENGHT+1] = {0xff,' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',0x00}; |
| u32 TimeSinceMotorStart = 0; |
| u8 IamMaster = NOTHING; // for Master/Slave Redundance |
| u16 ToFC_Parachute_Off; |
| u8 IO1_Function = 0; |
| s16 LuftdruckTemperaturKompensation = 0, LuftdruckTemperaturOffset = 0; |
| u8 BaroCalState = 0; |
| u8 ErrorOutSideOperationArea = 0; // I am outside the operation polygon area |
| u32 ShowNoFlyzoneErrorMessage = 0; |
| u8 ShowCalibrationErrorMessage = 0; |
| u8 TryAgain_UBX_Setup = 0; |
| u8 MenuBlinkBit; |
| //---------------------------------------------------------------------------------------------------- |
| void SCU_Config(void) |
| { |
| /* configure PLL and set it as master clock source */ |
| SCU_MCLKSourceConfig(SCU_MCLK_OSC); // set master clock source to external oscillator clock (25MHz) before diabling the PLL |
| SCU_PLLCmd(DISABLE); // now disable the PLL |
| #ifdef MCLK96MHZ |
| SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz |
| SCU_PCLKDivisorConfig(SCU_PCLK_Div4); // set PCLK (APB bus clock) divisor to 4 (half Reference Clock) |
| SCU_RCLKDivisorConfig(SCU_RCLK_Div2); // set RCLK (Reference Clock) divisor to 1 (full PPL clock) |
| SCU_HCLKDivisorConfig(SCU_HCLK_Div2); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock) |
| SCU_PLLFactorsConfig(192,25,2); // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2 |
| #else |
| SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz |
| SCU_PCLKDivisorConfig(SCU_PCLK_Div2); // set PCLK (APB bus clock) divisor to 2 (half Reference Clock) |
| SCU_RCLKDivisorConfig(SCU_RCLK_Div1); // set RCLK (Reference Clock) divisor to 1 (full PPL clock) |
| SCU_HCLKDivisorConfig(SCU_HCLK_Div1); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock) |
| SCU_PLLFactorsConfig(192,25,3); // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3 |
| #endif |
| SCU_PLLCmd(ENABLE); // Enable PLL (is disabled by SCU_PLLFactorsConfig) |
| SCU_MCLKSourceConfig(SCU_MCLK_PLL); // set master clock source to PLL |
| } |
| |
| //---------------------------------------------------------------------------------------------------- |
| void GetNaviCtrlVersion(void) |
| { |
| u8 msg[25]; |
| |
| sprintf(msg,"\r\n NaviCtrl HW: V%d.%d SW: V%d.%02d%c", UART_VersionInfo.HWMajor/10, UART_VersionInfo.HWMajor%10, VERSION_MAJOR, VERSION_MINOR, 'a'+ VERSION_PATCH); |
| UART1_PutString(msg); |
| } |
| |
| //---------------------------------------------------------------------------------------------------- |
| |
| void CheckErrors(void) |
| { |
| static s32 no_error_delay = 0; |
| s32 newErrorCode = 0; |
| UART_VersionInfo.HardwareError[0] = 0; |
| |
| if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout) || (Compass_Heading < 0)) DebugOut.StatusRed |= AMPEL_COMPASS; |
| else DebugOut.StatusRed &= ~AMPEL_COMPASS; // MK3Mag green status |
| |
| if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.StatusRed |= AMPEL_BL; |
| else DebugOut.StatusRed &= ~AMPEL_BL; // BL-Ctrl green status |
| |
| if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.StatusRed |= AMPEL_NC; |
| else DebugOut.StatusRed &= ~AMPEL_NC; |
| |
| if(CheckDelay(I2CBus(Compass_I2CPort)->Timeout)) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"no compass communica"); |
| //Reset Compass communication |
| if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0); else I2CBus_Init(I2C1); |
| Compass_Init(); |
| newErrorCode = 4; |
| StopNavigation = 1; |
| UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX; |
| DebugOut.StatusRed |= AMPEL_COMPASS; |
| } |
| else if(CompassValueErrorCount > 30) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"compass sensor error"); |
| newErrorCode = 34; |
| StopNavigation = 1; |
| UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
| if(Compass_I2CPort == NCMAG_PORT_EXTERN) I2CBus_Init(I2C0); else I2CBus_Init(I2C1); |
| Compass_Init(); |
| } |
| else if((FCCalibActive || CompassCalState) && FC_Version.Hardware) |
| { |
| sprintf(ErrorMSG,"Calibrate... "); |
| newErrorCode = 0; |
| ErrorCode = 0; |
| no_error_delay = 1; |
| } |
| else if(CheckDelay(SPI0_Timeout)) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"no FC communication "); |
| newErrorCode = 3; |
| StopNavigation = 1; |
| DebugOut.StatusGreen &= ~AMPEL_FC; // status of FC Present |
| DebugOut.StatusGreen &= ~AMPEL_BL; // status of BL Present |
| UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX; |
| } |
| else if(FC_Version.Compatible != FC_SPI_COMPATIBLE) |
| { |
| LED_RED_ON; |
| #ifndef FOLLOW_ME |
| sprintf(ErrorMSG,"FC not compatible "); |
| #else |
| sprintf(ErrorMSG,"! FollowMe only ! "); |
| #endif |
| newErrorCode = 1; |
| StopNavigation = 1; |
| UART_VersionInfo.HardwareError[0] |= NC_ERROR0_FC_INCOMPATIBLE; |
| DebugOut.StatusRed |= AMPEL_NC; |
| } |
| else if(ToFC_Parachute_Off) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR: PARACHUTE"); |
| newErrorCode = 43; |
| } |
| else if(FC.Error[1] & FC_ERROR1_SPI_RX) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"FC spi rx error "); |
| newErrorCode = 8; |
| StopNavigation = 1; |
| } |
| else if(CntSpiErrorPerSecond > 2) |
| { |
| LED_RED_ON; |
| newErrorCode = 8; |
| sprintf(ErrorMSG,"FC spi CRC error"); |
| UART_VersionInfo.HardwareError[0] |= NC_ERROR0_SPI_RX; |
| } |
| else if(FC.Error[0] & FC_ERROR0_GYRO_NICK) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR: FC Nick Gyro"); |
| newErrorCode = 10; |
| } |
| else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR: FC Roll Gyro"); |
| newErrorCode = 11; |
| } |
| else if(FC.Error[0] & FC_ERROR0_GYRO_YAW) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR: FC Yaw Gyro"); |
| newErrorCode = 12; |
| } |
| else if(FC.Error[0] & FC_ERROR0_ACC_NICK) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR: FC Nick ACC"); |
| newErrorCode = 13; |
| } |
| else if(FC.Error[0] & FC_ERROR0_ACC_ROLL) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR: FC Roll ACC"); |
| newErrorCode = 14; |
| } |
| else if(FC.Error[0] & FC_ERROR0_ACC_TOP) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:FC Z-ACC"); |
| newErrorCode = 15; |
| } |
| else if(FC.Error[0] & FC_ERROR0_PRESSURE) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:Pressure sensor"); |
| newErrorCode = 16; |
| } |
| else if(FC.Error[1] & FC_ERROR1_I2C) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:I2C FC to BL"); |
| newErrorCode = 17; |
| } |
| else if(FC.Error[1] & FC_ERROR1_BL_MISSING) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR: Bl Missing"); |
| newErrorCode = 18; |
| } |
| else if(FC.Error[1] & FC_ERROR1_MIXER) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"Mixer Error"); |
| newErrorCode = 19; |
| } |
| else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"no GPS communication"); |
| UART_VersionInfo.HardwareError[0] |= NC_ERROR0_GPS_RX; |
| UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_GPS_PRESENT; |
| newErrorCode = 5; |
| StopNavigation = 1; |
| } |
| else if(Compass_Heading < 0 && NCMAG_Present && !NCMAG_IsCalibrated) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"compass not calibr."); |
| newErrorCode = 31; |
| StopNavigation = 1; |
| UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
| } |
| else if(Compass_Heading < 0) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"bad compass value "); |
| newErrorCode = 6; |
| StopNavigation = 1; |
| UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
| } |
| else if(FC.Error[0] & FC_ERROR0_CAREFREE) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"FC: Carefree Error"); |
| newErrorCode = 20; |
| } |
| else if(FC.BAT_Voltage < 45) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:Power Supply"); |
| newErrorCode = 41; |
| } |
| else |
| if(FC.Error[1] & FC_ERROR1_RC_VOLTAGE) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR: 5V RC-Supply"); |
| newErrorCode = 40; |
| } |
| else if(FC.Error[1] & FC_ERROR1_PPM) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"RC Signal lost "); |
| newErrorCode = 7; |
| } |
| else if(ErrorGpsFixLost) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"GPS Fix lost "); |
| newErrorCode = 21; |
| } |
| else if(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:Flying range!"); |
| newErrorCode = 28; |
| } |
| else if(ErrorDisturbedEarthMagnetField) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"Magnet error "); |
| newErrorCode = 22; |
| DebugOut.StatusRed |= AMPEL_COMPASS | AMPEL_NC; |
| UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_VALUE; |
| } |
| else if(((ErrorCheck_BL_MinOfMaxPWM == 40 && (TimeSinceMotorStart > 3)) || (ErrorCheck_BL_MinOfMaxPWM == 39)) && !ErrorCode) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:Motor restart "); |
| newErrorCode = 23; |
| DebugOut.StatusRed |= AMPEL_BL; |
| } |
| else if(BL_MinOfMaxPWM && BL_MinOfMaxPWM < 30 && !ErrorCode) |
| { |
| u16 i; |
| for(i = 0; i < 16; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break; // find the motor number |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM); |
| newErrorCode = 32; |
| DebugOut.StatusRed |= AMPEL_BL; |
| } |
| else if(ErrorCheck_BL_MinOfMaxPWM < 248 && (TimeSinceMotorStart > 3) && !ErrorCode) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:BL Limitation "); |
| newErrorCode = 24; |
| DebugOut.StatusRed |= AMPEL_BL; |
| } |
| else if((NCFlags & NC_FLAG_RANGE_LIMIT) && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:GPS WP range "); |
| newErrorCode = 25; |
| DebugOut.StatusRed |= AMPEL_NC; |
| } |
| else if((SDCardInfo.Valid == 0) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START && !(FC.StatusFlags & FC_STATUS_FLY)) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:No SD-Card "); |
| newErrorCode = 26; |
| DebugOut.StatusRed |= AMPEL_NC; |
| } |
| else if((SD_LoggingError || (SD_WatchDog < 2000 && SD_WatchDog != 0)) && Parameter.GlobalConfig3 & CFG3_NO_SDCARD_NO_START) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:SD Logging abort"); |
| newErrorCode = 27; |
| DebugOut.StatusRed |= AMPEL_NC; |
| SD_LoggingError = 0; |
| } |
| else if(((AbsoluteFlyingAltitude) && (NaviData.Altimeter_5cm / 20 >= AbsoluteFlyingAltitude)) && (FC.StatusFlags & FC_STATUS_FLY)) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:Max Altitude "); |
| newErrorCode = 29; |
| DebugOut.StatusRed |= AMPEL_NC; |
| } |
| else if(Parameter.GlobalConfig3 & CFG3_NO_GPSFIX_NO_START && !(NCFlags & NC_FLAG_GPS_OK) && ((FC.StatusFlags & (FC_STATUS_START | FC_STATUS_MOTOR_RUN)) || (FC.StickGas < -50 && FC.StickYaw < -50))) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"No GPS Fix "); |
| newErrorCode = 30; |
| } |
| else if((FreqNewGpsData <= 35 || FreqNewGpsData > 60) && TimeSinceMotorStart > 15) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"GPS Update rate "); |
| newErrorCode = 38; |
| } |
| else if(NC_GPS_ModeCharacter == 'F') |
| { |
| sprintf(ErrorMSG,"FAILSAFE pos.! "); |
| newErrorCode = 35; |
| } |
| else if(ErrorOutSideOperationArea && FC_is_Calibrated) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR:Outside Flyzone "); |
| newErrorCode = 44; |
| DebugOut.StatusRed |= AMPEL_NC; |
| } |
| else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_ERROR) |
| { |
| sprintf(ErrorMSG,"ERR:Redundancy "); |
| newErrorCode = 36; |
| } |
| else if(FC.StatusFlags3 & FC_STATUS3_REDUNDANCE_TEST) |
| { |
| sprintf(ErrorMSG,"Redundancy test "); |
| newErrorCode = 37; |
| } |
| else if(CanbusTimeOut == 1) |
| { |
| sprintf(ErrorMSG,"ERR: Canbus"); |
| CanbusInit(); |
| newErrorCode = 39; |
| } |
| else |
| if(FC.Error[1] & FC_ERROR1_ACC_NOT_CAL) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ACC not calib."); |
| newErrorCode = 42; |
| } |
| else |
| if(ShowNoFlyzoneErrorMessage > 5) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR: no Flyzone "); |
| newErrorCode = 45; |
| } |
| else |
| if(ShowCalibrationErrorMessage) |
| { |
| LED_RED_ON; |
| sprintf(ErrorMSG,"ERR: Calibration "); |
| newErrorCode = 46; |
| } |
| else // no error occured |
| { |
| StopNavigation = 0; |
| LED_RED_OFF; |
| if(no_error_delay) { no_error_delay--; } |
| else |
| { |
| sprintf(ErrorMSG,"No Error "); |
| ErrorCode = 0; |
| } |
| } |
| |
| if(newErrorCode) |
| { |
| if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running |
| ErrorCode = newErrorCode; |
| } |
| FC.Error[0] = 0; |
| FC.Error[1] = 0; |
| FC.Error[2] = 0; |
| FC.Error[3] = 0; |
| FC.Error[4] = 0; |
| ErrorGpsFixLost = 0; |
| ErrorCheck_BL_MinOfMaxPWM = BL_MinOfMaxPWM; |
| } |
| |
| unsigned char RAM_Checksum(unsigned char* pBuffer, u16 len) |
| { |
| u8 crc = 0xAA; |
| u16 i; |
| for(i=0; i<len; i++) crc += pBuffer[i]; |
| return crc; |
| } |
| |
| |
| u8 Polling(void) |
| { |
| static u8 running = 0, oldFcFlags = 0, count5sec, TimeoutGPS_Process, t20ms; |
| static u32 old_ms = 0; |
| |
| if(running) {/*DebugOut.Analog[]++;*/ return(1);}; |
| running = 1; |
| |
| if(CountMilliseconds != old_ms) // 1 ms |
| { |
| if(!t20ms--) { CalcNickServoValue(); t20ms = 20; }; |
| |
| // Trigger internal Portpin or Flag -> will set the Hardware Triggeroutput on the GimbalCtrl |
| if(((UART_VersionInfo.HWMajor >= 30) && TRIGGER_PP_INTERN) || ((UART_VersionInfo.HWMajor < 30) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE))) ToGimbalCtrl.BitCmd |= GIMBAL_CMD_OUT_TRIGGER; |
| |
| if(UART1_BaudrateFallbackTimeout) |
| { |
| if(--UART1_BaudrateFallbackTimeout == 0) UART1_Configure(Uart1Baudrate); |
| } |
| |
| if(CanbusTimeOut >= 2) CanbusTimeOut--; |
| if(I2C0_Timeout) |
| { |
| I2C0_Timeout--; |
| if((I2C0_Timeout % 5000) == 0) I2CBus_StateReset(I2C0); // reset the I2C-Bus every 5 seconds |
| } |
| if(LaserCtrlTimeout) |
| { |
| LaserCtrlTimeout--; |
| } |
| else |
| { |
| FromLaserCtrl.Distance = 0; |
| FromLaserCtrl.LaserStatus = 0; |
| }; |
| |
| if(GimbalCtrlTimeout) |
| { |
| GimbalCtrlTimeout--; |
| } |
| else |
| { |
| FromGimbalCtrl.Nick = -1; |
| FromGimbalCtrl.Roll = -1; |
| FromGimbalCtrl.Yaw = -1; |
| FromGimbalCtrl.GimbalStatus = 0; |
| } |
| |
| if(CamCtrlTimeout) |
| { |
| if(--CamCtrlTimeout == 1) CamCtrlCharacter = '?'; |
| else if(CamCtrlTimeout == 20000) CamCtrlTimeout = 0; // this is used during NC startup-Time phase |
| } |
| else |
| { |
| if(TrigLogging.CountExternal) CamCtrlCharacter = TrigLogging.CountExternal % 10 + '0'; |
| FromCamCtrl.CamStatus = 0; |
| }; |
| old_ms = CountMilliseconds; |
| Compass_Update(); // update compass communication |
| Analog_Update(); // get new ADC values |
| CalcHeadFree(); |
| if(CamCtrlTimeout > 1) CamCtrl_GetData(3); |
| if(LaserCtrlTimeout > 1) LaserCtrl_GetData(3); |
| if(GimbalCtrlTimeout > 1) GimbalCtrl_GetData(3); |
| if(UART_VersionInfo.HWMajor >= 30) ProcessCanBus(); |
| if(!CheckDelay(SPI0_Timeout)) TimeoutGPS_Process = 0; |
| else if(CountMilliseconds - SPI0_Timeout > 30000000L) SPI0_Timeout = CountMilliseconds; // avoid too long overflows |
| if(++TimeoutGPS_Process >= 25) |
| { |
| GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected |
| TimeoutGPS_Process = 0; |
| } |
| if(UART_VersionInfo.HWMajor >= 30) |
| { |
| switch(IO1_Function) |
| { |
| case IO1FUNC_PARACHUTE: // parachute |
| if(IO1_INPUT == 0) |
| { |
| if(ToFC_Parachute_Off < 2000) ToFC_Parachute_Off++; |
| } |
| else ToFC_Parachute_Off = 0; |
| break; |
| |
| default: ToFC_Parachute_Off = 0; |
| break; // no function |
| } |
| } |
| } |
| |
| SPI0_UpdateBuffer(); // also calls the GPS-functions |
| UART0_ProcessRxData(); // GPS process request |
| UART0_TransmitTxData(); // GPS send answer |
| UART1_ProcessRxData(); // PC process request |
| UART1_TransmitTxData(); // PC send answer |
| UART2_TransmitTxData(); // FC send answer |
| |
| if(!(FC.StatusFlags & FC_STATUS_MOTOR_RUN)) TimeSinceMotorStart = 0; |
| |
| if(HugeBlockFromFC.WhoAmI == 1) // contains eeprom-Data |
| { |
| u8 crc1, crc2; |
| // static u8 first_time = 1; |
| crc1 = HugeBlockFromFC.Data[sizeof(EE_Parameter)-1]; // contains the EE_Parameter.crc |
| crc2 = RAM_Checksum((u8 *)(&HugeBlockFromFC.Data), sizeof(EE_Parameter)-1); // calculates the current crc |
| if(crc1 == crc2) |
| { |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + get EE_Parameters from FC |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| memcpy(&EE_Parameter, (u8 *) HugeBlockFromFC.Data, sizeof(EE_Parameter)); |
| RequestConfigFromFC = 0; |
| if(UART_VersionInfo.HWMajor >= 30) SendOemName = 1; |
| // if(first_time) |
| { |
| WP_MissionParameter.ComingHomeAltitude = EE_Parameter.ComingHomeAltitude; |
| WP_MissionParameter.FailsafeAltitude = EE_Parameter.FailSafeAltitude; |
| WP_MissionParameter.TriggerMask = EE_Parameter.J16Bitmask; |
| } |
| } |
| HugeBlockFromFC.WhoAmI = 0; |
| } |
| |
| if(FCCalibActive) |
| { |
| count5sec = 0; |
| FreqNewGpsData = 50; |
| CountNewGpsDataIn5Sec = 25; |
| CountGpsProcessedIn5Sec = 0; |
| TimerCheckError = SetDelay(1000); |
| NickServoValue = 128 * 64; // Middle position |
| } |
| |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Baro Temperature offset |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(!BaroCalState && LuftdruckTemperaturKompensation) |
| { |
| LuftdruckTemperaturOffset = (31 * (s32)LuftdruckTemperaturOffset + ((FC_Temperatur_raw - FC_StartTemperature*10) * LuftdruckTemperaturKompensation) / 100) / 32; |
| } |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + one second timer |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(CheckDelay(TimerSecond)) // one second Timer |
| { |
| TimerSecond += 1000; |
| if(MenuBlinkBit) MenuBlinkBit = 0; else MenuBlinkBit = 1; |
| if(FC.StatusFlags & FC_STATUS_FLY) |
| { |
| NaviData.FlyingTime++; // we want to count the battery-time |
| TimeSinceMotorStart++; |
| } |
| |
| if((Parameter.ExtraConfig & CFG_FLYZONE_REQUIRED) && FlyzonePointCnt < 3 && FC_is_Calibrated) ShowNoFlyzoneErrorMessage++; else ShowNoFlyzoneErrorMessage = 0; |
| |
| if(ShowCalibrationErrorMessage) ShowCalibrationErrorMessage--; |
| |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + FC 3.0 Temperature |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(UART_VersionInfo.HWMajor >= 30) |
| { |
| if(FC_Temperatur == -1000 && FC_Temperatur_raw != -10000) // init |
| { |
| FC_Temperatur = FC_Temperatur_raw/10; |
| FC_StartTemperature = FC_Temperatur; |
| } |
| else |
| if(FC_Temperatur > FC_Temperatur_raw/10) FC_Temperatur--; |
| else |
| if(FC_Temperatur < FC_Temperatur_raw/10) FC_Temperatur++; |
| } |
| |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Re-Init GPS-Module? |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| if((Parameter.GlobalConfig & FC_CFG_GPS_AKTIV) && (UART0_Muxer != UART0_MK3MAG)) |
| { |
| if(TryAgain_UBX_Setup) |
| { |
| if(--TryAgain_UBX_Setup == 0) UBX_Setup(); |
| } |
| else |
| if(CheckDelay(UBX_Timeout)) // no GPS communication |
| { |
| GPS_Version = 0; |
| NCFlags &= ~NC_FLAG_GPS_OK; |
| TryAgain_UBX_Setup = 6; // Re-init GPS-Receiver in x seconds |
| } |
| } |
| } |
| // ++++++++++++++++++++++++++++++++++++++++++++++++ |
| // ---------------- Error Check Timing ---------------------------- |
| if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start |
| { |
| if(CheckDelay(TimerCheckError)) |
| { |
| if(!(FC.StatusFlags & FC_STATUS_FLY)) // do not change the Bit during flight |
| { |
| if(!BLITZ_CONNECTED) BlitzSchuhConnected = 1; |
| else BlitzSchuhConnected = 0; |
| } |
| TimerCheckError = SetDelay(1000); |
| if(CompassValueErrorCount) CompassValueErrorCount--; |
| if(++count5sec == 5) |
| { |
| FreqGpsNavProcessed = CountGpsProcessedIn5Sec * 2; //400 = 40Hz |
| FreqNewGpsData = CountNewGpsDataIn5Sec; // 50 = 5,0Hz |
| //DebugOut.Analog[] = FreqNewGpsData; |
| count5sec = 0; |
| CountGpsProcessedIn5Sec = 0; |
| CountNewGpsDataIn5Sec = FreqNewGpsData / 2; |
| ConfirmGpsUpdateRate(FreqNewGpsData); |
| } |
| } |
| oldFcFlags = FC.StatusFlags; |
| // if(CheckDelay(SPI0_Timeout) && (DebugUART == UART1)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected |
| |
| if(!CheckDelay(SPI0_Timeout) || (DebugUART == UART1)) CheckErrors(); |
| |
| if(EE_Parameter.Revision == 0) RequestConfigFromFC = 1; |
| if(SerialLinkOkay) SerialLinkOkay--; |
| if(SerialLinkOkay < 250 - 6) NCFlags |= NC_FLAG_NOSERIALLINK; // 6 seconds timeout for serial communication |
| else NCFlags &= ~NC_FLAG_NOSERIALLINK; |
| if(StopNavigation && (Parameter.NaviGpsModeControl >= 50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000; |
| CntSpiErrorPerSecond = 0; |
| } |
| running = 0; |
| // if(!PollingTimeout) DebugOut.Analog[17]++; |
| return(0); |
| } |
| |
| // the handler will be cyclic called by the timer 1 ISR |
| // used is for critical timing parts that normaly would handled |
| // within the main loop that could block longer at logging activities |
| void EXTIT3_IRQHandler(void) // 1ms - Takt |
| { |
| IENABLE; |
| VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq |
| |
| if(PollingTimeout == 0) |
| { |
| PollingTimeout = 5; |
| //if(Polling() == 0) DebugOut.Analog[]++; |
| Polling(); |
| } |
| |
| VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit |
| VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq |
| IDISABLE; |
| VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register |
| } |
| |
| //---------------------------------------------------------------------------------------------------- |
| int main(void) |
| { |
| |
| // static u32 ftimer =0; |
| // static u8 fstate = 0; |
| // static File_t* f = NULL; |
| |
| |
| /* Configure the system clocks */ |
| SCU_Config(); |
| /* init VIC (Vectored Interrupt Controller) */ |
| SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC |
| SCU_AHBPeriphReset(__VIC, DISABLE); // disable reset state for VIC |
| VIC_DeInit(); // deinitializes the VIC module registers to their default reset values. |
| VIC_InitDefaultVectors(); |
| |
| // initialize timer 1 for System Clock and delay rountines |
| TIMER1_Init(); |
| // initialize the LEDs (needs Timer 1) |
| Led_Init(); |
| // initialize the debug UART1 |
| Uart1Baudrate = 57600; |
| UART1_Init(); |
| I2CBus_Init(I2C1); |
| ReadBaudrateIndexfromEEprom(); |
| if(Uart1Baudrate != 57600) |
| { |
| { |
| u8 msg[30]; |
| sprintf(msg, "\r\n<%uBd>\r\n\r\n",Uart1Baudrate); |
| UART1_PutString(msg); |
| } |
| DELAY(100); |
| UART1_Init(); |
| } |
| UART1_PutString("\r---------------------------------------------"); |
| // initialize timer 2 for servo outputs |
| //TIMER2_Init(); |
| // initialize UART2 to FLIGHTCTRL |
| UART2_Init(); |
| // initialize UART0 (to MKGPS or MK3MAG) |
| UART0_Init(); |
| // initialize adc |
| Analog_Init(); |
| // initialize SPI0 to FC |
| SPI0_Init(); |
| // initialize i2c busses (needs Timer 1) |
| InitCamCtrl(); |
| // initialize fat16 partition on sd card (needs Timer 1) |
| I2CBus_Init(I2C0); |
| I2CBus_Init(I2C1); |
| Fat16_Init(); |
| // initialize NC params |
| NCParams_Init(); |
| // initialize the settings |
| Settings_Init(); |
| // initialize logging (needs settings) |
| Logging_Init(); |
| |
| //UART_VersionInfo.HWMajor = 30; |
| LED_GRN_ON; |
| TimerCheckError = SetDelay(3000); |
| TimerSecond = SetDelay(3500); |
| |
| GetBaudrateFromSdCard(Uart1Baudrate); |
| |
| UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++"); |
| UART1_PutString("\r\n Version information:"); |
| |
| GetNaviCtrlVersion(); |
| DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag |
| DebugOut.StatusRed = 0x00; |
| UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++"); |
| Compass_Init(); |
| #ifdef FOLLOW_ME |
| TransmitAlsoToFC = 1; |
| UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++"); |
| UART1_PutString("\r\n FOLLOW-ME Transmitter only!"); |
| UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++\r\n\r\n"); |
| TransmitAlsoToFC = 0; |
| #else |
| // if(UBX_Setup() == 0) UBX_Setup(); // inits the GPS-Module via ubx -> try twice |
| SPI0_GetFlightCtrlVersion(); |
| if(IamMaster == SLAVE) UART1_PutString(" SLAVE\r\n"); |
| if(IamMaster == MASTER) UART1_PutString(" MASTER\r\n"); |
| if(FC_Version.Compatible != FC_SPI_COMPATIBLE) |
| { |
| UART1_PutString("\r\n Flight-Ctrl not compatible\r\n"); |
| LED_RED_ON; |
| } |
| #endif |
| if(UBX_Setup() == 0) // Achtung: muss nach FC-Versions check |
| { |
| if(UBX_Setup() == 0) // inits the GPS-Module via ubx -> try twice |
| { |
| if(IamMaster == SLAVE) TryAgain_UBX_Setup = 2; |
| else TryAgain_UBX_Setup = 4; |
| } |
| } |
| |
| // +++++++++++++++++++++++++++++++++++++++ |
| // ++ check xxCtrl version (if connected) |
| // +++++++++++++++++++++++++++++++++++++++ |
| if(Compass_I2CPort == NCMAG_PORT_INTERN) |
| { |
| if(I2CBus_LockBuffer(I2C0,3)) I2CBus_Transmission(I2C0, CAM_SLAVE_ADDRESS, &ToCamCtrl, 4, &CamCtrl_UpdateData, sizeof(FromCamCtrl)); |
| if(I2CBus_LockBuffer(I2C0,6)) I2CBus_Transmission(I2C0, GIMBAL_SLAVE_ADDRESS, &ToGimbalCtrl, 4, &GimbalCtrl_UpdateData, sizeof(FromGimbalCtrl)); |
| if(I2CBus_LockBuffer(I2C0,6)) I2CBus_Transmission(I2C0, LASER_SLAVE_ADDRESS, &ToLaserCtrl, 3, &LaserCtrl_UpdateData, sizeof(FromLaserCtrl)); |
| } |
| else |
| { |
| CamCtrlTimeout = 0; // disable CamCtrl communication if external compass is connected |
| GimbalCtrlTimeout = 0; // disable CamCtrl communication if external compass is connected |
| LaserCtrlTimeout = 0; // disable CamCtrl communication if external compass is connected |
| } |
| // +++++++++++++++++++++++++++++++++++++++ |
| |
| GPS_Init(); |
| |
| // ---------- Prepare the isr driven |
| // set to absolute lowest priority |
| VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW); |
| // enable interrupts |
| VIC_ITCmd(EXTIT3_ITLine, ENABLE); |
| |
| Debug_OK("START"); |
| UART1_PutString("\r\n"); |
| fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer |
| LED_GRN_ON; |
| LED_RED_OFF; |
| Settings_GetParamValue(PID_SEND_NMEA,(u16 *) &NMEA_Interval); |
| UART1_PutString("\r\n"); |
| |
| Settings_GetParamValue(PID_YAW_WITHOUT_CF,(u16 *) &WP_Yaw_WithoutCF); |
| // UART1_PutString(" Yaw without Carfefree in waypoint flight:"); |
| // if(WP_Yaw_WithoutCF) UART1_PutString("on\r\n"); else UART1_PutString("off\r\n"); |
| |
| CompassValueErrorCount = 0; |
| I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000); |
| |
| // Intilizes the Canbus |
| if(UART_VersionInfo.HWMajor >= 30) |
| { |
| EXT2_Init(); // External Output EXT2 |
| CanbusInit(); |
| ReadBaroCalibrationfromEEprom(); |
| } |
| if(FromCamCtrl.CamStatus) |
| { |
| u8 msg[30]; |
| sprintf(msg, " CamCtrl found V%i.%02i \r\n",1 + FromCamCtrl.Version / 100, FromCamCtrl.Version % 100); |
| UART1_PutString(msg); |
| } |
| else if(Compass_I2CPort == NCMAG_PORT_INTERN) UART1_PutString(" No CamCtrl connected \r\n"); |
| |
| if(FromLaserCtrl.LaserStatus) |
| { |
| u8 msg[30]; |
| sprintf(msg, " LaserCtrl found V%i.%02i ",1 + FromLaserCtrl.Version / 100, FromLaserCtrl.Version % 100); |
| UART1_PutString(msg); |
| if(!(FromLaserCtrl.LaserStatus & LASER_DATA_OK)) UART1_PutString("... but no Laser data!\r\n"); |
| else UART1_PutString("okay\r\n"); |
| } |
| else if(Compass_I2CPort == NCMAG_PORT_INTERN) UART1_PutString(" No LaserCtrl connected \r\n"); |
| |
| if(FromGimbalCtrl.GimbalStatus) |
| { |
| u8 msg[30]; |
| sprintf(msg, " GimbalCtrl found V%i.%02i ",1 + FromGimbalCtrl.Version / 100, FromGimbalCtrl.Version % 100); |
| UART1_PutString(msg); |
| if(!(FromGimbalCtrl.GimbalStatus & GIMBAL_DATA_OK)) UART1_PutString("... but no Gimbal data!\r\n"); |
| else UART1_PutString("okay\r\n"); |
| } |
| else if(Compass_I2CPort == NCMAG_PORT_INTERN) UART1_PutString(" No GimbalCtrl connected \r\n"); |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| UART1_PutString(" Initialization finished ... Start\r\n"); |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| for (;;) // the endless main loop |
| { |
| PollingTimeout = 5; |
| Polling(); |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| if(FromFC_LoadWP_List) |
| { |
| WPL_Store.Index = (FromFC_LoadWP_List & ~0x80); |
| if(WPL_Store.Index <= ToFC_MaxWpListIndex) |
| { |
| if(PointList_ReadFromFile(&WPL_Store) == WPL_OK) |
| { |
| if(FromFC_LoadWP_List & 0x80)// -> load relative |
| { |
| u32 angle; |
| angle = (360 + GyroCompassCorrected/10 + Parameter.OrientationAngle * 15) % 360; |
| if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance_dm > 7*10) |
| { // take actual position |
| if(!PointList_Move(1,&(GPSData.Position),angle)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position |
| } |
| else |
| { // take last target position |
| if(!PointList_Move(1, &(NaviData.TargetPosition),angle)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position |
| } |
| } |
| if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
| GPS_pWaypoint = PointList_WPBegin(0); // updates POI index |
| SpeakWaypointRached = 1; // Speak once when the last Point is reached |
| SpeakNextWaypoint = 1; // Speak once as soon as the Points are active |
| NCFlags &= ~NC_FLAG_TARGET_REACHED; |
| BeepTime = 150; |
| SpeakHoTT = SPEAK_LOAD; |
| } |
| } |
| FromFC_LoadWP_List = 0; |
| } |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| if(FromFC_Save_SinglePoint) |
| { |
| WPL_Store.Index = FromFC_Save_SinglePoint; |
| if(WPL_Store.Index <= ToFC_MaxWpListIndex) if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) |
| { |
| BeepTime = 150; |
| SpeakHoTT = SPEAK_SAVE; |
| } |
| FromFC_Save_SinglePoint = 0; |
| } |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| if(FromFC_Load_SinglePoint) |
| { |
| WPL_Store.Index = FromFC_Load_SinglePoint; |
| if(WPL_Store.Index <= ToFC_MaxWpListIndex) |
| { |
| if(PointList_LoadSinglePoint(&WPL_Store) == WPL_OK) |
| { |
| if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
| GPS_pWaypoint = PointList_WPBegin(0); // updates POI index |
| SpeakWaypointRached = 1; // Speak once when the last Point is reached |
| SpeakNextWaypoint = 1; // Speak once as soon as the Points are active |
| NCFlags &= ~NC_FLAG_TARGET_REACHED; |
| BeepTime = 150; |
| SpeakHoTT = SPEAK_LOAD; |
| } |
| } |
| FromFC_Load_SinglePoint = 0; |
| } |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| // ---------------- Logging --------------------------------------- |
| if(SD_WatchDog) |
| { |
| SD_WatchDog = 30000; |
| if(SDCardInfo.Valid == 1) Logging_Update(); // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1 |
| else |
| { |
| ToFC_MaxWpListIndex = 0; |
| if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100; |
| } |
| if(!SD_WatchDog) UART1_PutString("\r\nSD-Watchdog - Logging aborted\r\n"); |
| } |
| |
| /* |
| if(CheckDelay(ftimer)) |
| { |
| |
| static s8 filename[35]; |
| static u8 i = 0; |
| s8 dbgmsg[40]; |
| |
| |
| |
| ftimer = SetDelay(100); |
| if(FC.Poti[3]>100 && fstate == 0) |
| { |
| fstate = 1; |
| sprintf(filename, "/toast/toasta/toast%02i.txt",i++); |
| } |
| else if(FC.Poti[3]<100 && fstate == 2) |
| { |
| fstate = 3; |
| } |
| |
| switch(fstate) |
| { |
| case 1: |
| sprintf(dbgmsg,"\r\nStart writing file: %s", filename); |
| Debug(dbgmsg); |
| f = fopen_(filename, 'a'); |
| if(f== NULL) Fat16_Init(); |
| fstate = 2; |
| break; |
| |
| case 2: |
| fputs_("\r\ntest edins sdv dsivbds iv dsivb disbv idsv bisd bv d suiv dsibsivbdis fbvisdöb visdbvisdb vidbfibds ibv", f); |
| break; |
| |
| case 3: |
| sprintf(dbgmsg,"\r\nClosing file: %s", filename); |
| Debug(dbgmsg); |
| fclose_(f); |
| fstate = 0; |
| break; |
| |
| default: |
| break; |
| } |
| } |
| */ |
| } |
| } |
| //DebugOut.Analog[] |