WebSVN - NaviCtrl - Diff - Rev 89 and 92

         float P;
         float I;
         float D;
         float A;
         float ACC;
-        s32 P_Limit;
+        u32 P_Limit;
-        s32 I_Limit;
+        u32 I_Limit;
-        s32 D_Limit;
+        u32 D_Limit;
-        s32 PID_Limit;
+        u32 PID_Limit;
         u32 BrakingDuration;
         u8 MinSat;
         s8 StickThreshold;
         float WindCorrection;
         float SpeedCompensation;
-        s32 OperatingRadius;
+        u32 OperatingRadius;
         GPS_FlightMode_t  FlightMode;
 } __attribute__((packed)) GPS_Parameter_t;
 typedef struct
+{
         u8  Status; // invalid, newdata, processed
         s32 North;              // in cm
         s32 East;               // in cm
         s32 Bearing;    // in deg
-        s32 Distance;   // in cm
+        u32 Distance;   // in cm
 } __attribute__((packed)) GPS_Deviation_t;
 GPS_Deviation_t CurrentTargetDeviation;         // Deviation from Target
 GPS_Deviation_t CurrentHomeDeviation;           // Deviation from Home
 // the gps reference positions
 GPS_Pos_t GPS_HoldPosition      = {0,0,0, INVALID};                     // the hold position
 GPS_Pos_t GPS_HomePosition      = {0,0,0, INVALID};                     // the home position
+GPS_Pos_t * GPS_pTargetPosition = NULL;                             // pointer to the actual target position
-GPS_Pos_t * GPS_pTargetPosition = NULL;                             // pointer to the actual target position
+u32 GPS_TargetRadius = 0;                                                               // catch radius for target area
 Waypoint_t* GPS_pWaypoint = NULL;                                               // pointer to the actual waypoint
 //-------------------------------------------------------------
+        {
                 // update parameter from FC
                 if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
                 else
+                {
-                        if     (Parameter.NaviGpsModeControl <  50) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID;
+                        if     (Parameter.NaviGpsModeControl <  50) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
-                        else if(Parameter.NaviGpsModeControl < 180) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
+                        else if(Parameter.NaviGpsModeControl < 180) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID;
                         else                                                    GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT;
+                }
                 GPS_Parameter.Gain      = (float)Parameter.NaviGpsGain;
                 GPS_Parameter.P         = (float)Parameter.NaviGpsP;
                 GPS_Parameter.I         = (float)Parameter.NaviGpsI;
                 GPS_Parameter.D         = (float)Parameter.NaviGpsD;
                 GPS_Parameter.A         = (float)Parameter.NaviGpsD;
                 GPS_Parameter.ACC       = (float)Parameter.NaviGpsACC;
-                GPS_Parameter.P_Limit = (s32)Parameter.NaviGpsPLimit;
+                GPS_Parameter.P_Limit = (u32)Parameter.NaviGpsPLimit;
-                GPS_Parameter.I_Limit = (s32)Parameter.NaviGpsILimit;
+                GPS_Parameter.I_Limit = (u32)Parameter.NaviGpsILimit;
-                GPS_Parameter.D_Limit = (s32)Parameter.NaviGpsDLimit;
+                GPS_Parameter.D_Limit = (u32)Parameter.NaviGpsDLimit;
-                GPS_Parameter.PID_Limit = 2* (s32)Parameter.NaviAngleLimitation;
+                GPS_Parameter.PID_Limit = 2* (u32)Parameter.NaviAngleLimitation;
                 GPS_Parameter.BrakingDuration = (u32)Parameter.NaviPH_LoginTime;
                 GPS_Parameter.SpeedCompensation = (float)Parameter.NaviSpeedCompensation;
                 GPS_Parameter.MinSat = (u8)Parameter.NaviGpsMinSat;
                 GPS_Parameter.StickThreshold = (s8)Parameter.NaviStickThreshold;
                 GPS_Parameter.WindCorrection = (float)Parameter.NaviWindCorrection;
-                GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm
+                GPS_Parameter.OperatingRadius = (u32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm
+        }
         // FlightMode changed?
         if(GPS_Parameter.FlightMode != FlightMode_Old) BeepTime = 100; // beep to indicate that mode has switched
         FlightMode_Old = GPS_Parameter.FlightMode;
+}
+}
 //------------------------------------------------------------
 // Rescale xy-vector length if length limit is violated
-void GPS_LimitXY(s32 *x, s32 *y, s32 limit)
+void GPS_LimitXY(s32 *x, s32 *y, u32 limit)
 {
-        s32 dist;
+        u32 dist;
-        dist = (s32)hypot(*x,*y);       // the length of the vector
+        dist = (u32)hypot(*x,*y);       // the length of the vector
         if (dist == 0)
+        {
                 *x = 0;
         pDeviationFromTarget->North = (s32)(11119492.7f * temp2);
         pDeviationFromTarget->East  = (s32)(11119492.7f * temp1);
         // If the position deviation is small enough to neglect the earth curvature
         // (this is for our application always fulfilled) the distance to target
         // can be calculated by the pythagoras of north and east deviation.
-        pDeviationFromTarget->Distance = (s32)(11119492.7f * hypot(temp1, temp2));
+        pDeviationFromTarget->Distance = (u32)(11119492.7f * hypot(temp1, temp2));
         if (pDeviationFromTarget->Distance == 0L) pDeviationFromTarget->Bearing = 0L;
         else pDeviationFromTarget->Bearing = DirectionToTarget_N_E(temp2, temp1);
         pDeviationFromTarget->Status = NEWDATA;
         return TRUE;
         // pointer to current target position
         static GPS_Pos_t * pTargetPositionOld = NULL;
         static Waypoint_t* GPS_pWaypointOld = NULL;
         static GPS_Pos_t RangedTargetPosition = {0,0,0, INVALID};               // the limited target position, this is derived from the target position with repect to the operating radius
-        static s32 OperatingRadiusOld = -1;
-        static u8 WPArrived = FALSE;
+        static u32 OperatingRadiusOld = -1;
         static u32 WPTime = 0;
         //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                         // wait maximum of 3 times the normal data update time before data timemout
                         GPSDataTimeout = SetDelay(3 * GPS_UPDATETIME_MS);
                         beep_rythm++;
                         // debug
-                        DebugOut.Analog[21] = (u16)GPSData.Speed_North;
+                        DebugOut.Analog[21] = (s16)GPSData.Speed_North;
-                        DebugOut.Analog[22] = (u16)GPSData.Speed_East;
+                        DebugOut.Analog[22] = (s16)GPSData.Speed_East;
-                        DebugOut.Analog[31] = (u16)GPSData.NumOfSats;
+                        DebugOut.Analog[31] = (s16)GPSData.NumOfSats;
                         // If GPS signal condition is sufficient for a reliable position measurement
                         if(GPS_IsSignalOK())
                                 // check for current flight mode and set the target pointer GPS_pTargetPosition respectively
                                 switch(GPS_Parameter.FlightMode)
+                                {
                                         // the GPS control is deactived
                                         case GPS_FLIGHT_MODE_FREE:
-                                                NaviData.NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH);
+                                                NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH);
-                                                NaviData.NCFlags |= NC_FLAG_FREE;
+                                                NCFlags |= NC_FLAG_FREE;
                                                 GPS_Parameter.PID_Limit = 0; // disables PID output
                                                 // update hold position
                                                 GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
                                                 // no target position
                                                 GPS_pTargetPosition = NULL;
+                                                GPS_TargetRadius = 0;
                                                 break;
                                         // the GPS supports the position hold, if the pilot takes no action
                                         case GPS_FLIGHT_MODE_AID:
-                                                NaviData.NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_CH);
+                                                NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_CH);
-                                                NaviData.NCFlags |= NC_FLAG_PH;
+                                                NCFlags |= NC_FLAG_PH;
                                                 // reset WPList to begin
                                                 GPS_pWaypoint = WPList_Begin();
                                                 if(GPS_IsManuallyControlled())
+                                                {
                                                         GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active
+                                                    GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
-                                                    GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
+                                                        GPS_pTargetPosition = NULL;
-                                                        GPS_pTargetPosition = NULL;
+                                                        GPS_TargetRadius = 0;
+                                                }
                                                 else
+                                                {
                                                         GPS_pTargetPosition = &GPS_HoldPosition;
-                                                        GPS_pTargetPosition = &GPS_HoldPosition;
+                                                        GPS_TargetRadius = 100; // 1 meter
+                                                }
                                                 break;
                                         // the GPS control is directed to a target position
                                         // given by a waypoint or by the home position
                                         case GPS_FLIGHT_MODE_WAYPOINT:
-                                                NaviData.NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH);
+                                                NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH);
-                                                NaviData.NCFlags |= NC_FLAG_CH;
+                                                NCFlags |= NC_FLAG_CH;
                                                 if(GPS_IsManuallyControlled()) // the human pilot takes the action
+                                                {
                                                         GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active
-                                                        GPS_Parameter.PID_Limit = 0; // disables PID output, as long as the manual conrol is active
+                                                    GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);  // update hold position
-                                                    GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);  // update hold position
+                                                        GPS_pTargetPosition = NULL;     // set target position invalid
-                                                        GPS_pTargetPosition = NULL;     // set target position invalid
+                                                        GPS_TargetRadius = 0;
+                                                }
                                                 else // no manual control  -> gps position hold active
+                                                {
                                                         // waypoint trigger logic
                                                         if(GPS_pWaypoint != NULL) // waypoint exist
-                                                        {
                                                                 if(GPS_pWaypoint->Position.Status == INVALID) // should never happen
                                                                 if(GPS_pWaypoint->Position.Status == INVALID) // should never happen
                                                                         GPS_pWaypoint = WPList_Next(); // goto to next WP
-                                                                        WPArrived = FALSE;
+                                                                        GPS_pWaypoint = WPList_Next(); // goto to next WP
                                                                         BeepTime = 255;
+                                                                }
                                                                 else // waypoint position is valid
+                                                                {
-                                                                        // check if the pointer to the waypoint has been changed or the data have been updated
-                                                                        if((GPS_pWaypoint != GPS_pWaypointOld) || (GPS_pWaypoint->Position.Status == NEWDATA))
-                                                                        {
+                                                                        // check if the pointer to the waypoint has been changed or the data have been updated
-                                                                                GPS_pWaypointOld = GPS_pWaypoint;
-                                                                                // reset the arrived bit to break a pending HoldTime of the old WP
-                                                                                WPArrived = FALSE;
-                                                                        }
-                                                                        if(CurrentTargetDeviation.Status != INVALID)
-                                                                        {       // if the waypoint was not catched and the target area has been reached
-                                                                                if(!WPArrived && (CurrentTargetDeviation.Distance < (GPS_pWaypoint->ToleranceRadius * 100)))
-                                                                                {
-                                                                                        WPArrived = TRUE;
+                                                                        if((GPS_pWaypoint != GPS_pWaypointOld) || (GPS_pWaypoint->Position.Status == NEWDATA))
-                                                                                        WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp
+                                                                        {
                                                                                 GPS_pWaypointOld = GPS_pWaypoint;
+                                                                        }
                                                                         // if WP has been reached once, wait hold time before trigger to next one
-                                                                        if(WPArrived)
+                                                                        if(NCFlags & NC_FLAG_TARGET_REACHED)
+                                                                        {
-                                                                                /* ToDo: Adjust GPS_pWaypoint->Heading, GPS_pWaypoint->Event handling */
-                                                                                if(CheckDelay(WPTime))
+                                                                                /* ToDo: Adjust GPS_pWaypoint->Heading, GPS_pWaypoint->Event handling */
                                                                                 if(CheckDelay(WPTime))
+                                                                                {
+                                                                                        GPS_pWaypoint = WPList_Next(); // goto to next waypoint, return NULL if end of list has been reached
+                                                                                }
+                                                                        } // EOF if(WPArrived)
-                                                                                        GPS_pWaypoint = WPList_Next(); // goto to next waypoint, return NULL if end of list has been reached
+                                                                        else
                                                                                         WPArrived = FALSE; // which is not arrived
                                                                                 WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp
                                                                         } // EOF if(WPArrived)
+                                                                }
                                                         } // EOF waypoint trigger logic
+                                                        if(GPS_pWaypoint != NULL) // Waypoint exist
+                                                        {
-                                                        if(GPS_pWaypoint != NULL) // Waypoint exist
+                                                                // update the hold position
                                                                 GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
-                                                                // update the hold position
+                                                                GPS_pTargetPosition = &(GPS_pWaypoint->Position);
-                                                                GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
+                                                                GPS_TargetRadius = (u32)(GPS_pWaypoint->ToleranceRadius) * 100L;
                                                                 GPS_pTargetPosition = &(GPS_pWaypoint->Position);
+                                                        }
                                                         else // no waypoint info available, i.e. the WPList is empty or the end of the list has been reached
+                                                        {
                                                                 // fly back to home postion
-                                                                // fly back to home postion
+                                                                if(GPS_HomePosition.Status == INVALID)
                                                                 if(GPS_HomePosition.Status == INVALID)
                                                                         GPS_pTargetPosition = &GPS_HoldPosition; // fall back to hold mode if home position is not available
-                                                                        GPS_pTargetPosition = &GPS_HoldPosition; // fall back to hold mode if home position is not available
+                                                                        GPS_TargetRadius = 100;
                                                                         BeepTime = 255; // beep to indicate missin home position
+                                                                }
                                                                 else // the home position is valid
+                                                                {
                                                                         // update the hold position
-                                                                        // update the hold position
+                                                                        GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
-                                                                        GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
+                                                                        // set target to home position
-                                                                        // set target to home position
+                                                                        GPS_pTargetPosition = &GPS_HomePosition;
                                                 GPS_Parameter.PID_Limit = 0; // disables PID output
                                                 // update hold position
                                                 GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition);
                                                 // no target position
                                                 GPS_pTargetPosition = NULL;
+                                                GPS_TargetRadius = 0;
                                                 break;
                                 else
                                 {       // if the target position has been changed or the value has been updated or the OperatingRadius has changed
                                         if((GPS_pTargetPosition != pTargetPositionOld)  || (GPS_pTargetPosition->Status == NEWDATA) || (GPS_Parameter.OperatingRadius != OperatingRadiusOld) )
+                                        {
                                                 BeepTime = 255; // beep to indicate setting of a new target position
+                                                NCFlags &= ~NC_FLAG_TARGET_REACHED;     // clear target reached flag
                                                 // calculate deviation of new target position from home position
                                                 if(GPS_CalculateDeviation(GPS_pTargetPosition, &GPS_HomePosition, &TargetHomeDeviation))
+                                                {
                                                         // check distance from home position
                                                         if(TargetHomeDeviation.Distance > GPS_Parameter.OperatingRadius)
+                                                        {
                                                                 //calculate ranged target position to be within the operation radius area
-                                                                NaviData.NCFlags |= NC_FLAG_RANGE_LIMIT;
+                                                                NCFlags |= NC_FLAG_RANGE_LIMIT;
                                                                 TargetHomeDeviation.North *= GPS_Parameter.OperatingRadius;
                                                                 TargetHomeDeviation.North /= TargetHomeDeviation.Distance;
                                                                 TargetHomeDeviation.East *= GPS_Parameter.OperatingRadius;
+                                                        }
                                                         else
                                                         {       // the target is located within the operation radius area
                                                                 // simple copy the loaction to the ranged target position
                                                                 GPS_CopyPosition(GPS_pTargetPosition, &RangedTargetPosition);
-                                                                NaviData.NCFlags &= ~NC_FLAG_RANGE_LIMIT;
+                                                                NCFlags &= ~NC_FLAG_RANGE_LIMIT;
+                                                        }
+                                                }
                                                 else
                                                 {       // deviation could not be determined
                                                         GPS_ClearPosition(&RangedTargetPosition);
                                 /* Calculate position deviation from ranged target */
+                                // calculate deviation of current position to ranged target position in cm
+                                if(GPS_CalculateDeviation(&(GPSData.Position), &RangedTargetPosition, &CurrentTargetDeviation))
-                                // calculate deviation of current position to ranged target position in cm
+                                {       // set target reached flag of we once reached the target point
+                                        if(!(NCFlags |= NC_FLAG_TARGET_REACHED) && (CurrentTargetDeviation.Distance < GPS_TargetRadius))
+                                        {
-                                if(GPS_CalculateDeviation(&(GPSData.Position), &RangedTargetPosition, &CurrentTargetDeviation))
+                                                NCFlags |= NC_FLAG_TARGET_REACHED;      // set target reached flag
+                                        }
                                         // implement your control code here based
                                         // in the info available in the CurrentTargetDeviation, GPSData and FromFlightCtrl.GyroHeading
                                         GPS_Stick.Nick = 0;
         NaviData.GroundSpeed = (u16)GPSData.Speed_Ground;
         NaviData.Heading = (s16)(GPSData.Heading/100000L);
         NaviData.CompassHeading = (s16)FromFlightCtrl.GyroHeading/10; // in deg
         NaviData.AngleNick = FromFlightCtrl.AngleNick / 10;        // in deg
         NaviData.AngleRoll = FromFlightCtrl.AngleRoll / 10;        // in deg
-        NaviData.RC_Quality = (u8) FC.RC_Quality;
+        NaviData.RC_Quality = (u8)FC.RC_Quality;
         NaviData.MKFlags = (u8)FC.MKFlags;
+        NaviData.NCFlags = NCFlags;
         NaviData.OperatingRadius = Parameter.NaviOperatingRadius;
         NaviData.TopSpeed = (s16)GPSData.Speed_Top;  // in cm/s
+        NaviData.TargetHoldTime = (u16)GetDelay(WPTime);
         //+++++++++++++++++++++++++++++++++++++++++++++++++++
         return;
+}

Subversion Repositories NaviCtrl

(root)/tags/V0.30a/GPS.c @ 811 - Rev 89 → 92