WebSVN - NaviCtrl - Diff - Rev 254 and 255


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 Ingo Busker, Holger Buss
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
// + FOR NON COMMERCIAL USE ONLY
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Teile, die nicht eindeutig mit unserer Lizenzvereinbarung gekennzeichnet sind, unterliegen u.U. eigenen Vereinbarungen:
// + z.B. Die Funktion printf_P() unterliegt ihrer eigenen Lizenz
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
//
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//#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 "timer1.h"
#include "timer2.h"
#include "analog.h"
#include "spi_slave.h"
#include "fat16.h"
#include "usb.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"
 
u8 BoardRelease = 0;
u32 TimerCheckError;
u8 ErrorCode = 0;
u16 BeepTime;
u8  NCFlags = 0;
s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
 
u8 ClearFCStatusFlags = 0;
u8 StopNavigation = 0;
Param_t Parameter;
volatile FC_t FC;
 
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 V%d.%d%c",  VERSION_MAJOR,  VERSION_MINOR, 'a'+ VERSION_PATCH);
        UART1_PutString(msg);
}
 
//----------------------------------------------------------------------------------------------------
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
#define ERROR_FC_COMMUNICATION          0x01
#define ERROR_MK3_COMMUNICATION         0x02
#define ERROR_FC_INCOMPATIBLE           0x04
#define ERROR_MK3_INCOMPATIBLE          0x08
#define ERROR_GPS_COMMUNICATION         0x10
#define ERROR_COMPASS_VALUE                     0x20
//----------------------------------------------------------------------------------------------------
 
void CheckErrors(void)
{
        UART_VersionInfo.HardwareError[0] = 0;
 
        if(/*(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) ||*/ CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.Status[1] |= 0x08;
        else DebugOut.Status[1] &= ~0x08; // MK3Mag green status
 
        if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.Status[1] |= 0x02;
        else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status
 
        if(CheckDelay(SPI0_Timeout))
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"no FC communication ");
                ErrorCode = 3;
                StopNavigation = 1;
                DebugOut.Status[0] &= ~0x01; // status of FC Present
                DebugOut.Status[0] &= ~0x02; // status of BL Present
                UART_VersionInfo.HardwareError[0] |= ERROR_FC_COMMUNICATION;
        }
        else if(CheckDelay(I2C1_Timeout))
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"no MK3Mag communication ");
                //Reset I2CBus
                I2C1_Deinit();
                I2C1_Init();
                ErrorCode = 4;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= ERROR_MK3_COMMUNICATION;
                DebugOut.Status[1] |= 0x08;
        }
        else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"FC not compatible ");
                ErrorCode = 1;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= ERROR_FC_INCOMPATIBLE;
        }
 
        else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Nick Gyro");
                ErrorCode = 10;
        }
        else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Roll Gyro");
                ErrorCode = 11;
        }
        else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
                ErrorCode = 12;
        }
        else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Nick ACC");
                ErrorCode = 13;
        }
        else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Roll ACC");
                ErrorCode = 14;
        }
        else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC Z-ACC");
                ErrorCode = 15;
        }
        else if(FC.Error[0] & FC_ERROR0_PRESSURE)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: Pressure sensor");
                ErrorCode = 16;
        }
        else if(FC.Error[1] &  FC_ERROR1_I2C)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: FC I2C");
                ErrorCode = 17;
        }
        else if(FC.Error[1] &  FC_ERROR1_BL_MISSING)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"ERR: Bl Missing");
                ErrorCode = 18;
        }
        else if(FC.Error[1] &  FC_ERROR1_MIXER)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"Mixer Error");
                ErrorCode = 19;
        }
/*      else if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
        {
                sprintf(ErrorMSG,"MK3Mag not compatible ");
                LED_RED_ON;
                ErrorCode = 2;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= ERROR_MK3_INCOMPATIBLE;
        } */
        else if(CheckDelay(UBX_Timeout))
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"no GPS communication ");
                ErrorCode = 5;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= ERROR_GPS_COMMUNICATION;
        }
        else if(Compass_Heading < 0)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"bad compass value ");
                ErrorCode = 6;
                StopNavigation = 1;
                UART_VersionInfo.HardwareError[0] |= ERROR_COMPASS_VALUE;
        }
        else if((FC.Error[1] &  FC_ERROR1_SPI_RX))
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"FC spi rx error ");
                ErrorCode = 8;
                StopNavigation = 1;
        }
        else if(FC.Error[0] &  FC_ERROR0_CAREFREE)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"FC: Carefree Error");
                ErrorCode = 20;
        }
        else if(FC.Error[1] &  FC_ERROR1_PPM)
        {
                LED_RED_ON;
                sprintf(ErrorMSG,"RC Signal lost ");
                ErrorCode = 7;
        }
        else // no error occured
        {
                sprintf(ErrorMSG,"No Error               ");
                ErrorCode = 0;
                StopNavigation = 0;
                LED_RED_OFF;
        }
        if(UART_VersionInfo.HardwareError[0] || UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 0x04;
        else DebugOut.Status[1] &= ~0x04;
}
 
// 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
        VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
        Compass_UpdateHeading();        // update compass communication
        Analog_Update();                // get new ADC values
        VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
 
        IDISABLE;
}
 
//----------------------------------------------------------------------------------------------------
int main(void)
{
        /* 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 usb
        USB_ConfigInit();
        // initialize SPI0 to FC
        SPI0_Init();
        // initialize i2c bus (needs Timer 1)
        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();
 
        TimerCheckError = SetDelay(3000);
        UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
        UART1_PutString("\n\r Version information:");
 
        GetNaviCtrlVersion();
        DebugOut.Status[0] = 0x04 | 0x08; // NC and MK3Mag
        DebugOut.Status[1] = 0x00;
 
        Compass_Init();
       
        SPI0_GetFlightCtrlVersion();
        if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
        {
                UART1_PutString("\n\r Flight-Ctrl not compatible");
                LED_RED_ON;
        }
 
        GPS_Init();    
 
        /*{
        u8 i;
        u8 text[20];
        u8 mesg[10];
        UART1_PutString("\r\nFill\r\n");
        for(i=0;i<20;i++)
        {
                text[i] = 0xAA;
                sprintf(mesg,"%02X ", text[i]);
                UART1_PutString(mesg);
        }
 
        sprintf(mesg, "\r\nEERead = %d\r\n", EEPROM_ReadBlock(50, text, 18));
        UART1_PutString(mesg);
        for(i=0;i<20;i++)
        {
                sprintf(mesg,"%02X ", text[i]);
                UART1_PutString(mesg);
                text[i] = i;
        }
        UART1_PutString("\r\nFill\r\n");
        for(i=0;i<20;i++)
        {
                sprintf(mesg,"%02X ", text[i]);
                UART1_PutString(mesg);
        }
        sprintf(mesg, "\r\nEEWrite = %d\r\n", EEPROM_WriteBlock(0, text, 18));
        UART1_PutString(mesg);
        for(i=0;i<20;i++)
        {
                sprintf(mesg,"%02X ", text[i]);
                UART1_PutString(mesg);
                text[i]=0xAA;
        }
        UART1_PutString("\r\nFill\r\n");
        for(i=0;i<20;i++)
        {
                sprintf(mesg,"%02X ", text[i]);
                UART1_PutString(mesg);
        }
        sprintf(mesg, "\r\nEERead = %d\r\n", EEPROM_ReadBlock(0, text, 18));
        UART1_PutString(mesg);
        for(i=0;i<20;i++)
        {
                sprintf(mesg,"%02X ", text[i]);
                UART1_PutString(mesg);
        }
        }*/
 
        // ---------- 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");
 
 
        for (;;) // the endless main loop
        {
                UART0_ProcessRxData();  // process request
                UART1_ProcessRxData();  // process request
                USB_ProcessRxData();    // process request
                UART0_TransmitTxData(); // send answer
                UART1_TransmitTxData(); // send answer
                UART2_TransmitTxData(); // send answer
                USB_TransmitTxData();   // send answer
                SPI0_UpdateBuffer();    // handle new SPI Data
                // ---------------- Error Check Timing ----------------------------
                if(CheckDelay(TimerCheckError))
                {
                        TimerCheckError = SetDelay(1000);
                        if(CheckDelay(SPI0_Timeout)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
                        CheckErrors();
                        if(FC.StatusFlags & FC_STATUS_FLY) NaviData.FlyingTime++; // we want to count the battery-time
//                      else NaviData.FlyingTime = 0; // not the time per flight
                        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)) BeepTime = 1000;
                }
                // ---------------- Logging  ---------------------------------------
                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
        }
}
 
 
 

Rev 254	Rev 255
1	/#######################################################################################/	1	/#######################################################################################/
2	/* !!! THIS IS NOT FREE SOFTWARE !!! */	2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
3	/#######################################################################################/	3	/#######################################################################################/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5	// + Copyright (c) 2008 Ingo Busker, Holger Buss	5	// + Copyright (c) 2008 Ingo Busker, Holger Buss
6	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY	6	// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
7	// + FOR NON COMMERCIAL USE ONLY	7	// + FOR NON COMMERCIAL USE ONLY
8	// + www.MikroKopter.com	8	// + www.MikroKopter.com
9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	9	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	10	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	11	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	12	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13	// + bzgl. der Nutzungsbedingungen aufzunehmen.	13	// + bzgl. der Nutzungsbedingungen aufzunehmen.
14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	14	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15	// + Verkauf von Luftbildaufnahmen, usw.	15	// + Verkauf von Luftbildaufnahmen, usw.
16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	16	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	17	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	18	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	20	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	21	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22	// + eindeutig als Ursprung verlinkt werden	22	// + eindeutig als Ursprung verlinkt werden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	24	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25	// + Benutzung auf eigene Gefahr	25	// + Benutzung auf eigene Gefahr
26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	26	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	27	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	28	// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
29	// + mit unserer Zustimmung zulässig	29	// + mit unserer Zustimmung zulässig
30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	30	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31	// + Teile, die nicht eindeutig mit unserer Lizenzvereinbarung gekennzeichnet sind, unterliegen u.U. eigenen Vereinbarungen:	31	// + Teile, die nicht eindeutig mit unserer Lizenzvereinbarung gekennzeichnet sind, unterliegen u.U. eigenen Vereinbarungen:
32	// + z.B. Die Funktion printf_P() unterliegt ihrer eigenen Lizenz	32	// + z.B. Die Funktion printf_P() unterliegt ihrer eigenen Lizenz
33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	34	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
35	// + this list of conditions and the following disclaimer.	35	// + this list of conditions and the following disclaimer.
36	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	36	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
37	// + from this software without specific prior written permission.	37	// + from this software without specific prior written permission.
38	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted	38	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
39	// + for non-commercial use (directly or indirectly)	39	// + for non-commercial use (directly or indirectly)
40	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	40	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
41	// + with our written permission	41	// + with our written permission
42	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	42	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
43	// + clearly linked as origin	43	// + clearly linked as origin
44	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed	44	// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
45	//	45	//
46	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	46	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
47	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	47	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
48	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	48	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
49	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	49	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
50	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	50	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
51	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	51	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
52	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	52	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
53	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN	53	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
54	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	54	// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
55	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	55	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
56	// + POSSIBILITY OF SUCH DAMAGE.	56	// + POSSIBILITY OF SUCH DAMAGE.
57	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	57	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
58	//#define MCLK96MHZ	58	//#define MCLK96MHZ
59	const unsigned long _Main_Crystal = 25000;	59	const unsigned long _Main_Crystal = 25000;
60	#include <stdio.h>	60	#include <stdio.h>
61	#include "91x_lib.h"	61	#include "91x_lib.h"
62	#include "led.h"	62	#include "led.h"
63	#include "uart0.h"	63	#include "uart0.h"
64	#include "uart1.h"	64	#include "uart1.h"
65	#include "uart2.h"	65	#include "uart2.h"
66	#include "gps.h"	66	#include "gps.h"
67	#include "i2c.h"	67	#include "i2c.h"
68	#include "compass.h"	68	#include "compass.h"
69	#include "timer1.h"	69	#include "timer1.h"
70	#include "timer2.h"	70	#include "timer2.h"
71	#include "analog.h"	71	#include "analog.h"
72	#include "spi_slave.h"	72	#include "spi_slave.h"
73	#include "fat16.h"	73	#include "fat16.h"
74	#include "usb.h"	74	#include "usb.h"
75	#include "sdc.h"	75	#include "sdc.h"
76	#include "logging.h"	76	#include "logging.h"
77	#include "params.h"	77	#include "params.h"
78	#include "settings.h"	78	#include "settings.h"
79	#include "config.h"	79	#include "config.h"
80	#include "main.h"	80	#include "main.h"
81	#include "debug.h"	81	#include "debug.h"
82		82
83	#include "eeprom.h"	83	#include "eeprom.h"
84		84
85	u8 BoardRelease = 0;	85	u8 BoardRelease = 0;
86	u32 TimerCheckError;	86	u32 TimerCheckError;
87	u8 ErrorCode = 0;	87	u8 ErrorCode = 0;
88	u16 BeepTime;	88	u16 BeepTime;
89	u8 NCFlags = 0;	89	u8 NCFlags = 0;
90	s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg	90	s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg
91		91
92	u8 ClearFCFlags = 0;	92	u8 ClearFCStatusFlags = 0;
93	u8 StopNavigation = 0;	93	u8 StopNavigation = 0;
94	Param_t Parameter;	94	Param_t Parameter;
95	volatile FC_t FC;	95	volatile FC_t FC;
96		96
97	s8 ErrorMSG[25];	97	s8 ErrorMSG[25];
98		98
99	//----------------------------------------------------------------------------------------------------	99	//----------------------------------------------------------------------------------------------------
100	void SCU_Config(void)	100	void SCU_Config(void)
101	{	101	{
102	/* configure PLL and set it as master clock source */	102	/* configure PLL and set it as master clock source */
103	SCU_MCLKSourceConfig(SCU_MCLK_OSC); // set master clock source to external oscillator clock (25MHz) before diabling the PLL	103	SCU_MCLKSourceConfig(SCU_MCLK_OSC); // set master clock source to external oscillator clock (25MHz) before diabling the PLL
104	SCU_PLLCmd(DISABLE); // now disable the PLL	104	SCU_PLLCmd(DISABLE); // now disable the PLL
105	#ifdef MCLK96MHZ	105	#ifdef MCLK96MHZ
106	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz	106	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div2); // set BRCLK to MCLK/2 = 48MHz
107	SCU_PCLKDivisorConfig(SCU_PCLK_Div4); // set PCLK (APB bus clock) divisor to 4 (half Reference Clock)	107	SCU_PCLKDivisorConfig(SCU_PCLK_Div4); // set PCLK (APB bus clock) divisor to 4 (half Reference Clock)
108	SCU_RCLKDivisorConfig(SCU_RCLK_Div2); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)	108	SCU_RCLKDivisorConfig(SCU_RCLK_Div2); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)
109	SCU_HCLKDivisorConfig(SCU_HCLK_Div2); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)	109	SCU_HCLKDivisorConfig(SCU_HCLK_Div2); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)
110	SCU_PLLFactorsConfig(192,25,2); // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2	110	SCU_PLLFactorsConfig(192,25,2); // PLL = 96 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=2
111	#else	111	#else
112	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz	112	SCU_BRCLKDivisorConfig(SCU_BRCLK_Div1); // set BRCLK to MCLK = 48MHz
113	SCU_PCLKDivisorConfig(SCU_PCLK_Div2); // set PCLK (APB bus clock) divisor to 2 (half Reference Clock)	113	SCU_PCLKDivisorConfig(SCU_PCLK_Div2); // set PCLK (APB bus clock) divisor to 2 (half Reference Clock)
114	SCU_RCLKDivisorConfig(SCU_RCLK_Div1); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)	114	SCU_RCLKDivisorConfig(SCU_RCLK_Div1); // set RCLK (Reference Clock) divisor to 1 (full PPL clock)
115	SCU_HCLKDivisorConfig(SCU_HCLK_Div1); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)	115	SCU_HCLKDivisorConfig(SCU_HCLK_Div1); // set HCLK (AHB bus clock) divisor to 1 (full Reference Clock)
116	SCU_PLLFactorsConfig(192,25,3); // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3	116	SCU_PLLFactorsConfig(192,25,3); // PLL = 48 MHz, Feedback Divider N=192, Pre-Divider M=25, Post-Divider P=3
117	#endif	117	#endif
118	SCU_PLLCmd(ENABLE); // Enable PLL (is disabled by SCU_PLLFactorsConfig)	118	SCU_PLLCmd(ENABLE); // Enable PLL (is disabled by SCU_PLLFactorsConfig)
119	SCU_MCLKSourceConfig(SCU_MCLK_PLL); // set master clock source to PLL	119	SCU_MCLKSourceConfig(SCU_MCLK_PLL); // set master clock source to PLL
120	}	120	}
121		121
122	//----------------------------------------------------------------------------------------------------	122	//----------------------------------------------------------------------------------------------------
123	void GetNaviCtrlVersion(void)	123	void GetNaviCtrlVersion(void)
124	{	124	{
125	u8 msg[25];	125	u8 msg[25];
126		126
127	sprintf(msg,"\n\r NaviCtrl V%d.%d%c", VERSION_MAJOR, VERSION_MINOR, 'a'+ VERSION_PATCH);	127	sprintf(msg,"\n\r NaviCtrl V%d.%d%c", VERSION_MAJOR, VERSION_MINOR, 'a'+ VERSION_PATCH);
128	UART1_PutString(msg);	128	UART1_PutString(msg);
129	}	129	}
130		130
131	//----------------------------------------------------------------------------------------------------	131	//----------------------------------------------------------------------------------------------------
132	#define DEFEKT_G_NICK 0x01	-
133	#define DEFEKT_G_ROLL 0x02	-
134	#define DEFEKT_G_GIER 0x04	-
135	#define DEFEKT_A_NICK 0x08	-
136	#define DEFEKT_A_ROLL 0x10	-
137	#define DEFEKT_A_Z 0x20	-
138	#define DEFEKT_PRESSURE 0x40	-
139	#define DEFEKT_CAREFREE_ERR 0x80	-
140		-
141	#define DEFEKT_I2C 0x01	-
142	#define DEFEKT_BL_MISSING 0x02	-
143	#define DEFEKT_SPI_RX_ERR 0x04	-
144	#define DEFEKT_PPM_ERR 0x08	-
145	#define DEFEKT_MIXER_ERR 0x10	-
146	//----------------------------------------------------------------------------------------------------	-
147	#define ERROR_FC_COMMUNICATION 0x01	132	#define ERROR_FC_COMMUNICATION 0x01
148	#define ERROR_MK3_COMMUNICATION 0x02	133	#define ERROR_MK3_COMMUNICATION 0x02
149	#define ERROR_FC_INCOMPATIBLE 0x04	134	#define ERROR_FC_INCOMPATIBLE 0x04
150	#define ERROR_MK3_INCOMPATIBLE 0x08	135	#define ERROR_MK3_INCOMPATIBLE 0x08
151	#define ERROR_GPS_COMMUNICATION 0x10	136	#define ERROR_GPS_COMMUNICATION 0x10
152	#define ERROR_COMPASS_VALUE 0x20	137	#define ERROR_COMPASS_VALUE 0x20
153	//----------------------------------------------------------------------------------------------------	138	//----------------------------------------------------------------------------------------------------
154		139
155	void CheckErrors(void)	140	void CheckErrors(void)
156	{	141	{
157	UART_VersionInfo.HardwareError[0] = 0;	142	UART_VersionInfo.HardwareError[0] = 0;
158		143
159	if(/(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) \|\|/ CheckDelay(I2C1_Timeout) \|\| (Compass_Heading < 0)) DebugOut.Status[1] \|= 0x08;	144	if(/(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) \|\|/ CheckDelay(I2C1_Timeout) \|\| (Compass_Heading < 0)) DebugOut.Status[1] \|= 0x08;
160	else DebugOut.Status[1] &= ~0x08; // MK3Mag green status	145	else DebugOut.Status[1] &= ~0x08; // MK3Mag green status
161		146
162	if((FC_ErrorCode[1] & DEFEKT_I2C) \|\| (FC_ErrorCode[1] & DEFEKT_BL_MISSING)) DebugOut.Status[1] \|= 0x02;	147	if((FC.Error[1] & FC_ERROR1_I2C) \|\| (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.Status[1] \|= 0x02;
163	else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status	148	else DebugOut.Status[1] &= ~0x02; // BL-Ctrl green status
164		149
165	if(CheckDelay(SPI0_Timeout))	150	if(CheckDelay(SPI0_Timeout))
166	{	151	{
167	LED_RED_ON;	152	LED_RED_ON;
168	sprintf(ErrorMSG,"no FC communication ");	153	sprintf(ErrorMSG,"no FC communication ");
169	ErrorCode = 3;	154	ErrorCode = 3;
170	StopNavigation = 1;	155	StopNavigation = 1;
171	DebugOut.Status[0] &= ~0x01; // status of FC Present	156	DebugOut.Status[0] &= ~0x01; // status of FC Present
172	DebugOut.Status[0] &= ~0x02; // status of BL Present	157	DebugOut.Status[0] &= ~0x02; // status of BL Present
173	UART_VersionInfo.HardwareError[0] \|= ERROR_FC_COMMUNICATION;	158	UART_VersionInfo.HardwareError[0] \|= ERROR_FC_COMMUNICATION;
174	}	159	}
175	else if(CheckDelay(I2C1_Timeout))	160	else if(CheckDelay(I2C1_Timeout))
176	{	161	{
177	LED_RED_ON;	162	LED_RED_ON;
178	sprintf(ErrorMSG,"no MK3Mag communication ");	163	sprintf(ErrorMSG,"no MK3Mag communication ");
179	//Reset I2CBus	164	//Reset I2CBus
180	I2C1_Deinit();	165	I2C1_Deinit();
181	I2C1_Init();	166	I2C1_Init();
182	ErrorCode = 4;	167	ErrorCode = 4;
183	StopNavigation = 1;	168	StopNavigation = 1;
184	UART_VersionInfo.HardwareError[0] \|= ERROR_MK3_COMMUNICATION;	169	UART_VersionInfo.HardwareError[0] \|= ERROR_MK3_COMMUNICATION;
185	DebugOut.Status[1] \|= 0x08;	170	DebugOut.Status[1] \|= 0x08;
186	}	171	}
187	else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)	172	else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
188	{	173	{
189	LED_RED_ON;	174	LED_RED_ON;
190	sprintf(ErrorMSG,"FC not compatible ");	175	sprintf(ErrorMSG,"FC not compatible ");
191	ErrorCode = 1;	176	ErrorCode = 1;
192	StopNavigation = 1;	177	StopNavigation = 1;
193	UART_VersionInfo.HardwareError[0] \|= ERROR_FC_INCOMPATIBLE;	178	UART_VersionInfo.HardwareError[0] \|= ERROR_FC_INCOMPATIBLE;
194	}	179	}
195		180
196	else if(FC_ErrorCode[0] & DEFEKT_G_NICK)	181	else if(FC.Error[0] & FC_ERROR0_GYRO_NICK)
197	{	182	{
198	LED_RED_ON;	183	LED_RED_ON;
199	sprintf(ErrorMSG,"ERR: FC Nick Gyro");	184	sprintf(ErrorMSG,"ERR: FC Nick Gyro");
200	ErrorCode = 10;	185	ErrorCode = 10;
201	}	186	}
202	else if(FC_ErrorCode[0] & DEFEKT_G_ROLL)	187	else if(FC.Error[0] & FC_ERROR0_GYRO_ROLL)
203	{	188	{
204	LED_RED_ON;	189	LED_RED_ON;
205	sprintf(ErrorMSG,"ERR: FC Roll Gyro");	190	sprintf(ErrorMSG,"ERR: FC Roll Gyro");
206	ErrorCode = 11;	191	ErrorCode = 11;
207	}	192	}
208	else if(FC_ErrorCode[0] & DEFEKT_G_GIER)	193	else if(FC.Error[0] & FC_ERROR0_GYRO_YAW)
209	{	194	{
210	LED_RED_ON;	195	LED_RED_ON;
211	sprintf(ErrorMSG,"ERR: FC Yaw Gyro");	196	sprintf(ErrorMSG,"ERR: FC Yaw Gyro");
212	ErrorCode = 12;	197	ErrorCode = 12;
213	}	198	}
214	else if(FC_ErrorCode[0] & DEFEKT_A_NICK)	199	else if(FC.Error[0] & FC_ERROR0_ACC_NICK)
215	{	200	{
216	LED_RED_ON;	201	LED_RED_ON;
217	sprintf(ErrorMSG,"ERR: FC Nick ACC");	202	sprintf(ErrorMSG,"ERR: FC Nick ACC");
218	ErrorCode = 13;	203	ErrorCode = 13;
219	}	204	}
220	else if(FC_ErrorCode[0] & DEFEKT_A_ROLL)	205	else if(FC.Error[0] & FC_ERROR0_ACC_ROLL)
221	{	206	{
222	LED_RED_ON;	207	LED_RED_ON;
223	sprintf(ErrorMSG,"ERR: FC Roll ACC");	208	sprintf(ErrorMSG,"ERR: FC Roll ACC");
224	ErrorCode = 14;	209	ErrorCode = 14;
225	}	210	}
226	else if(FC_ErrorCode[0] & DEFEKT_A_Z)	211	else if(FC.Error[0] & FC_ERROR0_ACC_TOP)
227	{	212	{
228	LED_RED_ON;	213	LED_RED_ON;
229	sprintf(ErrorMSG,"ERR: FC Z-ACC");	214	sprintf(ErrorMSG,"ERR: FC Z-ACC");
230	ErrorCode = 15;	215	ErrorCode = 15;
231	}	216	}
232	else if(FC_ErrorCode[0] & DEFEKT_PRESSURE)	217	else if(FC.Error[0] & FC_ERROR0_PRESSURE)
233	{	218	{
234	LED_RED_ON;	219	LED_RED_ON;
235	sprintf(ErrorMSG,"ERR: Pressure sensor");	220	sprintf(ErrorMSG,"ERR: Pressure sensor");
236	ErrorCode = 16;	221	ErrorCode = 16;
237	}	222	}
238	else if(FC_ErrorCode[1] & DEFEKT_I2C)	223	else if(FC.Error[1] & FC_ERROR1_I2C)
239	{	224	{
240	LED_RED_ON;	225	LED_RED_ON;
241	sprintf(ErrorMSG,"ERR: FC I2C");	226	sprintf(ErrorMSG,"ERR: FC I2C");
242	ErrorCode = 17;	227	ErrorCode = 17;
243	}	228	}
244	else if(FC_ErrorCode[1] & DEFEKT_BL_MISSING)	229	else if(FC.Error[1] & FC_ERROR1_BL_MISSING)
245	{	230	{
246	LED_RED_ON;	231	LED_RED_ON;
247	sprintf(ErrorMSG,"ERR: Bl Missing");	232	sprintf(ErrorMSG,"ERR: Bl Missing");
248	ErrorCode = 18;	233	ErrorCode = 18;
249	}	234	}
250	else if(FC_ErrorCode[1] & DEFEKT_MIXER_ERR)	235	else if(FC.Error[1] & FC_ERROR1_MIXER)
251	{	236	{
252	LED_RED_ON;	237	LED_RED_ON;
253	sprintf(ErrorMSG,"Mixer Error");	238	sprintf(ErrorMSG,"Mixer Error");
254	ErrorCode = 19;	239	ErrorCode = 19;
255	}	240	}
256	/* else if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)	241	/* else if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
257	{	242	{
258	sprintf(ErrorMSG,"MK3Mag not compatible ");	243	sprintf(ErrorMSG,"MK3Mag not compatible ");
259	LED_RED_ON;	244	LED_RED_ON;
260	ErrorCode = 2;	245	ErrorCode = 2;
261	StopNavigation = 1;	246	StopNavigation = 1;
262	UART_VersionInfo.HardwareError[0] \|= ERROR_MK3_INCOMPATIBLE;	247	UART_VersionInfo.HardwareError[0] \|= ERROR_MK3_INCOMPATIBLE;
263	} */	248	} */
264	else if(CheckDelay(UBX_Timeout))	249	else if(CheckDelay(UBX_Timeout))
265	{	250	{
266	LED_RED_ON;	251	LED_RED_ON;
267	sprintf(ErrorMSG,"no GPS communication ");	252	sprintf(ErrorMSG,"no GPS communication ");
268	ErrorCode = 5;	253	ErrorCode = 5;
269	StopNavigation = 1;	254	StopNavigation = 1;
270	UART_VersionInfo.HardwareError[0] \|= ERROR_GPS_COMMUNICATION;	255	UART_VersionInfo.HardwareError[0] \|= ERROR_GPS_COMMUNICATION;
271	}	256	}
272	else if(Compass_Heading < 0)	257	else if(Compass_Heading < 0)
273	{	258	{
274	LED_RED_ON;	259	LED_RED_ON;
275	sprintf(ErrorMSG,"bad compass value ");	260	sprintf(ErrorMSG,"bad compass value ");
276	ErrorCode = 6;	261	ErrorCode = 6;
277	StopNavigation = 1;	262	StopNavigation = 1;
278	UART_VersionInfo.HardwareError[0] \|= ERROR_COMPASS_VALUE;	263	UART_VersionInfo.HardwareError[0] \|= ERROR_COMPASS_VALUE;
279	}	264	}
280	else if(FC.Flags & FCFLAG_SPI_RX_ERR)	265	else if((FC.Error[1] & FC_ERROR1_SPI_RX))
281	{	266	{
282	LED_RED_ON;	267	LED_RED_ON;
283	sprintf(ErrorMSG,"FC spi rx error ");	268	sprintf(ErrorMSG,"FC spi rx error ");
284	ErrorCode = 8;	269	ErrorCode = 8;
285	StopNavigation = 1;	270	StopNavigation = 1;
286	}	271	}
287	else if(FC_ErrorCode[0] & DEFEKT_CAREFREE_ERR)	272	else if(FC.Error[0] & FC_ERROR0_CAREFREE)
288	{	273	{
289	LED_RED_ON;	274	LED_RED_ON;
290	sprintf(ErrorMSG,"FC: Carefree Error");	275	sprintf(ErrorMSG,"FC: Carefree Error");
291	ErrorCode = 20;	276	ErrorCode = 20;
292	}	277	}
293	else if(FC.RC_Quality < 100)	278	else if(FC.Error[1] & FC_ERROR1_PPM)
294	{	279	{
295	LED_RED_ON;	280	LED_RED_ON;
296	sprintf(ErrorMSG,"RC Signal lost ");	281	sprintf(ErrorMSG,"RC Signal lost ");
297	ErrorCode = 7;	282	ErrorCode = 7;
298	}	283	}
299	else // no error occured	284	else // no error occured
300	{	285	{
301	sprintf(ErrorMSG,"No Error ");	286	sprintf(ErrorMSG,"No Error ");
302	ErrorCode = 0;	287	ErrorCode = 0;
303	StopNavigation = 0;	288	StopNavigation = 0;
304	LED_RED_OFF;	289	LED_RED_OFF;
305	}	290	}
306	if(UART_VersionInfo.HardwareError[0] \|\| UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] \|= 0x04;	291	if(UART_VersionInfo.HardwareError[0] \|\| UART_VersionInfo.HardwareError[1]) DebugOut.Status[1] \|= 0x04;
307	else DebugOut.Status[1] &= ~0x04;	292	else DebugOut.Status[1] &= ~0x04;
308	}	293	}
309		294
310	// the handler will be cyclic called by the timer 1 ISR	295	// the handler will be cyclic called by the timer 1 ISR
311	// used is for critical timing parts that normaly would handled	296	// used is for critical timing parts that normaly would handled
312	// within the main loop that could block longer at logging activities	297	// within the main loop that could block longer at logging activities
313	void EXTIT3_IRQHandler(void)	298	void EXTIT3_IRQHandler(void)
314	{	299	{
315	IENABLE;	300	IENABLE;
316		301
317	VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq	302	VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
318	VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit	303	VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
319	Compass_UpdateHeading(); // update compass communication	304	Compass_UpdateHeading(); // update compass communication
320	Analog_Update(); // get new ADC values	305	Analog_Update(); // get new ADC values
321	VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq	306	VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
322		307
323	IDISABLE;	308	IDISABLE;
324	}	309	}
325		310
326	//----------------------------------------------------------------------------------------------------	311	//----------------------------------------------------------------------------------------------------
327	int main(void)	312	int main(void)
328	{	313	{
329	/* Configure the system clocks */	314	/* Configure the system clocks */
330	SCU_Config();	315	SCU_Config();
331	/* init VIC (Vectored Interrupt Controller) */	316	/* init VIC (Vectored Interrupt Controller) */
332	SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC	317	SCU_AHBPeriphClockConfig(__VIC,ENABLE); // enable AHB bus clock for VIC
333	SCU_AHBPeriphReset(__VIC, DISABLE); // disable reset state for VIC	318	SCU_AHBPeriphReset(__VIC, DISABLE); // disable reset state for VIC
334	VIC_DeInit(); // deinitializes the VIC module registers to their default reset values.	319	VIC_DeInit(); // deinitializes the VIC module registers to their default reset values.
335	VIC_InitDefaultVectors();	320	VIC_InitDefaultVectors();
336		321
337	// initialize timer 1 for System Clock and delay rountines	322	// initialize timer 1 for System Clock and delay rountines
338	TIMER1_Init();	323	TIMER1_Init();
339	// initialize the LEDs (needs Timer 1)	324	// initialize the LEDs (needs Timer 1)
340	Led_Init();	325	Led_Init();
341	// initialize the debug UART1	326	// initialize the debug UART1
342	UART1_Init();	327	UART1_Init();
343	UART1_PutString("\r\n---------------------------------------------");	328	UART1_PutString("\r\n---------------------------------------------");
344	// initialize timer 2 for servo outputs	329	// initialize timer 2 for servo outputs
345	//TIMER2_Init();	330	//TIMER2_Init();
346	// initialize UART2 to FLIGHTCTRL	331	// initialize UART2 to FLIGHTCTRL
347	UART2_Init();	332	UART2_Init();
348	// initialize UART0 (to MKGPS or MK3MAG)	333	// initialize UART0 (to MKGPS or MK3MAG)
349	UART0_Init();	334	UART0_Init();
350	// initialize adc	335	// initialize adc
351	Analog_Init();	336	Analog_Init();
352	// initialize usb	337	// initialize usb
353	USB_ConfigInit();	338	USB_ConfigInit();
354	// initialize SPI0 to FC	339	// initialize SPI0 to FC
355	SPI0_Init();	340	SPI0_Init();
356	// initialize i2c bus (needs Timer 1)	341	// initialize i2c bus (needs Timer 1)
357	I2C1_Init();	342	I2C1_Init();
358	// initialize fat16 partition on sd card (needs Timer 1)	343	// initialize fat16 partition on sd card (needs Timer 1)
359	Fat16_Init();	344	Fat16_Init();
360	// initialize NC params	345	// initialize NC params
361	NCParams_Init();	346	NCParams_Init();
362	// initialize the settings	347	// initialize the settings
363	Settings_Init();	348	Settings_Init();
364	// initialize logging (needs settings)	349	// initialize logging (needs settings)
365	Logging_Init();	350	Logging_Init();
366		351
367	TimerCheckError = SetDelay(3000);	352	TimerCheckError = SetDelay(3000);
368	UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");	353	UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
369	UART1_PutString("\n\r Version information:");	354	UART1_PutString("\n\r Version information:");
370		355
371	GetNaviCtrlVersion();	356	GetNaviCtrlVersion();
372	DebugOut.Status[0] = 0x04 \| 0x08; // NC and MK3Mag	357	DebugOut.Status[0] = 0x04 \| 0x08; // NC and MK3Mag
373	DebugOut.Status[1] = 0x00;	358	DebugOut.Status[1] = 0x00;
374		359
375	Compass_Init();	360	Compass_Init();
376		361
377	SPI0_GetFlightCtrlVersion();	362	SPI0_GetFlightCtrlVersion();
378	if(FC_Version.Compatible != FC_SPI_COMPATIBLE)	363	if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
379	{	364	{
380	UART1_PutString("\n\r Flight-Ctrl not compatible");	365	UART1_PutString("\n\r Flight-Ctrl not compatible");
381	LED_RED_ON;	366	LED_RED_ON;
382	}	367	}
383		368
384	GPS_Init();	369	GPS_Init();
385		370
386	/*{	371	/*{
387	u8 i;	372	u8 i;
388	u8 text[20];	373	u8 text[20];
389	u8 mesg[10];	374	u8 mesg[10];
390	UART1_PutString("\r\nFill\r\n");	375	UART1_PutString("\r\nFill\r\n");
391	for(i=0;i<20;i++)	376	for(i=0;i<20;i++)
392	{	377	{
393	text[i] = 0xAA;	378	text[i] = 0xAA;
394	sprintf(mesg,"%02X ", text[i]);	379	sprintf(mesg,"%02X ", text[i]);
395	UART1_PutString(mesg);	380	UART1_PutString(mesg);
396	}	381	}
397		382
398	sprintf(mesg, "\r\nEERead = %d\r\n", EEPROM_ReadBlock(50, text, 18));	383	sprintf(mesg, "\r\nEERead = %d\r\n", EEPROM_ReadBlock(50, text, 18));
399	UART1_PutString(mesg);	384	UART1_PutString(mesg);
400	for(i=0;i<20;i++)	385	for(i=0;i<20;i++)
401	{	386	{
402	sprintf(mesg,"%02X ", text[i]);	387	sprintf(mesg,"%02X ", text[i]);
403	UART1_PutString(mesg);	388	UART1_PutString(mesg);
404	text[i] = i;	389	text[i] = i;
405	}	390	}
406	UART1_PutString("\r\nFill\r\n");	391	UART1_PutString("\r\nFill\r\n");
407	for(i=0;i<20;i++)	392	for(i=0;i<20;i++)
408	{	393	{
409	sprintf(mesg,"%02X ", text[i]);	394	sprintf(mesg,"%02X ", text[i]);
410	UART1_PutString(mesg);	395	UART1_PutString(mesg);
411	}	396	}
412	sprintf(mesg, "\r\nEEWrite = %d\r\n", EEPROM_WriteBlock(0, text, 18));	397	sprintf(mesg, "\r\nEEWrite = %d\r\n", EEPROM_WriteBlock(0, text, 18));
413	UART1_PutString(mesg);	398	UART1_PutString(mesg);
414	for(i=0;i<20;i++)	399	for(i=0;i<20;i++)
415	{	400	{
416	sprintf(mesg,"%02X ", text[i]);	401	sprintf(mesg,"%02X ", text[i]);
417	UART1_PutString(mesg);	402	UART1_PutString(mesg);
418	text[i]=0xAA;	403	text[i]=0xAA;
419	}	404	}
420	UART1_PutString("\r\nFill\r\n");	405	UART1_PutString("\r\nFill\r\n");
421	for(i=0;i<20;i++)	406	for(i=0;i<20;i++)
422	{	407	{
423	sprintf(mesg,"%02X ", text[i]);	408	sprintf(mesg,"%02X ", text[i]);
424	UART1_PutString(mesg);	409	UART1_PutString(mesg);
425	}	410	}
426	sprintf(mesg, "\r\nEERead = %d\r\n", EEPROM_ReadBlock(0, text, 18));	411	sprintf(mesg, "\r\nEERead = %d\r\n", EEPROM_ReadBlock(0, text, 18));
427	UART1_PutString(mesg);	412	UART1_PutString(mesg);
428	for(i=0;i<20;i++)	413	for(i=0;i<20;i++)
429	{	414	{
430	sprintf(mesg,"%02X ", text[i]);	415	sprintf(mesg,"%02X ", text[i]);
431	UART1_PutString(mesg);	416	UART1_PutString(mesg);
432	}	417	}
433	}*/	418	}*/
434		419
435	// ---------- Prepare the isr driven	420	// ---------- Prepare the isr driven
436	// set to absolute lowest priority	421	// set to absolute lowest priority
437	VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);	422	VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);
438	// enable interrupts	423	// enable interrupts
439	VIC_ITCmd(EXTIT3_ITLine, ENABLE);	424	VIC_ITCmd(EXTIT3_ITLine, ENABLE);
440		425
441	Debug_OK("START");	426	Debug_OK("START");
442		427
443		428
444	for (;;) // the endless main loop	429	for (;;) // the endless main loop
445	{	430	{
446	UART0_ProcessRxData(); // process request	431	UART0_ProcessRxData(); // process request
447	UART1_ProcessRxData(); // process request	432	UART1_ProcessRxData(); // process request
448	USB_ProcessRxData(); // process request	433	USB_ProcessRxData(); // process request
449	UART0_TransmitTxData(); // send answer	434	UART0_TransmitTxData(); // send answer
450	UART1_TransmitTxData(); // send answer	435	UART1_TransmitTxData(); // send answer
451	UART2_TransmitTxData(); // send answer	436	UART2_TransmitTxData(); // send answer
452	USB_TransmitTxData(); // send answer	437	USB_TransmitTxData(); // send answer
453	SPI0_UpdateBuffer(); // handle new SPI Data	438	SPI0_UpdateBuffer(); // handle new SPI Data
454	// ---------------- Error Check Timing ----------------------------	439	// ---------------- Error Check Timing ----------------------------
455	if(CheckDelay(TimerCheckError))	440	if(CheckDelay(TimerCheckError))
456	{	441	{
457	TimerCheckError = SetDelay(1000);	442	TimerCheckError = SetDelay(1000);
458	if(CheckDelay(SPI0_Timeout)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected	443	if(CheckDelay(SPI0_Timeout)) GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick)); // process the GPS data even if the FC is not connected
459	CheckErrors();	444	CheckErrors();
460	if(FC.Flags & FCFLAG_FLY) NaviData.FlyingTime++; // we want to count the battery-time	445	if(FC.StatusFlags & FC_STATUS_FLY) NaviData.FlyingTime++; // we want to count the battery-time
461	// else NaviData.FlyingTime = 0; // not the time per flight	446	// else NaviData.FlyingTime = 0; // not the time per flight
462	if(SerialLinkOkay) SerialLinkOkay--;	447	if(SerialLinkOkay) SerialLinkOkay--;
463	if(SerialLinkOkay < 250 - 5) NCFlags \|= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication	448	if(SerialLinkOkay < 250 - 5) NCFlags \|= NC_FLAG_NOSERIALLINK; // 5 seconds timeout for serial communication
464	else NCFlags &= ~NC_FLAG_NOSERIALLINK;	449	else NCFlags &= ~NC_FLAG_NOSERIALLINK;
465	if(StopNavigation && (Parameter.NaviGpsModeControl >= 50)) BeepTime = 1000;	450	if(StopNavigation && (Parameter.NaviGpsModeControl >= 50)) BeepTime = 1000;
466	}	451	}
467	// ---------------- Logging ---------------------------------------	452	// ---------------- Logging ---------------------------------------
468	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	453	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
469	}	454	}
470	}	455	}
471		456
472		457
473		458

Subversion Repositories NaviCtrl

(root)/tags/V2.10e/main.c @ 841 - Rev 254 → 255