Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 377 → Rev 378

/trunk/analog.c
77,6 → 77,7
AnalogData.Ch7 = ADC_GetConversionValue(ADC_Channel_7);
}
IDISABLE;
VIC0->VAR = 0xFF; // write any value to VIC0 Vector address register
}
 
void Analog_Init(void)
117,6 → 118,8
ADC_ITConfig(ADC_IT_ECV, ENABLE); // enable end of conversion IRQ
 
VIC_Config(ADC_ITLine, VIC_IRQ, PRIORITY_ADC);
 
 
VIC_ITCmd(ADC_ITLine, ENABLE);
 
UART1_PutString("ok");
/trunk/config.h
34,13 → 34,15
#define PRIORITY_UART2 1 // VIC1.1: uart to FC
 
//<o> VIC1.2: debug uart1 <0-15>
#define PRIORITY_UART1 0 // VIC1.2: debug uart
//#define PRIORITY_UART1 0 // VIC1.2: debug uart
#define PRIORITY_UART1 3 // VIC1.2: debug uart
 
//<o> VIC1.4: i2c to MK3MAG <0-15>
#define PRIORITY_I2C1 4 // VIC1.4: i2c to MK3MAG
 
//<o> VIC1.5: SPI0 <0-15>
#define PRIORITY_SPI0 3 // VIC1.5: timing forced by FC, must be lower than UARTS for flashing FC thrue the NC
//#define PRIORITY_SPI0 3 // VIC1.5: timing forced by FC, must be lower than UARTS for flashing FC thrue the NC
#define PRIORITY_SPI0 0 // VIC1.5: timing forced by FC, must be lower than UARTS for flashing FC thrue the NC
 
//<o> VIC0.5: VIC1.10 switch SD card <0-15>
#define PRIORITY_SDSWITCH 14 // VIC1.10 switch at sd port
/trunk/fat16.c
60,6 → 60,7
#include "sdc.h"
#include "uart1.h"
#include "logging.h"
#include "led.h"
//________________________________________________________________________________________________________________________________________
// Module name: fat16.c
// Compiler used: avr-gcc 3.4.5
1739,8 → 1740,8
return(EOF);
}
}
file->Cache[file->ByteOfCurrSector] = (u8)c; // write databyte into the buffer. The byte will be written to the device at once
 
if(file->Size == file->Position) file->Size++; // a character has been written to the file so the size is incremented only when the character has been added at the end of the file.
file->Position++; // the actual positon within the file.
file->ByteOfCurrSector++; // goto next byte in sector
/trunk/fifo.c
1,5 → 1,6
#include "fifo.h"
#include "main.h"
#include "uart1.h"
 
u8 fifo_init (fifo_t* f, u8* buffer, const u16 size, u16 putvicsource, u16 getvicsource)
{
38,8 → 39,7
 
u8 fifo_get_wait (fifo_t *f, u8 *pdata)
{
while (!f->count && (SD_WatchDog));
 
while (!f->count);
return fifo_get(f, pdata);
}
 
/trunk/gpx.c
134,7 → 134,7
 
if(doc == NULL) return(0);
 
while(doc->state != GPX_DOC_CLOSED && (SD_WatchDog)) // close linestring, placemark and document before closing the file on the memorycard
while(doc->state != GPX_DOC_CLOSED) // close linestring, placemark and document before closing the file on the memorycard
{
switch(doc->state)
{
300,6 → 300,7
switch(part)
{
case 0:
DebugOut.Analog[19]++;
if(GPSData.Position.Latitude < 0) u8_1 = '-';
else u8_1 = '+';
i32_1 = abs(GPSData.Position.Latitude)/10000000L;
467,7 → 468,7
u8 GPX_LoggGPSCoordinates(GPX_Document_t *doc,unsigned char part)
{
u8 retval = 0;
while(doc->state != GPX_DOC_TRACKSEGMENT_OPENED && (SD_WatchDog)) // automatic create document with default filename on the card.
while(doc->state != GPX_DOC_TRACKSEGMENT_OPENED) // automatic create document with default filename on the card.
{
switch(doc->state)
{
/trunk/i2c.c
380,6 → 380,7
}
}
//IDISABLE; // do not enable IRQ nesting for I2C!!!!
VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
}
 
// ----------------------------------------------------------------------------------------
/trunk/logging.c
62,10 → 62,13
#include "gpx.h"
#include "ssc.h"
#include "settings.h"
#include "led.h"
 
#define MIN_SD_INTERVAL 500
 
u8 SD_LoggingError = 0;
 
#define LOG_FLUSH_INTERVAL 20000 // 20s
#define LOG_FLUSH_INTERVAL 2000 // 20s //##
 
typedef enum
{
188,7 → 191,7
do
{ // try to generate a new logfile name
logfilename = GenerateKMLLogFileName();
}while((logfilename != NULL) && fexist_(logfilename) && (SD_WatchDog));
}while((logfilename != NULL) && fexist_(logfilename));
// if logfilename exist
if(logfilename != NULL)
{
304,7 → 307,7
// no init
if(CheckDelay(logtimer) || part)
{
logtimer = SetDelay(LogDelay); // standard interval
if(!part) logtimer = SetDelay(LogDelay); // standard interval
 
if(FC.StatusFlags & FC_STATUS_MOTOR_RUN || part)
{
323,7 → 326,7
do
{ // try to generate a new logfile name
logfilename = GenerateGPXLogFileName();
}while((logfilename != NULL) && fexist_(logfilename) && (SD_WatchDog));
}while((logfilename != NULL) && fexist_(logfilename));
// if logfilename exist
if(logfilename != NULL)
{
372,7 → 375,11
if(CheckDelay(flushtimer))
{
flushtimer = SetDelay(LOG_FLUSH_INTERVAL);
LED_RED_ON;
 
fflush_(logfile.file);
LED_RED_OFF;
 
}
}
}
419,14 → 426,14
void Logging_Init(void)
{
SD_LoggingError = 0;
LogCfg.KML_Interval = 500; //default
LogCfg.KML_Interval = 1000; //default
Settings_GetParamValue(PID_KML_LOGGING, (u16*)&LogCfg.KML_Interval); // overwrite by settings value
if(LogCfg.KML_Interval != 0 && LogCfg.KML_Interval < 500) LogCfg.KML_Interval = 500;
if(LogCfg.KML_Interval != 0 && LogCfg.KML_Interval < MIN_SD_INTERVAL) LogCfg.KML_Interval = MIN_SD_INTERVAL;
 
Logging_KML(0); // initialize
LogCfg.GPX_Interval = 0; //default
Settings_GetParamValue(PID_GPX_LOGGING, (u16*)&LogCfg.GPX_Interval); // overwrite by settings value
if(LogCfg.GPX_Interval != 0 && LogCfg.GPX_Interval < 500) LogCfg.GPX_Interval = 500;
if(LogCfg.GPX_Interval != 0 && LogCfg.GPX_Interval < MIN_SD_INTERVAL) LogCfg.GPX_Interval = MIN_SD_INTERVAL;
Logging_GPX(0); // initialize
}
 
/trunk/main.c
93,9 → 93,10
 
u8 ClearFCStatusFlags = 0;
u8 StopNavigation = 0;
volatile u32 PollingTimeout = 10000;
Param_t Parameter;
volatile FC_t FC;
volatile u32 MainWatchDog = 15000; // stop Navigation if this goes to zero
volatile u32 SPIWatchDog = 15000; // stop Navigation if this goes to zero
volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero
 
s8 ErrorMSG[25];
376,15 → 377,43
// 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 Polling(void)
{
static u8 running = 0;
if(running) return;
running = 1;
LED_GRN_OFF;
// PollingTimeout = 20;
SPI0_UpdateBuffer(); // also calls the GPS-functions
UART0_ProcessRxData(); // GPS process request
UART0_TransmitTxData(); // GPS send answer
UART1_TransmitTxData(); // PC send answer
UART2_TransmitTxData(); // FC send answer
CalcHeadFree();
LED_GRN_ON;
running = 0;
}
 
 
void EXTIT3_IRQHandler(void)
{
IENABLE;
VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
Compass_Update(); // update compass communication
Analog_Update(); // get new ADC values
 
if(!PollingTimeout)
{
PollingTimeout = 3;
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
}
 
//----------------------------------------------------------------------------------------------------
469,17 → 498,14
Debug_OK("START");
UART1_PutString("\r\n");
 
Polling();
for (;;) // the endless main loop
{
MainWatchDog = 4500; // stop communication to FC after this timeout
UART0_ProcessRxData(); // process request
UART1_ProcessRxData(); // process request
UART0_TransmitTxData(); // send answer
UART1_TransmitTxData(); // send answer
UART2_TransmitTxData(); // send answer
SPI0_UpdateBuffer(); // handle new SPI Data
// USB_ProcessRxData(); // process request
// USB_TransmitTxData(); // send answer
Polling();
PollingTimeout = 15;
DebugOut.Analog[17] = CountMilliseconds;
UART1_ProcessRxData(); // PC process request
// UART1_TransmitTxData(); // PC send answer
 
// ---------------- Error Check Timing ----------------------------
if(CheckDelay(TimerCheckError))
497,13 → 523,16
if(StopNavigation && (Parameter.NaviGpsModeControl >= 50) && (Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) BeepTime = 1000;
}
// ---------------- Logging ---------------------------------------
 
 
if(SD_WatchDog)
{
SD_WatchDog = 4000;
SD_WatchDog = 10000;
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");
}
 
/*
// test
if(CheckDelay(ftimer))
/trunk/main.h
197,7 → 197,8
extern u8 ErrorCode;
extern u8 StopNavigation;
extern u8 ErrorGpsFixLost;
extern volatile u32 MainWatchDog; // stop Navigation if this goes to zero
extern volatile u32 SPIWatchDog; // stop Navigation if this goes to zero
extern volatile u32 SD_WatchDog; // stop Logging if this goes to zero
extern volatile u32 PollingTimeout;
 
#endif // _MAIN_H
/trunk/mkprotocol.c
58,6 → 58,7
#include "mkprotocol.h"
#include "ramfunc.h"
#include "usb.h"
#include "uart1.h"
 
/**************************************************************/
/* Create serial output frame */
235,9 → 236,9
{
u8 a,b,c,d;
u8 x,y,z;
u8 ptrIn = 3; // start with first data byte in rx buffer
u8 ptrOut = 3;
u8 len = pRxBuff->DataBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
u16 ptrIn = 3; // start with first data byte in rx buffer
u16 ptrOut = 3;
u16 len = pRxBuff->DataBytes - 6; // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
while(len)
{
a = pRxBuff->pData[ptrIn++] - '=';
/trunk/sdc.c
175,7 → 175,7
do
{
rsp = SSC_GetChar();
if(CheckDelay(timestamp)) break;
if(CheckDelay(timestamp)) break;
}while(rsp != 0xFF && (SD_WatchDog)); // wait while card is busy (data out low)
return(rsp);
}
199,7 → 199,7
 
SSC_ClearRxFifo(); // clear the rx fifo
SSC_Enable(); // enable chipselect.
SDC_WaitForBusy(500); // wait 500ms until card is busy
SDC_WaitForBusy(600); // wait 500ms until card is busy
SSC_ClearRxFifo(); // clear the rx fifo
SSC_GetChar(); // dummy to sync
 
/trunk/spi_slave.c
84,13 → 84,13
 
// tx packet buffer
#define SPI_TXBUFFER_LEN (2 + sizeof(ToFlightCtrl)) // 2 bytes at start are for synchronization
volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN];
volatile u8 SPI_TxBuffer[SPI_TXBUFFER_LEN + 10];
volatile u8 SPI_TxBufferIndex = 0;
u8 *Ptr_TxChksum = NULL ; // pointer to checksum in TxBuffer
 
// rx packet buffer
#define SPI_RXBUFFER_LEN sizeof(FromFlightCtrl)
volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN];
volatile u8 SPI_RxBuffer[SPI_RXBUFFER_LEN+10];
volatile u8 SPI_RxBufferIndex = 0;
volatile u8 SPI_RxBuffer_Request = 0;
#define SPI_COMMAND_INDEX 0
130,25 → 130,24
#define SPI_SYNC2 1
#define SPI_DATA 2
static u8 SPI_State = SPI_SYNC1;
//IENABLE;
 
IENABLE;
 
// clear pending bits
SSP_ClearITPendingBit(SSP0, SSP_IT_RxTimeOut);
SSP_ClearITPendingBit(SSP0, SSP_IT_RxFifo);
 
// while RxFIFO not empty
while (SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET && (SD_WatchDog))
while(SSP_GetFlagStatus(SSP0, SSP_FLAG_RxFifoNotEmpty) == SET)
{
rxdata = SSP0->DR; // catch the received byte
// Fill TxFIFO while its not full or end of packet is reached
while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET && (SD_WatchDog))
while (SSP_GetFlagStatus(SSP0, SSP_FLAG_TxFifoNotFull) == SET)
{
if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) // still data to send ?
{
SSP0->DR = SPI_TxBuffer[SPI_TxBufferIndex]; // send a byte
*Ptr_TxChksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum
if(MainWatchDog == 0) *Ptr_TxChksum += 1; // disturbe this packet to stop the communication!
if(SPIWatchDog == 0) *Ptr_TxChksum += 1; // disturbe this packet to stop the communication!
SPI_TxBufferIndex++; // pointer to next byte
}
else // end of packet is reached reset and copy data to tx buffer
184,7 → 183,7
break;
case SPI_DATA:
SPI_RxBuffer[SPI_RxBufferIndex++]= rxdata; // copy databyte to rx buffer
if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached
if(SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) // end of packet is reached
{
if (rxdata == rxchksum) // verify checksum byte
{
194,6 → 193,10
memcpy((u8 *) &FromFlightCtrl, (u8 *) SPI_RxBuffer, sizeof(FromFlightCtrl));
SPI_RxBuffer_Request = 1;
}
else
{
DebugOut.Analog[16]++;
}
// reset timeout counter on good packet
SPI0_Timeout = SetDelay(SPI0_TIMEOUT);
DebugOut.Analog[13]++;
215,7 → 218,8
}
}
 
IDISABLE;
// IDISABLE;
VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
}
 
//--------------------------------------------------------------
259,7 → 263,6
 
SSP_Init(SSP0, &SSP_InitStructure);
SSP_ITConfig(SSP0, SSP_IT_RxFifo | SSP_IT_RxTimeOut, ENABLE);
 
SSP_Cmd(SSP0, ENABLE);
// initialize the syncbytes in the tx buffer
SPI_TxBuffer[0] = SPI_TXSYNCBYTE1;
288,16 → 291,16
static u8 CompassCalState = 0;
s16 tmp;
s32 i1,i2;
 
if (SPI_RxBuffer_Request)
SPIWatchDog = 3500; // stop communication to FC after this timeout
if(SPI_RxBuffer_Request)
{
// avoid sending data via SPI during the update of the ToFlightCtrl structure
VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
ToFlightCtrl.CompassHeading = Compass_Heading;
 
//ToFlightCtrl.CompassHeading += 360 + ((s32) Poti8 - 128);
//ToFlightCtrl.CompassHeading %= 360;
 
DebugOut.Analog[10] = ToFlightCtrl.CompassHeading;
if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360;
// ToFlightCtrl.MagVecX = MagVector.X;
312,7 → 315,6
switch (ToFlightCtrl.Command)
{
case SPI_NCCMD_KALMAN: // wird am häufigsten betätigt
CalcHeadFree();
ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift;
537,7 → 539,6
// 5 = 10,11
}
VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt
 
switch(FromFlightCtrl.Command)
{
case SPI_FCCMD_USER:
661,8 → 662,12
case SPI_FCCMD_MISC:
if(CompassCalState != FromFlightCtrl.Param.Byte[0])
{ // put only new CompassCalState into queue to send via I2C
CompassCalState = FromFlightCtrl.Param.Byte[0];
Compass_SetCalState(CompassCalState);
if(FromFlightCtrl.Param.Byte[0] == CompassCalState+1)
{
CompassCalState = FromFlightCtrl.Param.Byte[0];
Compass_SetCalState(CompassCalState);
}
else CompassCalState = 0;
}
Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1];
NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
717,29 → 722,33
/*
//+++++++++++++++++++++++++++++++++++++++++++++++++++
*/
 
/*
if(Parameter.User8 < 100) FC.StatusFlags = 0;
else
if(Parameter.User8 < 150) FC.StatusFlags = FC_STATUS_START;
else FC.StatusFlags = FC_STATUS_FLY | FC_STATUS_MOTOR_RUN;
BL_MinOfMaxPWM = 255;
*/
DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
DebugOut.Analog[2] = FromFlightCtrl.AccNick;
DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
Data3D.AngleNick = FromFlightCtrl.AngleNick; // in 0.1 deg
Data3D.AngleRoll = FromFlightCtrl.AngleRoll; // in 0.1 deg
Data3D.Heading = FromFlightCtrl.GyroHeading; // in 0.1 deg
// every time we got new data from the FC via SPI call the navigation routine
// and update GPSStick that are returned to FC
SPI_RxBuffer_Request = 0;
GPS_Navigation(&GPSData, &(ToFlightCtrl.GPSStick));
ClearFCStatusFlags = 1;
 
if(counter)
{
counter--; // count down to enable servo
if(!counter) TIMER2_Init(); // enable Servo Output
}
 
SPI_RxBuffer_Request = 0;
timeout = SetDelay(80); // 80 ms, new data are send every 20 ms
 
DebugOut.Analog[0] = FromFlightCtrl.AngleNick;
DebugOut.Analog[1] = FromFlightCtrl.AngleRoll;
DebugOut.Analog[2] = FromFlightCtrl.AccNick;
DebugOut.Analog[3] = FromFlightCtrl.AccRoll;
DebugOut.Analog[11] = FromFlightCtrl.GyroHeading/10;// in deg
Data3D.AngleNick = FromFlightCtrl.AngleNick; // in 0.1 deg
Data3D.AngleRoll = FromFlightCtrl.AngleRoll; // in 0.1 deg
Data3D.Heading = FromFlightCtrl.GyroHeading; // in 0.1 deg
} // EOF if(SPI_RxBuffer_Request)
else // no new SPI data
{
/trunk/ssc.c
123,7 → 123,7
{
GPIO_InitTypeDef GPIO_InitStructure;
SSP_InitTypeDef SSP_InitStructure;
WIU_InitTypeDef WIU_InitStructure;
// WIU_InitTypeDef WIU_InitStructure;
// enable APB clock for SPI1
SCU_APBPeriphClockConfig(__SSP1 ,ENABLE);
// configure P5.4 -> SD-CS as an output pin
161,7 → 161,7
// bit rate is BRCLK/SSP_ClockPrescaler/(1+SSP_ClockRate))
// With MSCLK = 48MHz/2 = BRCLK we get for the SPICLK = 24Mhz / 8 / (1+5) = 500 kHz
SSP_InitStructure.SSP_ClockRate = 5; //5
SSP_InitStructure.SSP_ClockPrescaler = 8;
SSP_InitStructure.SSP_ClockPrescaler = 4; //8
SSP_Init(SSP1, &SSP_InitStructure);
SSC_Disable();
SSP_Cmd(SSP1, ENABLE);
249,9 → 249,9
u8 SSC_GetChar (void)
{
u8 Byte = 0;
while(SSP_GetFlagStatus(SSP1, SSP_FLAG_TxFifoNotFull) != SET && (SD_WatchDog)); // wait for space in the tx fifo
while(SSP_GetFlagStatus(SSP1, SSP_FLAG_TxFifoNotFull) != SET); // wait for space in the tx fifo
SSP_SendData(SSP1, 0xFF);// send dymmy byte (0xFF) as master to receive a byte from the slave
while(SSP_GetFlagStatus(SSP1, SSP_FLAG_TxFifoEmpty) != SET && (SD_WatchDog)); // wait for the byte to be sent
while(SSP_GetFlagStatus(SSP1, SSP_FLAG_TxFifoEmpty) != SET); // wait for the byte to be sent
Byte = SSP_ReceiveData(SSP1); // read the byte transmitted from the slave
return (Byte);
}
260,9 → 260,9
void SSC_ClearRxFifo (void)
{
// wait that the tx fifo is empty
while(SSP_GetFlagStatus(SSP1, SSP_FLAG_TxFifoEmpty) != SET && (SD_WatchDog));
while(SSP_GetFlagStatus(SSP1, SSP_FLAG_TxFifoEmpty) != SET);
// then empty the rx fifo by reading all the bytes that are available
while(SSP_GetFlagStatus(SSP1, SSP_FLAG_RxFifoNotEmpty) == SET && (SD_WatchDog)) SSP_ReceiveData(SSP1);
while(SSP_GetFlagStatus(SSP1, SSP_FLAG_RxFifoNotEmpty) == SET) SSP_ReceiveData(SSP1);
}
//________________________________________________________________________________________________________________________________________
// Function: SSC_PutChar(u8 Byte);
276,7 → 276,7
void SSC_PutChar (u8 Byte)
{
// wait for some space in the tx fifo
while(SSP_GetFlagStatus(SSP1, SSP_FLAG_TxFifoNotFull) != SET && (SD_WatchDog));
while(SSP_GetFlagStatus(SSP1, SSP_FLAG_TxFifoNotFull) != SET);
// put the byte to send in the tx fifo
SSP_SendData(SSP1, Byte);
}
/trunk/timer1.c
58,6 → 58,7
#include "uart1.h"
#include "config.h"
#include "main.h"
#include "led.h"
 
u32 CountMilliseconds;
DateTime_t SystemTime;
65,8 → 66,7
//----------------------------------------------------------------------------------------------------
void TIM1_IRQHandler(void)
{
IENABLE;
 
// IENABLE;
if(TIM_GetFlagStatus(TIM1, TIM_FLAG_OC1) == SET)
{
TIM_ClearFlag(TIM1, TIM_FLAG_OC1); // clear irq pending bit
73,13 → 73,16
TIM1->OC1R += 200; // Timerfreq is 200kHz, generate an interrupt every 1ms
CountMilliseconds++;
if(SD_WatchDog) SD_WatchDog--;
if(MainWatchDog) MainWatchDog--;
if(SPIWatchDog) SPIWatchDog--;
if(PollingTimeout) PollingTimeout--;
 
// generate SW Interrupt to make a regular timing
// independent from the mainloop at the lowest IRQ priority
VIC_SWITCmd(EXTIT3_ITLine, ENABLE);
}
 
IDISABLE;
// IDISABLE;
VIC0->VAR = 0xFF; // write any value to VIC0 Vector address register
}
 
//----------------------------------------------------------------------------------------------------
/trunk/timer2.c
58,6 → 58,7
#include "uart1.h"
#include "spi_slave.h"
#include "config.h"
#include "led.h"
 
#define CR1_OLVL1_MASK 0x0100
#define CR1_OLVL2_MASK 0x0200
173,8 → 174,8
}
TIM2->OC2R += pulselen;
}
 
IDISABLE;
VIC0->VAR = 0xFF; // write any value to VIC0 Vector address register
}
 
//----------------------------------------------------------------------------------------------------
182,6 → 183,7
//----------------------------------------------------------------------------------------------------
void TIMER2_Init(void)
{
 
GPIO_InitTypeDef GPIO_InitStructure;
TIM_InitTypeDef TIM_InitStructure;
 
223,7 → 225,7
TIM_ITConfig(TIM2, TIM_IT_OC1|TIM_IT_OC2, ENABLE); // enable interrupts for the OC 1 & 2
 
VIC_Config(TIM2_ITLine, VIC_IRQ, PRIORITY_TIMER2);
VIC_ITCmd(TIM2_ITLine, ENABLE);
//## VIC_ITCmd(TIM2_ITLine, ENABLE);
 
TIM2->OC1R = 10;
TIM2->OC2R = 20;
/trunk/uart0.c
289,7 → 289,7
{
case UART0_MKGPS:
UBX_RxParser(c); // if connected to GPS forward byte to ubx parser
MKProtocol_CollectSerialFrame(&UART0_rx_buffer, c); // ckeck for MK-Frames also
// MKProtocol_CollectSerialFrame(&UART0_rx_buffer, c); // ckeck for MK-Frames also
break;
case UART0_MK3MAG:
// ignore any byte send from MK3MAG
302,6 → 302,8
} // eof while
} // eof UART0 is not the DebugUART
} // eof receive irq or receive timeout irq
 
VIC1->VAR = 0xFF; // write any value to VIC0 Vector address register
}
 
/**************************************************************/
366,8 → 368,6
{
u8 tmp_tx;
 
IENABLE;
 
if(DebugUART == UART0) return; // no data output if debug uart is rederected to UART0
// if something has to be send and the txd fifo is not full
if((UART0_tx_buffer.Locked == TRUE) && (UART_GetFlagStatus(UART0, UART_FLAG_TxFIFOFull) == RESET))
381,7 → 381,6
}
}
 
IDISABLE;
}
 
 
/trunk/uart1.c
77,6 → 77,7
#include "debug.h"
#include "spi_slave.h"
#include "ftphelper.h"
#include "led.h"
 
#define FALSE 0
#define TRUE 1
114,12 → 115,12
#endif
 
// the primary rx fifo
#define UART1_RX_FIFO_LEN 512
#define UART1_RX_FIFO_LEN 1024
u8 UART1_rxfifobuffer[UART1_RX_FIFO_LEN];
fifo_t UART1_rx_fifo;
 
// the rx buffer
#define UART1_RX_BUFFER_LEN 150
#define UART1_RX_BUFFER_LEN 1024
u8 UART1_rbuffer[UART1_RX_BUFFER_LEN];
Buffer_t UART1_rx_buffer;
 
352,6 → 353,7
}
 
IDISABLE;
VIC1->VAR = 0xFF; // write any value to VIC1 Vector address register
}
 
/**************************************************************/
361,7 → 363,6
{
// return on forwarding uart or unlocked rx buffer
if(DebugUART != UART1) return;
 
u8 c;
// if rx buffer is not locked
if(UART1_rx_buffer.Locked == FALSE)
664,6 → 665,7
}
}
 
 
/**************************************************************/
/* Send the answers to incomming commands at the debug uart */
/**************************************************************/
678,6 → 680,7
UART1_NaviData_Interval = 0;
UART1_Data3D_Interval = 0;
UART1_Display_Interval = 0;
UART1_MotorData_Interval = 0;
}
 
UART1_Transmit(); // output pending bytes in tx buffer
727,7 → 730,7
else if(( ((UART1_NaviData_Interval > 0) && CheckDelay(UART1_NaviData_Timer) ) || UART1_Request_NaviData) && (UART1_tx_buffer.Locked == FALSE))
{
NaviData.Errorcode = ErrorCode;
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));
MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS, 1 , (u8 *)&NaviData, sizeof(NaviData));
UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval);
UART1_Request_NaviData = FALSE;
LastTransmittedFCStatusFlags2 = NaviData.FCStatusFlags2;
/trunk/uart2.c
204,6 → 204,7
} // eof receive irq or receive timeout irq
IDISABLE;
VIC1->VAR = 0xFF; // write any value to VIC1 Vector address register
}
 
/**************************************************************/
/trunk/usb.c
154,7 → 154,7
Buffer_Init(&USB_rx_buffer, USB_rbuffer, USB_RX_BUFFER_LEN);
 
VIC_Config(USBLP_ITLine, VIC_IRQ, PRIORITY_USB);
VIC_ITCmd(USBLP_ITLine, ENABLE);
//## VIC_ITCmd(USBLP_ITLine, ENABLE);
 
USB_Init();