Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 515 → Rev 516

/trunk/kml.c
60,7 → 60,9
#include "kml_header.h"
#include "uart1.h"
#include "logging.h"
#include "timer1.h"
 
 
//________________________________________________________________________________________________________________________________________
// Module name: kml.c
// Compiler used: avr-gcc 3.4.5
119,6 → 121,7
{
 
u8 retvalue = 0;
s8 string[50];
 
if(doc == NULL) return(0);
 
129,7 → 132,10
{
retvalue = 1; // the document could be created on the drive.
doc->state = KML_DOC_OPENED; // change document state to opened. At next a placemark has to be opened.
fwrite_((void*)KML_DOCUMENT_HEADER, sizeof(KML_DOCUMENT_HEADER)-1,1,doc->file);// write the KML-header to the document.
fwrite_((void*)KML_DOCUMENT_HEADER1, sizeof(KML_DOCUMENT_HEADER1)-1,1,doc->file);// write the KML-header to the document.
sprintf(string, "%4i.%02i.%02i Nr:%i (%02i%02i%02i%02i)", SystemTime.Year, SystemTime.Month, SystemTime.Day, KML_Filenum, SystemTime.Year%100, SystemTime.Month, SystemTime.Day, KML_Filenum);
fputs_(string, doc->file);
fwrite_((void*)KML_DOCUMENT_HEADER2, sizeof(KML_DOCUMENT_HEADER2)-1,1,doc->file);// write the KML-header to the document.
}
 
return(retvalue);
/trunk/kml_header.h
3,13 → 3,18
// Definition of KML header and footer elements for documents, placemarks and linestrings
//
//________________________________________________________________________________________________________________________________________
const s8 KML_DOCUMENT_HEADER[] =
const s8 KML_DOCUMENT_HEADER1[] =
{
"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\r\n"
"<kml xmlns=\"http://earth.google.com/kml/2.2\">\r\n"
"<Document>\r\n"
"<name>Mikrokopter GPS logging</name>\r\n"
"\r\n"
"<name>MikroKopter "
};
 
const s8 KML_DOCUMENT_HEADER2[] =
{
"</name>\r\n"
//"\r\n"
"<Style id=\"MK_gps-style\">\r\n"
"<LineStyle>\r\n"
"<color>ff0000ff</color>\r\n"
18,6 → 23,7
"</Style>\r\n"
};
 
 
//________________________________________________________________________________________________________________________________________
//
// footer of an KML- file.
/trunk/logging.c
88,8 → 88,8
 
LogCfg_t LogCfg = {500 , 1000};
u32 Logged_GPX_Counter = 0, Logged_KML_Counter = 0;
u16 KML_Filenum = 0;
 
 
//----------------------------------------------------------------------------------------------------
s8* GenerateKMLLogFileName(void)
{
111,6 → 111,7
if(filenum < 100) sprintf(filename, "/LOG/%04i%02i%02i/KML/%02i%02i%02i%02i.KML", SystemTime.Year, SystemTime.Month, SystemTime.Day, SystemTime.Year % 100, SystemTime.Month, SystemTime.Day, filenum);
else sprintf(filename, "/LOG/%04i%02i%02i/KML/GPS%05i.KML", SystemTime.Year, SystemTime.Month, SystemTime.Day, filenum);
filenum++;
KML_Filenum = filenum;
return filename;
}
else return NULL;
137,6 → 138,7
if(filenum < 100) sprintf(filename, "/LOG/%04i%02i%02i/GPX/%02i%02i%02i%02i.GPX", SystemTime.Year, SystemTime.Month, SystemTime.Day, SystemTime.Year % 100, SystemTime.Month, SystemTime.Day, filenum);
else sprintf(filename, "/LOG/%04i%02i%02i/GPX/GPS%05i.GPX", SystemTime.Year, SystemTime.Month, SystemTime.Day, filenum);
filenum++;
KML_Filenum = filenum;
return filename;
}
else return NULL;
/trunk/logging.h
13,6 → 13,6
} LogCfg_t;
 
extern LogCfg_t LogCfg;
extern u16 KML_Filenum;
 
 
#endif //_LOGGING_H
/trunk/main.c
104,7 → 104,7
volatile u32 SD_WatchDog = 15000; // stop Logging if this goes to zero
u32 CountGpsProcessedIn5Sec = 0,CountNewGpsDataIn5Sec = 0, FreqGpsProcessedIn5Sec = 0, FreqNewGpsDataIn5Sec = 0;
u8 NewWPL_Name = 0;
 
u32 MaxRadius_in_m = 0;
s8 ErrorMSG[25];
 
//----------------------------------------------------------------------------------------------------
143,7 → 143,7
 
void CheckErrors(void)
{
static s32 no_error_delay = 0;
static s32 no_error_delay = 0;
s32 newErrorCode = 0;
UART_VersionInfo.HardwareError[0] = 0;
 
278,7 → 278,7
else if(CheckDelay(UBX_Timeout) && Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)
{
LED_RED_ON;
// if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected ");
// if(!(Parameter.GlobalConfig & FC_CFG_GPS_AKTIV)) sprintf(ErrorMSG,"GPS disconnected ");
// else
{
sprintf(ErrorMSG,"no GPS communication");
348,11 → 348,11
else if(BL_MinOfMaxPWM < 30 && !ErrorCode)
{
u16 i;
for(i = 0; i < 12; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break;
for(i = 0; i < 12; i++) if(Motor[i].MaxPWM == BL_MinOfMaxPWM) break;
LED_RED_ON;
sprintf(ErrorMSG,"ERR:BL%2d Test:%2d ",i+1,BL_MinOfMaxPWM);
newErrorCode = 32;
DebugOut.StatusRed |= AMPEL_BL;
newErrorCode = 32;
DebugOut.StatusRed |= AMPEL_BL;
}
else if(BL_MinOfMaxPWM < 248 && (FC.StatusFlags & FC_STATUS_FLY) && !ErrorCode)
{
390,7 → 390,7
newErrorCode = 29;
DebugOut.StatusRed |= AMPEL_NC;
}
else if(Parameter.GlobalConfig3 & CFG3_NO_GPSFIX_NO_START && !(NCFlags & NC_FLAG_GPS_OK) && ((FC.StatusFlags & (FC_STATUS_START | FC_STATUS_MOTOR_RUN)) || (FC.StickGas < -50 && FC.StickYaw < -50)))
else if(Parameter.GlobalConfig3 & CFG3_NO_GPSFIX_NO_START && !(NCFlags & NC_FLAG_GPS_OK) && ((FC.StatusFlags & (FC_STATUS_START | FC_STATUS_MOTOR_RUN)) || (FC.StickGas < -50 && FC.StickYaw < -50)))
{
LED_RED_ON;
sprintf(ErrorMSG,"No GPS Fix ");
408,17 → 408,17
}
}
 
if(newErrorCode)
{
if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running
ErrorCode = newErrorCode;
}
FC.Error[0] = 0;
FC.Error[1] = 0;
FC.Error[2] = 0;
FC.Error[3] = 0;
FC.Error[4] = 0;
ErrorGpsFixLost = 0;
if(newErrorCode)
{
if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) no_error_delay = 8; // delay the errors if the motors are running
ErrorCode = newErrorCode;
}
FC.Error[0] = 0;
FC.Error[1] = 0;
FC.Error[2] = 0;
FC.Error[3] = 0;
FC.Error[4] = 0;
ErrorGpsFixLost = 0;
}
 
 
428,7 → 428,7
static u8 running = 0, oldFcFlags = 0, count5sec;
static u32 old_ms = 0;
 
if(running) {/*DebugOut.Analog[18]++;*/ return(1);};
if(running) {/*DebugOut.Analog[]++;*/ return(1);};
running = 1;
 
if(CountMilliseconds != old_ms) // 1 ms
486,11 → 486,11
IENABLE;
VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
 
if(PollingTimeout == 0)
{
PollingTimeout = 5;
//if(Polling() == 0) DebugOut.Analog[16]++;
Polling();
if(PollingTimeout == 0)
{
PollingTimeout = 5;
//if(Polling() == 0) DebugOut.Analog[]++;
Polling();
}
 
VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
555,7 → 555,7
DebugOut.StatusRed = 0x00;
UART1_PutString("\r\n++++++++++++++++++++++++++++++++++++++++++");
 
Compass_Init();
Compass_Init();
 
GPS_Init();
 
575,9 → 575,9
#endif
// ---------- Prepare the isr driven
// set to absolute lowest priority
VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);
VIC_Config(EXTIT3_ITLine, VIC_IRQ, PRIORITY_SW);
// enable interrupts
VIC_ITCmd(EXTIT3_ITLine, ENABLE);
VIC_ITCmd(EXTIT3_ITLine, ENABLE);
 
Debug_OK("START");
UART1_PutString("\r\n");
584,7 → 584,7
fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
LED_GRN_ON;
LED_RED_OFF;
Settings_GetParamValue(PID_SEND_NMEA, &NMEA_Interval);
Settings_GetParamValue(PID_SEND_NMEA, &NMEA_Interval);
UART1_PutString("\r\n");
CompassValueErrorCount = 0;
I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000);
594,21 → 594,21
Polling();
// ++++++++++++++++++++++++++++++++++++++++++++++
if(FromFC_LoadWP_List)
{
WPL_Store.Index = (FromFC_LoadWP_List & ~0x80);
if(WPL_Store.Index <= ToFC_MaxWpListIndex)
{
{
WPL_Store.Index = (FromFC_LoadWP_List & ~0x80);
if(WPL_Store.Index <= ToFC_MaxWpListIndex)
{
if(PointList_ReadFromFile(&WPL_Store) == WPL_OK)
{
if(FromFC_LoadWP_List & 0x80)// -> load relative
{
{
if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance > 7*10)
{ // take actual position
if(!PointList_Move(1,&(GPSData.Position),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position
}
else
{ // take actual position
if(!PointList_Move(1,&(GPSData.Position),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position
}
else
{ // take last target position
if(!PointList_Move(1, &(NaviData.TargetPosition),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position
if(!PointList_Move(1, &(NaviData.TargetPosition),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position
}
}
if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
622,13 → 622,15
if(FromFC_Save_SinglePoint)
{
WPL_Store.Index = FromFC_Save_SinglePoint;
if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) BeepTime = 150;
if(WPL_Store.Index <= ToFC_MaxWpListIndex) if(PointList_SaveSinglePoint(&WPL_Store) == WPL_OK) BeepTime = 150;
FromFC_Save_SinglePoint = 0;
}
// ++++++++++++++++++++++++++++++++++++++++++++++
if(FromFC_Load_SinglePoint)
{
WPL_Store.Index = FromFC_Load_SinglePoint;
WPL_Store.Index = FromFC_Load_SinglePoint;
if(WPL_Store.Index <= ToFC_MaxWpListIndex)
{
if(PointList_LoadSinglePoint(&WPL_Store) == WPL_OK)
{
if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
635,6 → 637,7
GPS_pWaypoint = PointList_WPBegin(); // updates POI index
BeepTime = 150;
}
}
FromFC_Load_SinglePoint = 0;
}
// ++++++++++++++++++++++++++++++++++++++++++++++
643,10 → 646,10
{
SD_WatchDog = 30000;
if(SDCardInfo.Valid == 1) Logging_Update(); // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1
else
{
ToFC_MaxWpListIndex = 0;
if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100;
else
{
ToFC_MaxWpListIndex = 0;
if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100;
}
if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r");
}
700,4 → 703,4
*/
}
}
//DebugOut.Analog[16]
//DebugOut.Analog[]
/trunk/main.h
36,7 → 36,7
// 18 = S
 
#ifndef FOLLOW_ME
#define FC_SPI_COMPATIBLE 63 // <------------------
#define FC_SPI_COMPATIBLE 64 // <------------------
#else
#define FC_SPI_COMPATIBLE 0xFF
#endif
175,9 → 175,10
u8 ExtraConfig;
u8 ComingHomeAltitude;
u8 GlobalConfig3;
u8 NaviOut1Parameter; // Distance between Photo releases
u8 AutoPhotoDistance; // Distance between Photo releases (Position)
u8 FromFC_LandingSpeed;
u8 FromFC_LowVoltageHomeActive;
u8 AutoPhotoAltitudes; // Distance between Photo releases (Altitudes)
} __attribute__((packed)) Param_t;
 
typedef struct
217,5 → 218,6
extern u16 SD_PosAccuracy;
extern u16 SD_ComingHomeSpeed;
extern u16 SD_DPH_Speed;
extern u32 MaxRadius_in_m;
 
#endif // _MAIN_H
/trunk/menu.c
78,7 → 78,7
 
void Menu_Putchar(char c)
{
if(DispPtr < DISPLAYBUFFSIZE) DisplayBuff[DispPtr++] = c; ;
if(DispPtr < DISPLAYBUFFSIZE) DisplayBuff[DispPtr++] = c; ;
}
 
void Menu_Clear(void)
90,7 → 90,7
// Display with 20 characters in 4 lines
void Menu_Update(u8 Keys)
{
s32 i1,i2;
s32 i1,i2;
u8 sign;
 
if(Keys & KEY1) { if(MenuItem > 0) MenuItem--; else MenuItem = MaxMenuItem;}
99,8 → 99,8
if(MenuItem > MaxMenuItem) MenuItem = MaxMenuItem;
Menu_Clear();
// print menu item number in the upper right corner
if(MenuItem < 10)
{
if(MenuItem < 10)
{
LCD_printfxy(17,0,"[%i]",MenuItem);
}
else // Menuitem >= 10
108,22 → 108,22
LCD_printfxy(16,0,"[%i]",MenuItem);
}
 
switch(MenuItem)
{
switch(MenuItem)
{
// Version Info
case 0:
LCD_printfxy(0,0,"++ Navi-Ctrl ++");
LCD_printfxy(0,1,"HW V%d.%d SW V%d.%02d%c", (Version_HW & 0x7F)/10, (Version_HW & 0x7F)%10, VERSION_MAJOR, VERSION_MINOR, 'a'+ VERSION_PATCH);
 
if(ErrorCode)
if(ErrorCode)
{
LCD_printfxy(0,2,"Error: %d",ErrorCode);
LCD_printfxy(0,3,"%s",ErrorMSG);
LCD_printfxy(0,3,"%s",ErrorMSG);
}
else
{
LCD_printfxy(0,3,"(c) HiSystems GmbH");
LCD_printfxy(0,2,"%s",ErrorMSG);
LCD_printfxy(0,3,"(c) HiSystems GmbH");
LCD_printfxy(0,2,"%s",ErrorMSG);
}
break;
case 1:
235,27 → 235,27
break;
case 4: // Navi Params 1 from FC
LCD_printfxy(0,0,"NaviMode: %3i" , Parameter.NaviGpsModeControl);
LCD_printfxy(0,1,"G :%3i P :%3i ",Parameter.NaviGpsGain, Parameter.NaviGpsP);
LCD_printfxy(0,2,"I :%3i D :%3i ",Parameter.NaviGpsI, Parameter.NaviGpsD);
LCD_printfxy(0,3,"ACC:%3i SAT:%3i ",Parameter.NaviGpsACC, Parameter.NaviGpsMinSat);
LCD_printfxy(0,1,"G :%3i P :%3i ",Parameter.NaviGpsGain, Parameter.NaviGpsP);
LCD_printfxy(0,2,"I :%3i D :%3i ",Parameter.NaviGpsI, Parameter.NaviGpsD);
LCD_printfxy(0,3,"ACC:%3i SAT:%3i ",Parameter.NaviGpsACC, Parameter.NaviGpsMinSat);
break;
case 5: // Navi Params 2 from FC
case 5: // Navi Params 2 from FC
LCD_printfxy(0,0,"Stick TS: %3i", Parameter.NaviStickThreshold);
LCD_printfxy(0,1,"MaxRadius: %3im", DebugOut.Analog[4]);
LCD_printfxy(0,2,"WindCorr: %3i", Parameter.NaviWindCorrection);
LCD_printfxy(0,3,"AccComp: %3i", Parameter.NaviSpeedCompensation);
LCD_printfxy(0,1,"MaxRadius: %3im",MaxRadius_in_m);
LCD_printfxy(0,2,"WindCorr: %3i", Parameter.NaviWindCorrection);
LCD_printfxy(0,3,"AccComp: %3i", Parameter.NaviSpeedCompensation);
break;
case 6: // Navi Params 3 from FC
LCD_printfxy(0,0,"Angle-Limit: %3i", Parameter.NaviAngleLimitation);
LCD_printfxy(0,1," P-Limit: %3i", Parameter.NaviGpsPLimit);
LCD_printfxy(0,2," I-Limit: %3i", Parameter.NaviGpsILimit);
LCD_printfxy(0,3," D-Limit: %3i", Parameter.NaviGpsDLimit);
LCD_printfxy(0,1," P-Limit: %3i", Parameter.NaviGpsPLimit);
LCD_printfxy(0,2," I-Limit: %3i", Parameter.NaviGpsILimit);
LCD_printfxy(0,3," D-Limit: %3i", Parameter.NaviGpsDLimit);
break;
case 7: // Max Ranges
LCD_printfxy(0,0,"Maximum flying ");
LCD_printfxy(0,1,"Range: %4im ", AbsoluteFlyingRange);
LCD_printfxy(0,2,"Descend: %4im ", AutoDescendRange);
LCD_printfxy(0,3,"Altitude:%4im ", AbsoluteFlyingAltitude);
LCD_printfxy(0,1,"Range: %4im ", AbsoluteFlyingRange);
LCD_printfxy(0,2,"Descend: %4im ", AutoDescendRange);
LCD_printfxy(0,3,"Altitude:%4im ", AbsoluteFlyingAltitude);
if(!AbsoluteFlyingRange) LCD_printfxy(9,1,"disabled");
if(!AutoDescendRange) LCD_printfxy(9,2,"disabled");
if(!AbsoluteFlyingAltitude) LCD_printfxy(9,3,"disabled");
316,19 → 316,19
break;
case 10: // RC stick controls from FC
LCD_printfxy(0,0,"RC-Sticks" );
LCD_printfxy(0,1,"Ni:%4i Ro:%4i ",FC.StickNick, FC.StickRoll);
LCD_printfxy(0,2,"Gs:%4i Ya:%4i ",FC.StickGas, FC.StickYaw);
LCD_printfxy(0,3,"RC-Level: %3i", FC.RC_Quality); // Remote Control Level from FC
LCD_printfxy(0,1,"Ni:%4i Ro:%4i ",FC.StickNick, FC.StickRoll);
LCD_printfxy(0,2,"Gs:%4i Ya:%4i ",FC.StickGas, FC.StickYaw);
LCD_printfxy(0,3,"RC-Level: %3i", FC.RC_Quality); // Remote Control Level from FC
break;
case 11: // RC poti controls from FC
LCD_printfxy(0,0,"RC-Potis 1" );
LCD_printfxy(0,1,"Po1:%3i Po2:%3i ",FC.Poti[0], FC.Poti[1]);
LCD_printfxy(0,2,"Po3:%3i Po4:%3i ",FC.Poti[2], FC.Poti[3]);
LCD_printfxy(0,1,"Po1:%3i Po2:%3i ",FC.Poti[0], FC.Poti[1]);
LCD_printfxy(0,2,"Po3:%3i Po4:%3i ",FC.Poti[2], FC.Poti[3]);
break;
case 12: // RC poti controls from FC
LCD_printfxy(0,0,"RC-Potis 2" );
LCD_printfxy(0,1,"Po5:%3i Po6:%3i ",FC.Poti[4], FC.Poti[5]);
LCD_printfxy(0,2,"Po7:%3i Po8:%3i ",FC.Poti[6], FC.Poti[7]);
LCD_printfxy(0,1,"Po5:%3i Po6:%3i ",FC.Poti[4], FC.Poti[5]);
LCD_printfxy(0,2,"Po7:%3i Po8:%3i ",FC.Poti[6], FC.Poti[7]);
break;
case 13: // attitude from FC
if(FromFlightCtrl.AngleNick < 0) sign = '-';
340,21 → 340,21
else sign = '+';
i1 = abs(FromFlightCtrl.AngleRoll)/10;
i2 = abs(FromFlightCtrl.AngleRoll)%10;
LCD_printfxy(0,1,"GyroRoll:%c%03ld.%01ld", sign, i1, i2);
LCD_printfxy(0,1,"GyroRoll:%c%03ld.%01ld", sign, i1, i2);
if(FromFlightCtrl.AccNick < 0) sign = '-';
else sign = '+';
i1 = abs(FromFlightCtrl.AccNick)/10;
i2 = abs(FromFlightCtrl.AccNick)%10;
LCD_printfxy(0,2," AccNick:%c%03ld.%01ld", sign, i1, i2);
if(FromFlightCtrl.AccRoll < 0) sign = '-';
if(FromFlightCtrl.AccRoll < 0) sign = '-';
else sign = '+';
i1 = abs(FromFlightCtrl.AccRoll)/10;
i2 = abs(FromFlightCtrl.AccRoll)%10;
LCD_printfxy(0,3," AccRoll:%c%03ld.%01ld", sign, i1, i2);
LCD_printfxy(0,3," AccRoll:%c%03ld.%01ld", sign, i1, i2);
break;
case 14: // gyros from FC
LCD_printfxy(0,0,"GyroNick: %4i", FromFlightCtrl.GyroNick);
LCD_printfxy(0,1,"GyroRoll: %4i", FromFlightCtrl.GyroRoll);
LCD_printfxy(0,0,"GyroNick: %4i", FromFlightCtrl.GyroNick);
LCD_printfxy(0,1,"GyroRoll: %4i", FromFlightCtrl.GyroRoll);
LCD_printfxy(0,2,"GyroYaw: %4i", FromFlightCtrl.GyroYaw);
if(FC_is_Calibrated) LCD_printfxy(0,3,"Calibrated ")
else LCD_printfxy(0,3,"not calibrated");
363,7 → 363,7
LCD_printfxy(0,0,"Compass: %3i", FromFlightCtrl.GyroHeading / 10);
LCD_printfxy(0,1,"Man.-Offset:%3i", FC.FromFC_CompassOffset / 10);
if(FC.FromFC_DisableDeclination)
{
{
LCD_printfxy(0,2,"Mag.Declinat.:disabl");
}
else
373,7 → 373,7
LCD_printfxy(0,2,"Mag.Declinat.:%c%i.%1i", sign, abs(GeoMagDec)/10,abs(GeoMagDec)%10);
}
LCD_printfxy(0,3,"True Compass: %3i", GyroCompassCorrected/10);
break;
break;
case 16: // User Parameter
LCD_printfxy(0,0,"UP1:%3i UP2:%3i",Parameter.User1,Parameter.User2);
LCD_printfxy(0,1,"UP3:%3i UP4:%3i",Parameter.User3,Parameter.User4);
390,7 → 390,7
{
LCD_printfxy(0,0,"Calibration:");
LCD_printfxy(0,1,"Step %d/", Compass_CalState);
LCD_printfxy(0,2,"X %4i Y %4i Z %4i",MagVector.X,MagVector.Y,MagVector.Z);
LCD_printfxy(0,2,"X %4i Y %4i Z %4i",MagVector.X,MagVector.Y,MagVector.Z);
LCD_printfxy(9,3,"(ESC)(NEXT)");
switch(Compass_CalState)
{
421,7 → 421,7
if(GeoMagDec < 0) sign = '-';
else sign = '+';
LCD_printfxy(0,0,"Magnetic Field");
LCD_printfxy(0,1,"X:%5i",MagVector.X);
LCD_printfxy(0,1,"X:%5i",MagVector.X);
LCD_printfxy(0,2,"Y:%5i",MagVector.Y);
LCD_printfxy(0,3,"Z:%5i",MagVector.Z);
LCD_printfxy(8,1,"Field:%3i",EarthMagneticField/5);
471,13 → 471,13
LCD_printfxy(0,0,"Ext. Compass" );
if(Compass_I2CPort == NCMAG_PORT_EXTERN)
{
u8 tmp;
LCD_printfxy(0,1,"ACC X Y Z");
LCD_printfxy(0,2," %5d %5d %5d", AccVector.X/40, AccVector.Y/40, AccVector.Z/40);
tmp = NCMAG_GetOrientationFromAcc();
LCD_printfxy(0,3,"Orientat.: ");
if(!tmp) LCD_printfxy(11,3,"??") else LCD_printfxy(11,3,"%2d",tmp);
LCD_printfxy(15,3,"(%d)",NCMAG_Orientation);
u8 tmp;
LCD_printfxy(0,1,"ACC X Y Z");
LCD_printfxy(0,2," %5d %5d %5d", AccVector.X/40, AccVector.Y/40, AccVector.Z/40);
tmp = NCMAG_GetOrientationFromAcc();
LCD_printfxy(0,3,"Orientat.: ");
if(!tmp) LCD_printfxy(11,3,"??") else LCD_printfxy(11,3,"%2d",tmp);
LCD_printfxy(15,3,"(%d)",NCMAG_Orientation);
}
else
{
499,12 → 499,12
// 12345678901234567890
LCD_printfxy(0,2,"Points Index ");
LCD_printfxy(0,3," %3d %3d LOAD", PointList_GetCount(), index);
}
}
break;
case 25:
{
static u8 index = 1;
if(Keys & KEY3)
if(Keys & KEY3)
{
if(index < ToFC_MaxWpListIndex) index++;
else index = 1;
513,19 → 513,19
LCD_printfxy(0,1,"Name: %s", WPL_Store.Name);
// 12345678901234567890
LCD_printfxy(0,2,"Points Index ");
if(GPSData.SatFix == SATFIX_3D)
{
LCD_printfxy(0,3," %3d %3d LOAD", PointList_GetCount(), index);
if(Keys & KEY4) FromFC_LoadWP_List = index | 0x80;
}
else LCD_printfxy(0,3," No Satfix ! ", index);
if(GPSData.SatFix == SATFIX_3D)
{
LCD_printfxy(0,3," %3d %3d LOAD", PointList_GetCount(), index);
if(Keys & KEY4) FromFC_LoadWP_List = index | 0x80;
}
else LCD_printfxy(0,3," No Satfix ! ", index);
 
}
}
break;
case 26:
{
static u8 index = 1;
if(Keys & KEY3)
if(Keys & KEY3)
{
if(index < ToFC_MaxWpListIndex) index++;
else index = 1;
536,7 → 536,7
// 12345678901234567890
// LCD_printfxy(0,2,"Points Index ");
LCD_printfxy(0,3,"Number: %3d (LOAD)", index);
}
}
break;
case 27:
{
550,13 → 550,13
LCD_printfxy(0,1,"Alt: %3dm", NaviData.Altimeter/20);
// 12345678901234567890
LCD_printfxy(0,2,"Dir: %3d ", CompassSetpointCorrected/10);
if(GPSData.SatFix == SATFIX_3D)
if(GPSData.SatFix == SATFIX_3D)
{
LCD_printfxy(0,3,"Number: %3d (SAVE)", index);
if(Keys & KEY4) FromFC_Save_SinglePoint = index;
}
else LCD_printfxy(0,3," No Satfix ! ", index);
}
}
break;
 
default:
563,5 → 563,5
//MaxMenuItem = MenuItem - 1;
MenuItem = 0;
break;
}
}
}
/trunk/sdc.c
797,7 → 797,7
}
if (retryCounter > SDCardWriteRetryCounterMax)
{ SDCardWriteRetryCounterMax = retryCounter;
// DebugOut.Analog[18] = SDCardWriteRetryCounterMax;
// DebugOut.Analog[] = SDCardWriteRetryCounterMax;
}
return(result);
/trunk/spi_slave.c
448,6 → 448,7
{
ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint;
}
//DebugOut.Analog[] = ToFlightCtrl.Param.Byte[8];
break;
case SPI_NCCMD_HOTT_INFO:
if(NewWPL_Name) hott_index = 100;
690,7 → 691,7
CHK_POTI_MM(Parameter.NaviAngleLimitation,FromFlightCtrl.Param.Byte[11],0,255);
break;
case SPI_FCCMD_PARAMETER2:
CHK_POTI_MM(Parameter.NaviOut1Parameter,FromFlightCtrl.Param.Byte[0],0,255);
CHK_POTI_MM(Parameter.AutoPhotoDistance,FromFlightCtrl.Param.Byte[0],0,255);
if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1]; // will be cleared in the SD-Logging
Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[2];
Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3];
700,7 → 701,7
if(FromFlightCtrl.Param.Byte[7]) FromFC_Save_SinglePoint = FromFlightCtrl.Param.Byte[7];
CompassSetpoint = FromFlightCtrl.Param.sInt[4] * 10; // 8 & 9
CompassSetpointCorrected = (3600 + CompassSetpoint + FC.FromFC_CompassOffset + GeoMagDec) % 3600;
//FromFlightCtrl.Param.Byte[10]
CHK_POTI_MM(Parameter.AutoPhotoAltitudes,FromFlightCtrl.Param.Byte[10],0,255);
//FromFlightCtrl.Param.Byte[11]
break;
case SPI_FCCMD_STICK:
/trunk/uart1.c
147,7 → 147,7
"AngleRoll ",
"AccNick ",
"AccRoll ",
"OperatingRadius ",
"Altitude [0.1m] ",
"FC-Flags ", //5
"NC-Flags ",
"Voltage [0.1V] ",
160,8 → 160,8
"I2C Error ",
"I2C Okay ", //15
"16 dist (cm) ",
"17 pos accuracy ",
"18 YawCorrection",
"17 ",
"18 ",
"19 ", // SD-Card-time
"EarthMagnet [%] ", //20
"Ground Speed ", // "Z_Speed ",
/trunk/waypoints.c
7,7 → 7,7
// + Software Nutzungsbedingungen (english version: see below)
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18,7 → 18,7
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
// + des Mitverschuldens offen.
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
32,7 → 32,7
// + Software LICENSING TERMS
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
// + The Software may only be used with the Licensor's products.
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
55,7 → 55,7
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdlib.h>
#include <string.h>
#include "91x_lib.h"
#include "waypoints.h"
81,7 → 81,7
 
u8 PointList_Init(void)
{
return PointList_Clear();
return PointList_Clear();
}
 
u8 PointList_Clear(void)
90,7 → 90,7
WPIndex = 0; // real list position are 1 ,2, 3 ...
POIIndex = 0; // real list position are 1 ,2, 3 ...
WPCount = 0; // no waypoints
POICount = 0;
POICount = 0;
PointCount = 0; // no contents
WPActive = FALSE;
NaviData.WaypointNumber = WPCount;
115,7 → 115,7
PointList[i].Name[0] = 0;
}
ClearWLP_Name();
return TRUE;
return TRUE;
}
 
u8 PointList_GetCount(void)
149,14 → 149,14
WPCount++;
PointCount++;
break;
 
case POINT_TYPE_POI:
POICount++;
PointCount++;
break;
}
break;
 
break;
case POINT_TYPE_WP: // was a waypoint
switch(pPoint->Type)
{
169,7 → 169,7
case POINT_TYPE_WP:
//nothing to do
break;
 
case POINT_TYPE_POI:
POICount++;
WPCount--;
176,7 → 176,7
break;
}
break;
 
case POINT_TYPE_POI: // was a poi
switch(pPoint->Type)
{
189,15 → 189,15
WPCount++;
POICount--;
break;
 
case POINT_TYPE_POI:
default:
// nothing to do
break;
}
break;
break;
}
memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
NaviData.WaypointNumber = WPCount;
return pPoint->Index;
}
213,7 → 213,7
if(WPActive == FALSE) return(NULL);
 
POIIndex = 0; // set invalid POI
if(PointCount > 0)
if(PointCount > 0)
{
// search for first wp in list
for(i = 0; i <MAX_LIST_LEN; i++)
229,7 → 229,7
NaviData.WaypointIndex = 1;
// update index to POI
if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
else POIIndex = 0;
else POIIndex = 0;
}
else // some points in the list but no WP found
{
247,8 → 247,8
}
else // no point in the list
{
POIIndex = 0;
NaviData.WaypointIndex = 0;
POIIndex = 0;
NaviData.WaypointIndex = 0;
}
 
if(WPIndex) return(&(PointList[WPIndex-1]));
258,7 → 258,7
// returns the last waypoint
Point_t* PointList_WPEnd(void)
{
 
u8 i;
WPIndex = 0; // set list position invalid
POIIndex = 0; // set invalid
271,7 → 271,7
for(i = 1; i <= MAX_LIST_LEN; i++)
{
if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
{
{
WPIndex = MAX_LIST_LEN - i + 1;
break;
}
280,7 → 280,7
{
NaviData.WaypointIndex = WPCount;
if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
else POIIndex = 0;
else POIIndex = 0;
}
else // list contains some points but no WP in the list
{
288,12 → 288,12
for(i = 1; i <= MAX_LIST_LEN; i++)
{
if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
{
{
POIIndex = MAX_LIST_LEN - i + 1;
break;
}
}
NaviData.WaypointIndex = 0;
NaviData.WaypointIndex = 0;
}
}
else // no point in the list
310,7 → 310,7
{
u8 wp_found = 0;
if(WPActive == FALSE) return(NULL);
 
if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
{
u8 i;
331,18 → 331,18
else POIIndex = 0;
return(&(PointList[WPIndex-1])); // return pointer to this waypoint
}
else
else
{ // no next wp found
NaviData.WaypointIndex = 0;
NaviData.WaypointIndex = 0;
POIIndex = 0;
return(NULL);
}
}
}
 
void PointList_WPActive(u8 set)
{
if(set)
{
{
WPActive = TRUE;
PointList_WPBegin(); // updates POI index
}
352,10 → 352,10
POIIndex = 0; // disable POI also
}
}
 
Point_t* PointList_GetPOI(void)
{
return PointList_GetAt(POIIndex);
return PointList_GetAt(POIIndex);
}
 
 
374,11 → 374,11
{ // check if wpl file is existing
if(fexist_(filename))
{ //file is existent
if(!(overwride))
if(!(overwride))
{
UART1_PutString("Error: file exist!\r\n");
return(WPL_FILEEXIST);
}
}
}
fp = fopen_(filename, 'w'); // try to open the file
if(fp == NULL)
388,9 → 388,9
}
// Create general section and key entries
fputs_("[General]\r\n", fp);
sprintf(wpline, "Name=%s\r\n", listname);
sprintf(wpline, "Name=%s\r\n", listname);
fputs_(wpline, fp);
sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
fputs_(wpline, fp);
sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);
fputs_(wpline, fp);
456,8 → 456,8
fputs_(wpline, fp);
// write point type
sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);
fputs_(wpline, fp);
// write prefix
fputs_(wpline, fp);
// write prefix
sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);
fputs_(wpline, fp);
} // EOF loop over all points
470,9 → 470,9
{
UART1_PutString("ok\r\n");
retval = WPL_OK;
}
}
} // EOF if(Fat16_IsValid())
else
else
{
UART1_PutString("no file system found!\r\n");
retval = WPL_NO_SDCARD_FOUND;
485,7 → 485,7
File_t *fp;
s8 wpline[LINE_MAX];
u8 retval = WPL_ERROR;
 
s8 *name, *value;
u8 i;
 
497,7 → 497,7
PointList_Clear();
 
UART1_PutString("\n\r Read ");
UART1_PutString(filename);
UART1_PutString(filename);
UART1_PutString("...");
 
if(Fat16_IsValid())
604,12 → 604,12
PointList[IsPointSection-1].Type = (u8)atoi(value);
if(PointList[IsPointSection-1].Type > 0) PointList[IsPointSection-1].Type--; // index shift
else PointList[IsPointSection-1].Type = POINT_TYPE_INVALID;
 
switch(PointList[IsPointSection-1].Type)
{
case POINT_TYPE_WP:
// this works only if altitude key is set before point type key in WPL file !!
PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs
PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs
WPCount++;
break;
 
628,22 → 628,22
UART1_PutString("Unknown key: ");
UART1_PutString(name);
UART1_PutString("\r\n");
}
}
} // EOF point section
else if(IsGeneralSection)
{
if(strcmp(name, "NUMBEROFWAYPOINTS") == 0)
{
{
WPNumber = (u8)atoi(value);
if(!WPNumber) // no waypoints in file
{
return(WPL_NO_WAYPOINTS); // we are done here
return(WPL_NO_WAYPOINTS); // we are done here
}
else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list
{
UART1_PutString("To many points!");
return(WPL_ERROR);
}
}
}
else if (strcmp(name, "FILEVERSION") == 0)
{
651,7 → 651,7
{
PointList_Clear();
UART1_PutString("Bad file version!\r\n");
return(WPL_ERROR);
return(WPL_ERROR);
}
}
else if (strcmp(name, "NAME") == 0)
689,14 → 689,14
} // EOF loop over all lines
fclose_(fp);
NaviData.WaypointNumber = WPCount;
retval = WPL_OK;
UART1_PutString("ok\r\n");
retval = WPL_OK;
UART1_PutString("ok\r\n");
} // EOF if(Fat16_IsValid())
else
else
{
UART1_PutString("no file system found!\r\n");
retval = WPL_NO_SDCARD_FOUND;
}
}
return(retval);
}
 
703,7 → 703,7
// load actual point list from SD card
u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
{
u8 filename[30];
u8 filename[30];
 
pWPL_Store->Name[0] = 0; // clear current list name
 
710,7 → 710,7
// user absolute path, i.e. leading /
if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root
{
sprintf(filename, "/default.wpl");
sprintf(filename, "/default.wpl");
}
else
{
723,10 → 723,10
u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store)
{
u8 filename[30];
 
 
if(PointCount == 0) return(WPL_NO_WAYPOINTS);
// user absolute path, i.e. leading /
// user absolute path, i.e. leading /
if(pWPL_Store->Index == 0)
{
sprintf(filename, "/default.wpl");
739,7 → 739,7
}
 
 
// save actual gps position and heading to file
// save actual gps positiin and heading to file
u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store)
{
u8 retval = WPL_ERROR;
747,11 → 747,11
Point_t WP;
 
UART1_PutString("\n\r write single point\n\r");
if(GPSData.Position.Status == INVALID)
{
if(GPSData.Position.Status == INVALID)
{
UART1_PutString("ERROR: No GPS - Fix\n\r");
return(retval);
}
}
 
// clear current point list
PointList_Clear();
765,16 → 765,16
WP.Index = 1;
WP.Type = POINT_TYPE_WP;
WP.WP_EventChannelValue = 0;
if(FC.StatusFlags & FC_STATUS_FLY && (NaviData.Altimeter) > 8 * 20)
if(FC.StatusFlags & FC_STATUS_FLY)
{
WP.AltitudeRate = 15;
WP.Position.Altitude = NaviData.Altimeter / 2;
}
else
{
WP.AltitudeRate = 30;
WP.Position.Altitude = NaviData.Altimeter / 2;
WP.AltitudeRate = 0;
WP.Position.Altitude = 0;
}
else
{
WP.AltitudeRate = 0;
WP.Position.Altitude = 0;
}
WP.Speed = SD_ComingHomeSpeed;
WP.CamAngle = 0;
WP.Name[0] = 'P';
783,7 → 783,7
PointList_SetAt(&WP);
 
sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
UART1_PutString(filename);
UART1_PutString(filename);
sprintf(pWPL_Store->Name, "POINT%03d ", pWPL_Store->Index);
retval = PointList_Save(filename, pWPL_Store->Name, 1);
 
791,12 → 791,11
PointList_Clear();
return(retval);
}
 
// load target gps posititon and heading from file
u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store)
{
u8 filename[30];
 
sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
pWPL_Store->Name[0] = 0; // clear current list name
return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name));
803,6 → 802,7
}
 
 
 
void ClearWLP_Name(void)
{
u8 i;
817,9 → 817,9
s32 altitude;
GPS_Pos_t OldRefPos;
GPS_Pos_Deviation_t RefDeviation;
 
// check inputs for plausibility;
if((RefIndex == 0) || (RefIndex > PointCount)) return(retval);
if((RefIndex == 0) || (RefIndex > PointCount)) return(retval);
if(pRefPos == NULL) return(retval);
if(pRefPos->Status == INVALID) return(retval);
 
836,7 → 836,7
if(!GPSPos_Deviation(&(PointList[i].Position), &OldRefPos, &RefDeviation)) break;
// copy of the new reference position into this list place
if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
// restore former altitude
// restore former altitude
PointList[i].Position.Altitude = altitude;
// move new reference according to the deviation of the old reference
if(RotationAngle > 0)
850,7 → 850,7
// move new reference according to the deviation of the old reference
retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East);
}
if(!retval) break;
if(!retval) break;
}
} // else ref pos old not copied!
if(!retval) PointList_Clear();
857,5 → 857,5
return(retval);
}