Rev 504 | Rev 514 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 504 | Rev 513 | ||
|---|---|---|---|
| Line 60... | Line 60... | ||
| 60 | #include "91x_lib.h" |
60 | #include "91x_lib.h" |
| 61 | #include "waypoints.h" |
61 | #include "waypoints.h" |
| 62 | #include "uart1.h" |
62 | #include "uart1.h" |
| 63 | #include "fat16.h" |
63 | #include "fat16.h" |
| 64 | #include "main.h" |
64 | #include "main.h" |
| - | 65 | #include "spi_slave.h" |
|
| Line 65... | Line 66... | ||
| 65 | 66 | ||
| Line 66... | Line 67... | ||
| 66 | 67 | ||
| Line 355... | Line 356... | ||
| 355 | Point_t* PointList_GetPOI(void) |
356 | Point_t* PointList_GetPOI(void) |
| 356 | { |
357 | { |
| 357 | return PointList_GetAt(POIIndex); |
358 | return PointList_GetAt(POIIndex); |
| 358 | } |
359 | } |
| Line -... | Line 360... | ||
| - | 360 | ||
| 359 | 361 | ||
| 360 | #define LINE_MAX 70 |
362 | #define LINE_MAX 70 |
| 361 | #define WP_FILE_VERSION_COMPATIBLE 3 |
- | |
| - | 363 | #define WP_FILE_VERSION_COMPATIBLE 3 |
|
| 362 | // save actual point list to SD card |
364 | |
| 363 | u8 PointList_SaveToFile(WPL_Store_t * pWPL_Store) |
365 | u8 PointList_Save(u8 * filename, u8* listname, u8 overwride) |
| 364 | { |
366 | { |
| 365 | File_t *fp; |
367 | File_t *fp; |
| 366 | s8 wpline[LINE_MAX]; |
368 | s8 wpline[LINE_MAX]; |
| 367 | u8 retval = WPL_ERROR; |
369 | u8 retval = WPL_ERROR; |
| 368 | - | ||
| 369 | if(PointCount == 0) return(WPL_NO_WAYPOINTS); |
- | |
| 370 | // user absolute path, i.e. leading / |
- | |
| 371 | if(pWPL_Store->Index == 0) |
- | |
| 372 | { |
- | |
| 373 | sprintf(wpline, "/default.wpl"); |
- | |
| 374 | } |
- | |
| 375 | else |
- | |
| 376 | { |
- | |
| 377 | sprintf(wpline, "/WPL/list_%03d.wpl", pWPL_Store->Index); |
- | |
| 378 | } |
370 | |
| Line 379... | Line 371... | ||
| 379 | UART1_PutString("\n\r Save WPL..."); |
371 | UART1_PutString("\n\r Save WPL..."); |
| 380 | 372 | ||
| 381 | if(Fat16_IsValid()) |
373 | if(Fat16_IsValid()) |
| 382 | { // check if wpl file is existing |
374 | { // check if wpl file is existing |
| 383 | if(fexist_(wpline)) |
375 | if(fexist_(filename)) |
| 384 | { //file is existent |
376 | { //file is existent |
| 385 | if(!(pWPL_Store->OverwriteFile)) |
377 | if(!(overwride)) |
| 386 | { |
378 | { |
| 387 | UART1_PutString("Error: file exist!\r\n"); |
379 | UART1_PutString("Error: file exist!\r\n"); |
| 388 | return(WPL_FILEEXIST); |
380 | return(WPL_FILEEXIST); |
| 389 | } |
381 | } |
| 390 | } |
382 | } |
| 391 | fp = fopen_(wpline, 'w'); // try to open the file |
383 | fp = fopen_(filename, 'w'); // try to open the file |
| 392 | if(fp == NULL) |
384 | if(fp == NULL) |
| 393 | { |
385 | { |
| 394 | UART1_PutString("ERROR: Creating waypoint file!\r\n"); |
386 | UART1_PutString("ERROR: Creating waypoint file!\r\n"); |
| 395 | return(retval); |
387 | return(retval); |
| 396 | } |
388 | } |
| 397 | // Create general section and key entries |
389 | // Create general section and key entries |
| 398 | fputs_("[General]\r\n", fp); |
390 | fputs_("[General]\r\n", fp); |
| 399 | sprintf(wpline, "Name=%s\r\n", pWPL_Store->Name); |
391 | sprintf(wpline, "Name=%s\r\n", listname); |
| 400 | fputs_(wpline, fp); |
392 | fputs_(wpline, fp); |
| 401 | sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE); |
393 | sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE); |
| 402 | fputs_(wpline, fp); |
394 | fputs_(wpline, fp); |
| Line 486... | Line 478... | ||
| 486 | retval = WPL_NO_SDCARD_FOUND; |
478 | retval = WPL_NO_SDCARD_FOUND; |
| 487 | } |
479 | } |
| 488 | return(retval); |
480 | return(retval); |
| 489 | } |
481 | } |
| Line 490... | Line -... | ||
| 490 | - | ||
| 491 | // load actual point list from SD card |
482 | |
| 492 | u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store) |
483 | u8 PointList_Load(u8 * filename, u8* listname, u8 listnamelen) |
| 493 | { |
484 | { |
| 494 | File_t *fp; |
485 | File_t *fp; |
| 495 | s8 wpline[LINE_MAX]; |
486 | s8 wpline[LINE_MAX]; |
| Line 499... | Line 490... | ||
| 499 | u8 i; |
490 | u8 i; |
| Line 500... | Line 491... | ||
| 500 | 491 | ||
| 501 | u8 IsGeneralSection = 0; |
492 | u8 IsGeneralSection = 0; |
| 502 | u8 IsPointSection = 0; |
493 | u8 IsPointSection = 0; |
| 503 | u8 WPNumber = 0; |
- | |
| Line 504... | Line 494... | ||
| 504 | 494 | u8 WPNumber = 0; |
|
| 505 | 495 | ||
| 506 | // clear point list first |
- | |
| 507 | PointList_Clear(); |
- | |
| 508 | pWPL_Store->Name[0] = 0; // clear current list name |
- | |
| 509 | - | ||
| 510 | // user absolute path, i.e. leading / |
- | |
| 511 | if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root |
- | |
| 512 | { |
- | |
| 513 | sprintf(wpline, "/default.wpl"); |
- | |
| 514 | } |
- | |
| 515 | else |
- | |
| 516 | { |
- | |
| Line 517... | Line 496... | ||
| 517 | sprintf(wpline, "/WPL/list_%03d.wpl", pWPL_Store->Index); |
496 | // clear point list first |
| 518 | } |
497 | PointList_Clear(); |
| 519 | 498 | ||
| Line 520... | Line 499... | ||
| 520 | UART1_PutString("\n\r Read "); |
499 | UART1_PutString("\n\r Read "); |
| 521 | UART1_PutString(wpline); |
500 | UART1_PutString(filename); |
| 522 | UART1_PutString("..."); |
501 | UART1_PutString("..."); |
| 523 | 502 | ||
| 524 | if(Fat16_IsValid()) |
503 | if(Fat16_IsValid()) |
| 525 | { // check if wpl file is existing |
504 | { // check if wpl file is existing |
| 526 | fp = fopen_(wpline, 'r'); // try to open the file |
505 | fp = fopen_(filename, 'r'); // try to open the file |
| 527 | if(fp == NULL) |
506 | if(fp == NULL) |
| Line 675... | Line 654... | ||
| 675 | return(WPL_ERROR); |
654 | return(WPL_ERROR); |
| 676 | } |
655 | } |
| 677 | } |
656 | } |
| 678 | else if (strcmp(name, "NAME") == 0) |
657 | else if (strcmp(name, "NAME") == 0) |
| 679 | { |
658 | { |
| 680 | u8 len = strlen(value); |
659 | if(listname) |
| 681 | if(value[len-1] == '\r') |
- | |
| 682 | { |
660 | { |
| - | 661 | u8 len = strlen(value); |
|
| - | 662 | if(len) |
|
| - | 663 | { |
|
| - | 664 | if(value[len-1] == '\r') |
|
| - | 665 | { |
|
| 683 | value[len-1] = 0; |
666 | value[len-1] = 0; |
| 684 | len--; |
667 | len--; |
| - | 668 | } |
|
| - | 669 | } |
|
| - | 670 | if(len > listnamelen) len = listnamelen; |
|
| - | 671 | if(len) |
|
| - | 672 | { |
|
| - | 673 | value[len-1] = 0; // terminate string |
|
| - | 674 | if(listname) memcpy(listname, value, len); |
|
| - | 675 | } |
|
| - | 676 | NewWPL_Name = 1; |
|
| 685 | } |
677 | } |
| 686 | if(len > 11) value[11] = 0; |
- | |
| 687 | else for(;len < 11; len++) value[len] = ' '; |
- | |
| 688 | memcpy(pWPL_Store->Name, value, 12); |
- | |
| 689 | NewWPL_Name = 1; |
- | |
| 690 | } |
678 | } |
| 691 | else |
679 | else |
| 692 | { |
680 | { |
| 693 | UART1_PutString("Unknown key: "); |
681 | UART1_PutString("Unknown key: "); |
| 694 | UART1_PutString(name); |
682 | UART1_PutString(name); |
| Line 710... | Line 698... | ||
| 710 | retval = WPL_NO_SDCARD_FOUND; |
698 | retval = WPL_NO_SDCARD_FOUND; |
| 711 | } |
699 | } |
| 712 | return(retval); |
700 | return(retval); |
| 713 | } |
701 | } |
| Line -... | Line 702... | ||
| - | 702 | ||
| - | 703 | // load actual point list from SD card |
|
| - | 704 | u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store) |
|
| - | 705 | { |
|
| - | 706 | u8 filename[30]; |
|
| - | 707 | ||
| - | 708 | pWPL_Store->Name[0] = 0; // clear current list name |
|
| - | 709 | ||
| - | 710 | // user absolute path, i.e. leading / |
|
| - | 711 | if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root |
|
| - | 712 | { |
|
| - | 713 | sprintf(filename, "/default.wpl"); |
|
| - | 714 | } |
|
| - | 715 | else |
|
| - | 716 | { |
|
| - | 717 | sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index); |
|
| - | 718 | } |
|
| - | 719 | return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name)); |
|
| - | 720 | } |
|
| - | 721 | ||
| - | 722 | // save actual point list to SD card |
|
| - | 723 | u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store) |
|
| - | 724 | { |
|
| - | 725 | u8 filename[30]; |
|
| - | 726 | ||
| - | 727 | ||
| - | 728 | if(PointCount == 0) return(WPL_NO_WAYPOINTS); |
|
| - | 729 | // user absolute path, i.e. leading / |
|
| - | 730 | if(pWPL_Store->Index == 0) |
|
| - | 731 | { |
|
| - | 732 | sprintf(filename, "/default.wpl"); |
|
| - | 733 | } |
|
| - | 734 | else |
|
| - | 735 | { |
|
| - | 736 | sprintf(filename, "/WPL/list_%03d.wpl", pWPL_Store->Index); |
|
| - | 737 | } |
|
| - | 738 | return PointList_Save(filename, pWPL_Store->Name, pWPL_Store->OverwriteFile); |
|
| - | 739 | } |
|
| - | 740 | ||
| - | 741 | ||
| - | 742 | // save actual gps positiin and heading to file |
|
| - | 743 | u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store) |
|
| - | 744 | { |
|
| - | 745 | u8 retval = WPL_ERROR; |
|
| - | 746 | u8 filename[30]; |
|
| - | 747 | Point_t WP; |
|
| - | 748 | ||
| - | 749 | if(GPSData.Position.Status != INVALID) return(retval); |
|
| - | 750 | ||
| - | 751 | // clear current point list |
|
| - | 752 | PointList_Clear(); |
|
| - | 753 | // prepare WP at current position |
|
| - | 754 | GPSPos_Copy(&GPSData.Position, &(WP.Position)); |
|
| - | 755 | // set heading |
|
| - | 756 | WP.Heading = CompassSetpointCorrected/10; |
|
| - | 757 | if(WP.Heading == 0) WP.Heading = 360; |
|
| - | 758 | WP.ToleranceRadius = 0; |
|
| - | 759 | WP.HoldTime = 0; |
|
| - | 760 | WP.Index = 1; |
|
| - | 761 | WP.Type = POINT_TYPE_WP; |
|
| - | 762 | WP.WP_EventChannelValue = 0; |
|
| - | 763 | WP.AltitudeRate = 0; |
|
| - | 764 | WP.Speed = 0; |
|
| - | 765 | WP.CamAngle = 0; |
|
| - | 766 | WP.Name[0] = 'P'; |
|
| - | 767 | WP.Name[1] = 0; |
|
| - | 768 | // add this point to wp list |
|
| - | 769 | PointList_SetAt(&WP); |
|
| - | 770 | ||
| - | 771 | sprintf(filename, "/WPL/point_%03d.wpl", pWPL_Store->Index); |
|
| - | 772 | sprintf(pWPL_Store->Name, "POINT%03d", pWPL_Store->Index); |
|
| - | 773 | retval = PointList_Save(filename, pWPL_Store->Name, 1); |
|
| - | 774 | return(retval); |
|
| - | 775 | } |
|
| - | 776 | // load target gps posititon and heading from file |
|
| - | 777 | u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store) |
|
| - | 778 | { |
|
| - | 779 | u8 filename[30]; |
|
| - | 780 | ||
| - | 781 | sprintf(filename, "/WPL/point_%03d.wpl", pWPL_Store->Index); |
|
| - | 782 | pWPL_Store->Name[0] = 0; // clear current list name |
|
| - | 783 | return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name)); |
|
| - | 784 | } |
|
| - | 785 | ||
| - | 786 | ||
| 714 | 787 | ||
| 715 | void ClearWLP_Name(void) |
788 | void ClearWLP_Name(void) |
| 716 | { |
789 | { |
| 717 | unsigned char i; |
790 | u8 i; |
| 718 | for(i=0; i<sizeof(WPL_Store.Name);i++) WPL_Store.Name[i] = 0; |
791 | for(i=0; i<sizeof(WPL_Store.Name);i++) WPL_Store.Name[i] = 0; |
| 719 | NewWPL_Name = 1; |
792 | NewWPL_Name = 1; |
| - | 793 | } |
|
| 720 | } |
794 | |
| 721 | // move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards |
795 | // move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards |
| 722 | u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle) |
796 | u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle) |
| 723 | { |
797 | { |
| - | 798 | u8 retval = 0; |
|
| 724 | u8 retval = 0; |
799 | s32 altitude; |
| 725 | GPS_Pos_t FirstPoint; |
800 | GPS_Pos_t OldRefPos; |
| Line 726... | Line 801... | ||
| 726 | GPS_Pos_Deviation_t RefDeviation; |
801 | GPS_Pos_Deviation_t RefDeviation; |
| 727 | 802 | ||
| 728 | // check inputs for plausibility; |
803 | // check inputs for plausibility; |
| 729 | if((RefIndex == 0) || (RefIndex > PointCount)) return(retval); |
804 | if((RefIndex == 0) || (RefIndex > PointCount)) return(retval); |
| Line 730... | Line -... | ||
| 730 | if(pRefPos == NULL) return(retval); |
- | |
| 731 | if(pRefPos->Status == INVALID) return(retval); |
805 | if(pRefPos == NULL) return(retval); |
| 732 | 806 | if(pRefPos->Status == INVALID) return(retval); |
|
| 733 | // try to copy the old reference in point list to a local buffer |
807 | |
| 734 | if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &FirstPoint)) |
808 | if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &OldRefPos)) // backup old reference position |
| 735 | { |
809 | { |
| 736 | u8 i; |
810 | u8 i; |
| 737 | // for each point position in the list |
811 | // iterate the position list |
| 738 | for(i = 0; i < PointCount; i++) |
812 | for(i = 0; i < PointCount; i++) |
| 739 | { |
813 | { |
| 740 | retval = 0; |
814 | retval = 0; |
| 741 | // Save altitude of that point |
815 | // backup altitude of this point |
| 742 | pRefPos->Altitude = PointList[i].Position.Altitude; |
816 | altitude = PointList[i].Position.Altitude; |
| 743 | // calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position |
817 | // calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position |
| - | 818 | if(!GPSPos_Deviation(&(PointList[i].Position), &OldRefPos, &RefDeviation)) break; |
|
| - | 819 | // copy of the new reference position into this list place |
|
| 744 | if(!GPSPos_Deviation(&(PointList[i].Position), &FirstPoint, &RefDeviation)) break; |
820 | if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break; |
| 745 | // copy of the new reference position into this list place |
- | |
| 746 | if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break; |
821 | // restore former altitude |
| 747 | // move new reference according to the deviation of the old reference |
822 | PointList[i].Position.Altitude = altitude; |
| 748 | retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East); |
823 | // move new reference according to the deviation of the old reference |
| 749 | if(!retval) break; |
824 | if(RotationAngle > 0) |
| 750 | } |
- | |
| 751 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
825 | { |
| 752 | // Now rotate around the reference point |
826 | retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360, RefDeviation.Distance ); |
| 753 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 754 | if(RotationAngle > 0) |
827 | // Now rotate the heading positions if they are fixed angles |
| 755 | { |
828 | if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360; |
| 756 | GPSPos_Copy(&(PointList[RefIndex-1].Position), &FirstPoint); // Rotate around the reference point |
- | |
| 757 | for(i = 0; i < PointCount; i++) |
- | |
| 758 | { |
- | |
| 759 | retval = 0; |
- | |
| 760 | // Save altitude of that point |
- | |
| 761 | pRefPos->Altitude = PointList[i].Position.Altitude; |
- | |
| 762 | // calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position |
- | |
| 763 | if(!GPSPos_Deviation(&(PointList[i].Position), &FirstPoint, &RefDeviation)) break; |
829 | } |
| 764 | // copy of the new reference position into this list place |
830 | else // no rotation |
| 765 | if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break; |
- | |
| 766 | // move new reference according to the deviation of the old reference |
- | |
| - | 831 | { |
|
| 767 | retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360,RefDeviation.Distance ); |
832 | // move new reference according to the deviation of the old reference |
| 768 | // Now rotate the heading positions if they are fixed angles |
833 | retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East); |
| 769 | if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360; |
- | |
| 770 | if(!retval) break; |
- | |
| 771 | } |
834 | } |
| 772 | } |
835 | if(!retval) break; |
| 773 | // ++++++++++++++++++++++++++++++++++++++++++++++ |
836 | } |
| 774 | } // else ref pos old not copied! |
837 | } // else ref pos old not copied! |