0,0 → 1,685 |
/*#######################################################################################*/ |
/* !!! 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 "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" |
|
|
|
#ifdef FOLLOW_ME |
u8 TransmitAlsoToFC = 0; |
#endif |
u32 TimerCheckError; |
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; |
u8 ToFC_MaxWpListIndex = 3; |
u8 ClearFCStatusFlags = 0; |
u8 StopNavigation = 0; |
volatile u32 PollingTimeout = 10000; |
Param_t Parameter; |
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 = 0,CountNewGpsDataIn5Sec = 0, FreqGpsProcessedIn5Sec = 0, FreqNewGpsDataIn5Sec = 0; |
u8 NewWPL_Name = 0; |
|
s8 ErrorMSG[25]; |
|
//---------------------------------------------------------------------------------------------------- |
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,"\n\r NaviCtrl HW: V%d.%d SW: V%d.%02d%c", Version_HW/10, Version_HW%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(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(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR:Flying range!"); |
newErrorCode = 28; |
} |
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; |
// if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected "); |
// else |
{ |
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; |
// UBX_Timeout = SetDelay(500); |
} |
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[1] & FC_ERROR1_SPI_RX)) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"FC spi rx error "); |
newErrorCode = 8; |
StopNavigation = 1; |
} |
else if(FC.Error[0] & FC_ERROR0_CAREFREE) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"FC: Carefree Error"); |
newErrorCode = 20; |
} |
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(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(((BL_MinOfMaxPWM == 40 && (FC.StatusFlags & FC_STATUS_FLY)) || BL_MinOfMaxPWM == 39) && !ErrorCode) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR:Motor restart "); |
newErrorCode = 23; |
DebugOut.StatusRed |= AMPEL_BL; |
} |
else if(BL_MinOfMaxPWM < 30 && !ErrorCode) |
{ |
u16 i; |
for(i = 0; i < 12; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break; |
LED_RED_ON; |
sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM); |
newErrorCode = 32; |
DebugOut.StatusRed |= AMPEL_BL; |
} |
else if(BL_MinOfMaxPWM < 248 && (FC.StatusFlags & FC_STATUS_FLY) && !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 range "); |
newErrorCode = 25; |
DebugOut.StatusRed |= AMPEL_NC; |
} |
else if((!SD_SWITCH || (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 / 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 // 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; |
} |
|
|
|
u8 Polling(void) |
{ |
static u8 running = 0, oldFcFlags = 0, count5sec; |
static u32 old_ms = 0; |
|
if(running) {/*DebugOut.Analog[18]++;*/ return(1);}; |
running = 1; |
|
if(CountMilliseconds != old_ms) // 1 ms |
{ |
old_ms = CountMilliseconds; |
Compass_Update(); // update compass communication |
Analog_Update(); // get new ADC values |
CalcHeadFree(); |
} |
|
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 |
|
// ---------------- Error Check Timing ---------------------------- |
if(CheckDelay(TimerCheckError) || (FC.StatusFlags & FC_STATUS_START && !(oldFcFlags & FC_STATUS_START))) // Timer or FY wants to start |
{ |
if(CheckDelay(TimerCheckError)) |
{ |
TimerCheckError = SetDelay(1000); |
if(CompassValueErrorCount) CompassValueErrorCount--; |
if(++count5sec == 5) |
{ |
count5sec = 0; |
FreqGpsProcessedIn5Sec = CountGpsProcessedIn5Sec * 2; |
FreqNewGpsDataIn5Sec = CountNewGpsDataIn5Sec * 2; |
CountGpsProcessedIn5Sec = 0; |
CountNewGpsDataIn5Sec = 0; |
} |
} |
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(FC.StatusFlags & FC_STATUS_FLY) NaviData.FlyingTime++; // we want to count the battery-time |
if(SerialLinkOkay) SerialLinkOkay--; |
if(SerialLinkOkay < 250 - 5) NCFlags |= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication |
else NCFlags &= ~NC_FLAG_NOSERIALLINK; |
if(StopNavigation && (Parameter.NaviGpsModeControl >= 50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000; |
} |
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[16]++; |
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 |
UART1_Init(); |
UART1_PutString("\r\n---------------------------------------------"); |
// 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) |
I2CBus_Init(I2C0); |
I2CBus_Init(I2C1); |
// initialize fat16 partition on sd card (needs Timer 1) |
Fat16_Init(); |
// initialize NC params |
NCParams_Init(); |
// initialize the settings |
Settings_Init(); |
// initialize logging (needs settings) |
Logging_Init(); |
|
LED_GRN_ON; |
TimerCheckError = SetDelay(3000); |
UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++"); |
UART1_PutString("\n\r Version information:"); |
|
GetNaviCtrlVersion(); |
DebugOut.StatusGreen = AMPEL_NC | AMPEL_COMPASS; // NC and MK3Mag |
DebugOut.StatusRed = 0x00; |
UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++"); |
|
Compass_Init(); |
|
GPS_Init(); |
|
#ifdef FOLLOW_ME |
TransmitAlsoToFC = 1; |
UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++"); |
UART1_PutString("\n\r FOLLOW-ME Transmitter only!"); |
UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++\r\n\r\n"); |
TransmitAlsoToFC = 0; |
#else |
SPI0_GetFlightCtrlVersion(); |
if(FC_Version.Compatible != FC_SPI_COMPATIBLE) |
{ |
UART1_PutString("\n\r Flight-Ctrl not compatible\n\r"); |
LED_RED_ON; |
} |
#endif |
// ---------- 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, &NMEA_Interval); |
UART1_PutString("\r\n"); |
CompassValueErrorCount = 0; |
I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000); |
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 |
{ |
if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance > 7*10) |
{ // take actual position |
if(!PointList_Move(1,&(GPSData.Position),NaviData.CompassHeading)) 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),NaviData.CompassHeading)) 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(); // updates POI index |
BeepTime = 150; |
} |
} |
FromFC_LoadWP_List = 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("\n\rSD-Watchdog - Logging aborted\n\r"); |
} |
|
/* |
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[16] |