62,6 → 62,7 |
#include "uart1.h" |
#include "fat16.h" |
#include "main.h" |
#include "spi_slave.h" |
|
|
WPL_Store_t WPL_Store; |
357,38 → 358,29 |
return PointList_GetAt(POIIndex); |
} |
|
|
#define LINE_MAX 70 |
#define WP_FILE_VERSION_COMPATIBLE 3 |
// save actual point list to SD card |
u8 PointList_SaveToFile(WPL_Store_t * pWPL_Store) |
|
u8 PointList_Save(u8 * filename, u8* listname, u8 overwride) |
{ |
File_t *fp; |
s8 wpline[LINE_MAX]; |
u8 retval = WPL_ERROR; |
|
if(PointCount == 0) return(WPL_NO_WAYPOINTS); |
// user absolute path, i.e. leading / |
if(pWPL_Store->Index == 0) |
{ |
sprintf(wpline, "/default.wpl"); |
} |
else |
{ |
sprintf(wpline, "/WPL/list_%03d.wpl", pWPL_Store->Index); |
} |
|
UART1_PutString("\n\r Save WPL..."); |
|
if(Fat16_IsValid()) |
{ // check if wpl file is existing |
if(fexist_(wpline)) |
if(fexist_(filename)) |
{ //file is existent |
if(!(pWPL_Store->OverwriteFile)) |
if(!(overwride)) |
{ |
UART1_PutString("Error: file exist!\r\n"); |
return(WPL_FILEEXIST); |
} |
} |
fp = fopen_(wpline, 'w'); // try to open the file |
fp = fopen_(filename, 'w'); // try to open the file |
if(fp == NULL) |
{ |
UART1_PutString("ERROR: Creating waypoint file!\r\n"); |
396,7 → 388,7 |
} |
// Create general section and key entries |
fputs_("[General]\r\n", fp); |
sprintf(wpline, "Name=%s\r\n", pWPL_Store->Name); |
sprintf(wpline, "Name=%s\r\n", listname); |
fputs_(wpline, fp); |
sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE); |
fputs_(wpline, fp); |
488,8 → 480,7 |
return(retval); |
} |
|
// load actual point list from SD card |
u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store) |
u8 PointList_Load(u8 * filename, u8* listname, u8 listnamelen) |
{ |
File_t *fp; |
s8 wpline[LINE_MAX]; |
501,29 → 492,17 |
u8 IsGeneralSection = 0; |
u8 IsPointSection = 0; |
u8 WPNumber = 0; |
|
|
// clear point list first |
PointList_Clear(); |
pWPL_Store->Name[0] = 0; // clear current list name |
|
// user absolute path, i.e. leading / |
if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root |
{ |
sprintf(wpline, "/default.wpl"); |
} |
else |
{ |
sprintf(wpline, "/WPL/list_%03d.wpl", pWPL_Store->Index); |
} |
|
UART1_PutString("\n\r Read "); |
UART1_PutString(wpline); |
UART1_PutString(filename); |
UART1_PutString("..."); |
|
if(Fat16_IsValid()) |
{ // check if wpl file is existing |
fp = fopen_(wpline, 'r'); // try to open the file |
fp = fopen_(filename, 'r'); // try to open the file |
if(fp == NULL) |
{ |
UART1_PutString("ERROR: Reading waypoint file!\r\n"); |
677,16 → 656,25 |
} |
else if (strcmp(name, "NAME") == 0) |
{ |
u8 len = strlen(value); |
if(value[len-1] == '\r') |
if(listname) |
{ |
value[len-1] = 0; |
len--; |
u8 len = strlen(value); |
if(len) |
{ |
if(value[len-1] == '\r') |
{ |
value[len-1] = 0; |
len--; |
} |
} |
if(len > listnamelen) len = listnamelen; |
if(len) |
{ |
value[len-1] = 0; // terminate string |
if(listname) memcpy(listname, value, len); |
} |
NewWPL_Name = 1; |
} |
if(len > 11) value[11] = 0; |
else for(;len < 11; len++) value[len] = ' '; |
memcpy(pWPL_Store->Name, value, 12); |
NewWPL_Name = 1; |
} |
else |
{ |
712,17 → 700,104 |
return(retval); |
} |
|
// load actual point list from SD card |
u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store) |
{ |
u8 filename[30]; |
|
pWPL_Store->Name[0] = 0; // clear current list name |
|
// 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"); |
} |
else |
{ |
sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index); |
} |
return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name)); |
} |
|
// save actual point list to SD card |
u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store) |
{ |
u8 filename[30]; |
|
|
if(PointCount == 0) return(WPL_NO_WAYPOINTS); |
// user absolute path, i.e. leading / |
if(pWPL_Store->Index == 0) |
{ |
sprintf(filename, "/default.wpl"); |
} |
else |
{ |
sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index); |
} |
return PointList_Save(filename, pWPL_Store->Name, pWPL_Store->OverwriteFile); |
} |
|
|
// save actual gps positiin and heading to file |
u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store) |
{ |
u8 retval = WPL_ERROR; |
u8 filename[30]; |
Point_t WP; |
|
if(GPSData.Position.Status != INVALID) return(retval); |
|
// clear current point list |
PointList_Clear(); |
// prepare WP at current position |
GPSPos_Copy(&GPSData.Position, &(WP.Position)); |
// set heading |
WP.Heading = CompassSetpointCorrected/10; |
if(WP.Heading == 0) WP.Heading = 360; |
WP.ToleranceRadius = 0; |
WP.HoldTime = 0; |
WP.Index = 1; |
WP.Type = POINT_TYPE_WP; |
WP.WP_EventChannelValue = 0; |
WP.AltitudeRate = 0; |
WP.Speed = 0; |
WP.CamAngle = 0; |
WP.Name[0] = 'P'; |
WP.Name[1] = 0; |
// add this point to wp list |
PointList_SetAt(&WP); |
|
sprintf(filename, "/WPL/point_%03d.wpl", pWPL_Store->Index); |
sprintf(pWPL_Store->Name, "POINT%03d", pWPL_Store->Index); |
retval = PointList_Save(filename, pWPL_Store->Name, 1); |
return(retval); |
} |
// load target gps posititon and heading from file |
u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store) |
{ |
u8 filename[30]; |
|
sprintf(filename, "/WPL/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)); |
} |
|
|
|
void ClearWLP_Name(void) |
{ |
unsigned char i; |
for(i=0; i<sizeof(WPL_Store.Name);i++) WPL_Store.Name[i] = 0; |
NewWPL_Name = 1; |
u8 i; |
for(i=0; i<sizeof(WPL_Store.Name);i++) WPL_Store.Name[i] = 0; |
NewWPL_Name = 1; |
} |
|
// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards |
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle) |
{ |
u8 retval = 0; |
GPS_Pos_t FirstPoint; |
s32 altitude; |
GPS_Pos_t OldRefPos; |
GPS_Pos_Deviation_t RefDeviation; |
|
// check inputs for plausibility; |
730,47 → 805,35 |
if(pRefPos == NULL) return(retval); |
if(pRefPos->Status == INVALID) return(retval); |
|
// try to copy the old reference in point list to a local buffer |
if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &FirstPoint)) |
if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &OldRefPos)) // backup old reference position |
{ |
u8 i; |
// for each point position in the list |
// iterate the position list |
for(i = 0; i < PointCount; i++) |
{ |
retval = 0; |
// Save altitude of that point |
pRefPos->Altitude = PointList[i].Position.Altitude; |
// calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position |
if(!GPSPos_Deviation(&(PointList[i].Position), &FirstPoint, &RefDeviation)) break; |
// backup altitude of this point |
altitude = PointList[i].Position.Altitude; |
// calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position |
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 |
PointList[i].Position.Altitude = altitude; |
// move new reference according to the deviation of the old reference |
retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East); |
if(RotationAngle > 0) |
{ |
retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360, RefDeviation.Distance ); |
// Now rotate the heading positions if they are fixed angles |
if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360; |
} |
else // no rotation |
{ |
// move new reference according to the deviation of the old reference |
retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East); |
} |
if(!retval) break; |
} |
// ++++++++++++++++++++++++++++++++++++++++++++++ |
// Now rotate around the reference point |
// ++++++++++++++++++++++++++++++++++++++++++++++ |
if(RotationAngle > 0) |
{ |
GPSPos_Copy(&(PointList[RefIndex-1].Position), &FirstPoint); // Rotate around the reference point |
for(i = 0; i < PointCount; i++) |
{ |
retval = 0; |
// Save altitude of that point |
pRefPos->Altitude = PointList[i].Position.Altitude; |
// calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position |
if(!GPSPos_Deviation(&(PointList[i].Position), &FirstPoint, &RefDeviation)) break; |
// copy of the new reference position into this list place |
if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break; |
// move new reference according to the deviation of the old reference |
retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360,RefDeviation.Distance ); |
// Now rotate the heading positions if they are fixed angles |
if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360; |
if(!retval) break; |
} |
} |
// ++++++++++++++++++++++++++++++++++++++++++++++ |
} // else ref pos old not copied! |
if(!retval) PointList_Clear(); |
return(retval); |