Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 480 → Rev 481

/tags/V2.02a/main.c
0,0 → 1,648
/*#######################################################################################*/
/* !!! 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 "i2c0.h"
#include "i2c1.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 "ncmag.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 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;
 
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((I2C_CompassPort == I2C_INTERN_1 && CheckDelay(I2C1_Timeout)) || (I2C_CompassPort == I2C_EXTERN_0 && CheckDelay(I2C0_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(I2C_CompassPort == I2C_EXTERN_0) LED_RED_OFF_T;
 
if((FCCalibActive || CompassCalState) && FC_Version.Hardware)
{
sprintf(ErrorMSG,"Calibrate... ");
newErrorCode = 0;
ErrorCode = 0;
no_error_delay = 1;
}
else if(CheckDelay(I2C1_Timeout) && (I2C_CompassPort == I2C_INTERN_1))
{
LED_RED_ON;
sprintf(ErrorMSG,"no compass communica");
//Reset I2CBus
I2C1_Deinit();
I2C1_Init();
newErrorCode = 4;
StopNavigation = 1;
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
DebugOut.StatusRed |= AMPEL_COMPASS;
}
else if(CheckDelay(I2C0_Timeout) && (I2C_CompassPort == I2C_EXTERN_0))
{
LED_RED_ON;
LED_RED_ON_T;
sprintf(ErrorMSG,"no ext. compass ");
//Reset I2CBus
I2C0_Deinit();
I2C0_Init();
NCMAG_Update(1);
newErrorCode = 33;
StopNavigation = 1;
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_RX;
DebugOut.StatusRed |= AMPEL_COMPASS;
}
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)
{
unsigned int 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;
}
 
 
 
void Polling(void)
{
static u8 running = 0, oldFcFlags = 0, count5sec;
if(running) return;
running = 1;
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
CalcHeadFree();
// ---------------- 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(++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;
}
 
// 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)
{
IENABLE;
VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
Compass_Update(); // update compass communication
Analog_Update(); // get new ADC values
 
if(!PollingTimeout)
{
PollingTimeout = 5;
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)
I2C0_Init();
I2C1_Init();
 
// 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");
for (;;) // the endless main loop
{
Polling();
PollingTimeout = 15;
// ---------------- 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 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]