| 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); |