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! |