Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 69 → Rev 70

/tags/V0.14a/main.c
0,0 → 1,432
/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!! */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2008 Ingo Busker, Holger Buss
// + Nur für den privaten Gebrauch
// + 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 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 this software (or part of it) to systems (other than 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 "timer.h"
#include "spi_slave.h"
#include "fat16.h"
#include "usb.h"
#include "sdc.h"
#include "kml.h"
#include "main.h"
 
 
typedef enum
{
LOGFILE_IDLE,
LOGFILE_CLOSED,
LOGFILE_OPENED,
LOGFILE_ERROR,
} logfilestate_t;
 
u32 TimerCompassUpdate;
u32 TimerKmlAddPointDelay;
u32 TimerCheckError;
u32 ErrorCode = 0;
u16 BeepTime;
 
u8 ClearMKFlags = 0;
u8 StopNavigation = 0;
Param_t Parameter;
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);
SerialPutString(msg);
}
 
 
//----------------------------------------------------------------------------------------------------
s8* GenerateGPSLogFileName(void)
{
static u16 filenum = 0; // file name counter
static s8 filename[30];
static DateTime_t LastTime = {0,0,0,0,0,0,0,0};
 
if(SystemTime.Valid)
{
// if the day has been changed
if((LastTime.Year != SystemTime.Year) || (LastTime.Month != SystemTime.Month) || (LastTime.Day != SystemTime.Day))
{
LastTime.Year = SystemTime.Year;
LastTime.Month = SystemTime.Month;
LastTime.Day = SystemTime.Day;
LastTime.Valid = 1;
filenum = 0; // reset file counter
}
sprintf(filename, "LOG/%04i%02i%02i/GPS%05i.KML", SystemTime.Year, SystemTime.Month, SystemTime.Day, filenum);
filenum++;
return filename;
}
else return NULL;
}
 
 
//----------------------------------------------------------------------------------------------------
void CheckErrors(void)
{
if(!CheckSPIOkay)
{
LED_RED_ON;
sprintf(ErrorMSG,"no FC communication ");
ErrorCode = 3;
StopNavigation = 1;
}
else if(!CheckI2COkay)
{
LED_RED_ON;
sprintf(ErrorMSG,"no MK3Mag communication ");
ErrorCode = 4;
StopNavigation = 1;
}
else if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
{
LED_RED_ON;
sprintf(ErrorMSG,"FC not compatible ");
ErrorCode = 1;
StopNavigation = 1;
}
else if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
{
sprintf(ErrorMSG,"MK3Mag not compatible ");
LED_RED_ON;
ErrorCode = 2;
StopNavigation = 1;
}
else if(!CheckGPSOkay)
{
LED_RED_ON;
sprintf(ErrorMSG,"no GPS communication ");
ErrorCode = 5;
StopNavigation = 1;
}
else if(I2C_Heading.Heading < 0)
{
LED_RED_ON;
sprintf(ErrorMSG,"bad compass value ");
ErrorCode = 6;
StopNavigation = 1;
}
else if(FC.RC_Quality < 100)
{
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;
}
CheckSPIOkay = 0;
CheckGPSOkay = 0;
CheckI2COkay = 0;
}
 
 
 
//----------------------------------------------------------------------------------------------------
int main(void)
{
logfilestate_t logfilestate = LOGFILE_IDLE;
KML_Document_t gpslogfile;
s8* gpslogfilename = NULL;
 
/* Configure the system clocks */
SCU_Config();
 
KML_DocumentInit(&gpslogfile); // Initialize the new kml-document for further use.
 
/* 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.
// initialize the interrupt handler
Interrupt_Init();
// initialize timer 1
TIMER1_Init();
// initialize the LEDs (needs Timer 1)
Led_Init();
// initialize the debug UART1
UART1_Init();
SerialPutString("\r\n---------------------------------------------");
 
// initialize UART2 to FLIGHTCTRL
UART2_Init();
// initialize UART0 (to MKGPS or MK3MAG)
UART0_Init();
// initialize usb
USB_ConfigInit();
// initialize SPI0 to FC
SPI0_Init();
// initialize i2c bus to MK3MAG (needs Timer 1)
I2C1_Init();
// initialize the gps position controller (needs Timer 1)
GPS_Init();
// initialize fat16 partition on sd card (needs Timer 1)
Fat16_Init();
 
 
TimerCompassUpdate = SetDelay(5);
TimerKmlAddPointDelay = SetDelay(250);
TimerCheckError = SetDelay(3000);
 
SerialPutString("\r\n---------------------------------------------");
SerialPutString("\n\r Version information:");
GetNaviCtrlVersion();
SPI0_GetFlightCtrlVersion();
if(FC_Version.Compatible != FC_SPI_COMPATIBLE)
{
//sprintf(ErrorMSG,"\r\n %d <-> %d", FlightCtrlVersion.NaviCompatible, FC_SPI_COMPATIBLE);
//SerialPutString(ErrorMSG);
SerialPutString("\n\r Flight-Ctrl not compatible");
LED_RED_ON;
}
I2C1_GetMK3MagVersion();
if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
{
SerialPutString("\n\r MK3Mag not compatible");
LED_RED_ON;
}
 
/*
ReadSetting(1);
*/
SerialPutString("\r\n---------------------------------------------\r\n\r\n");
 
for (;;)
{
if(rxd_buffer_locked) // new incomming command
{
UART1_ProcessRxData(); // process request
}
UART1_TransmitTxData(); // send answer
 
SPI0_UpdateBuffer();
UART0_Transmit(); // empty txd buffer
UART1_Transmit(); // empty txd buffer
 
// ------------------------- I2C Timing --------------------------------
if(I2C_State != I2C_OFF)
{
if (CheckDelay(TimerCompassUpdate))
{
// check for hanging I2C bus
if(CheckDelay(I2C1_Timeout))
{ // reset I2C
SerialPutString("\n\rCommunication-Error: MK3Mag\n\r");
I2C1_Deinit();
I2C1_Init();
}
else
{ // check for incomming compass calibration request
// update CalByte from spi input queue
fifo_get(&CompassCalcStateFiFo, (u8 *)&(I2C_WriteCal.CalByte));
// send new calstate
if(I2C_ReadCal.CalByte != I2C_WriteCal.CalByte)
{
I2C1_SendCommand(I2C_CMD_WRITE_CAL);
}
else // request current heading
{
I2C1_SendCommand(I2C_CMD_READ_HEADING);
}
TimerCompassUpdate = SetDelay(25); // every 25 ms
}
 
}
}
 
// ---------------- Error Check Timing ----------------------------
if(CheckDelay(TimerCheckError))
{
if(!CheckSPIOkay) GPS_Navigation(); // process the GPS data even if the FC is not connected
CheckErrors();
TimerCheckError = SetDelay(1000);
Request_SendFollowMe = TRUE;
}
// ---------------- KML Timing ------------------------------------
if(CheckDelay(TimerKmlAddPointDelay))
{
TimerKmlAddPointDelay = SetDelay(500); // every 500 ms
 
if(FC.MKFlags & MKFLAG_FLY) // model is flying
{
switch(logfilestate)
{
case LOGFILE_IDLE:
case LOGFILE_CLOSED:
// find unused logfile name
do
{ // try to generate a new logfile name
gpslogfilename = GenerateGPSLogFileName();
}while((gpslogfilename != NULL) && fexist_(gpslogfilename));
// if logfilename exist
if(gpslogfilename != NULL)
{
// try to create the log file
if(KML_DocumentOpen(gpslogfilename, &gpslogfile))
{
logfilestate = LOGFILE_OPENED; // goto next step
SerialPutString("\r\nOpening kml-file: ");
SerialPutString(gpslogfilename);
SerialPutString("\r\n");
}
else
{
logfilestate = LOGFILE_ERROR;
SerialPutString("\r\nError opening kml-file: ");
SerialPutString(gpslogfilename);
SerialPutString("\r\n");
TimerKmlAddPointDelay = SetDelay(10); // try again in open logfile in 10 mili sec
}
}
// else retry in next loop
break;
 
 
case LOGFILE_OPENED:
// append new gps log data
if((GPSData.Status != INVALID) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.SatFix == SATFIX_3D))
{
if(!KML_LoggGPSCoordinates(&(GPSData.Position), &gpslogfile))
{ // error logging data
SerialPutString("\r\nError Logging GPS data\r\n");
KML_DocumentClose(&gpslogfile);
logfilestate = LOGFILE_ERROR;
}
}
break;
 
case LOGFILE_ERROR:
// try to reinitialize the fat16 filesystem
if(0 == Fat16_Init())
{
logfilestate = LOGFILE_IDLE;
TimerKmlAddPointDelay = SetDelay(10); // try again in open logfile in 10 mili sec
}
else
{
logfilestate = LOGFILE_ERROR;
TimerKmlAddPointDelay = SetDelay(5000); // try again in 5 sec
}
break;
 
default:
break;
}
} // EOF model is flying
else // model is not flying
{ // close log file if opened
if(logfilestate == LOGFILE_OPENED)
{
if(KML_DocumentClose(&gpslogfile))
{
SerialPutString("\r\nClosing kml-file\r\n");
logfilestate = LOGFILE_CLOSED;
}
else
{
logfilestate = LOGFILE_ERROR;
}
}
} //EOF model is not flying
}
}
}