Subversion Repositories NaviCtrl

Rev

Rev 498 | Rev 504 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 498 Rev 503
Line 707... Line 707...
707 
        }      
 707 
        }      
708 
        return(retval);
 708 
        return(retval);
709 
}
 709 
}
Line 710... Line 710...
710 
 
 710 
 
711 
// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards
 711 
// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards
712 
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos)
 712 
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle)
713 
{
 713 
{
714 
        u8 retval = 0;
 714 
        u8 retval = 0;
715 
        GPS_Pos_t RefPos_old;
 715 
        GPS_Pos_t FirstPoint;
Line 716... Line 716...
716 
        GPS_Pos_Deviation_t RefDeviation;
 716 
        GPS_Pos_Deviation_t RefDeviation;
717 
         
 717 
         
718 
        // check inputs for plausibility;
 718 
        // check inputs for plausibility;
719 
        if((RefIndex == 0) || (RefIndex > PointCount)) return(retval); 
 719 
        if((RefIndex == 0) || (RefIndex > PointCount)) return(retval); 
Line 720... Line 720...
720 
        if(pRefPos == NULL) return(retval);
 720 
        if(pRefPos == NULL) return(retval);
721 
        if(pRefPos->Status == INVALID) return(retval);
 721 
        if(pRefPos->Status == INVALID) return(retval);
722 
 
 722 
 
723 
        // try to copy the old reference in point list to a local buffer
 723 
        // try to copy the old reference in point list to a local buffer
724 
        if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &RefPos_old))
 724 
        if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &FirstPoint))
725 
        {
 725 
        {
726 
                u8 i;
 726 
                u8 i;
727 
                // for each point position in the list
 727 
                // for each point position in the list
- 
 
 728 
                for(i = 0; i < PointCount; i++)
- 
 
 729 
                {
728 
                for(i = 0; i < PointCount; i++)
 730 
                        retval = 0;
729 
                {
 731 
                        // Save altitude of that point
730 
                        retval = 0;
 732 
                        pRefPos->Altitude = PointList[i].Position.Altitude;
731 
                    // calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position
 733 
                    // calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position
732 
                        if(!GPSPos_Deviation(&(PointList[i].Position), &RefPos_old, &RefDeviation)) break;
 734 
                        if(!GPSPos_Deviation(&(PointList[i].Position), &FirstPoint, &RefDeviation)) break;
733 
                        // copy of the new reference position into this list place
 735 
                        // copy of the new reference position into this list place
734 
                        if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
 736 
                        if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
735 
                        // move new reference according to the deviation of the old reference
 737 
                        // move new reference according to the deviation of the old reference
- 
 
 738 
                        retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East);
- 
 
 739 
                        if(!retval) break;             
- 
 
 740 
                }
- 
 
 741 
// ++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 742 
// Now rotate around the reference point
- 
 
 743 
// ++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 744 
          if(RotationAngle > 0)
- 
 
 745 
          {
- 
 
 746 
                GPSPos_Copy(&(PointList[RefIndex-1].Position), &FirstPoint); // Rotate around the reference point
- 
 
 747 
                for(i = 0; i < PointCount; i++)
- 
 
 748 
                {
- 
 
 749 
                        retval = 0;
- 
 
 750 
                        // Save altitude of that point
- 
 
 751 
                        pRefPos->Altitude = PointList[i].Position.Altitude;
- 
 
 752 
                    // calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position
- 
 
 753 
                        if(!GPSPos_Deviation(&(PointList[i].Position), &FirstPoint, &RefDeviation)) break;
- 
 
 754 
                        // copy of the new reference position into this list place
- 
 
 755 
                        if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
- 
 
 756 
                        // move new reference according to the deviation of the old reference
- 
 
 757 
                        retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360,RefDeviation.Distance );
- 
 
 758 
                        // Now rotate the heading positions if they are fixed angles
- 
 
 759 
                        if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360;
- 
 
 760 
                        if(!retval) break;             
736 
                        retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East);
 761 
                }
737 
                        if(!retval) break;             
 762 
          }
738 
                }
 763 
// ++++++++++++++++++++++++++++++++++++++++++++++
739 
        } // else ref pos old not copied!
 764 
        } // else ref pos old not copied!