Rev 1 | Rev 52 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1 | Rev 41 | ||
---|---|---|---|
Line 1... | Line 1... | ||
1 | /*#######################################################################################*/ |
1 | /*#######################################################################################*/ |
- | 2 | /*#######################################################################################*/ |
|
- | 3 | ||
- | 4 | // IMPORTANT NOTE: |
|
- | 5 | ||
- | 6 | // This is only a dummy implementation for errorfree compiling of the NaviCtrl sources. |
|
- | 7 | ||
- | 8 | // The GPS navigation routines are NOT included ! |
|
- | 9 | ||
- | 10 | /*#######################################################################################*/ |
|
- | 11 | /*#######################################################################################*/ |
|
2 | /* !!! THIS IS NOT FREE SOFTWARE !!! */ |
12 | /* !!! THIS IS NOT FREE SOFTWARE !!! */ |
3 | /*#######################################################################################*/ |
13 | /*#######################################################################################*/ |
4 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
14 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
5 | // + Copyright (c) 2008 Ingo Busker, Holger Buss |
15 | // + Copyright (c) 2008 Ingo Busker, Holger Buss |
6 | // + Nur für den privaten Gebrauch |
16 | // + Nur für den privaten Gebrauch |
Line 52... | Line 62... | ||
52 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
62 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
53 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
63 | // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
54 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
64 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
55 | // + POSSIBILITY OF SUCH DAMAGE. |
65 | // + POSSIBILITY OF SUCH DAMAGE. |
56 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
66 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 67 | #include <stdio.h> |
|
- | 68 | #include <stdlib.h> |
|
- | 69 | #include <math.h> |
|
- | 70 | #include "91x_lib.h" |
|
57 | #include "main.h" |
71 | #include "main.h" |
- | 72 | #include "uart1.h" |
|
- | 73 | #include "GPS.h" |
|
- | 74 | #include "timer.h" |
|
- | 75 | #include "spi_slave.h" |
|
- | 76 | #include "waypoints.h" |
|
- | 77 | #include "i2c.h" |
|
- | 78 | ||
- | 79 | ||
- | 80 | #define M_PI_180 (M_PI / 180.0f) |
|
- | 81 | #define GPS_UPDATETIME_MS 200 // 200ms is 5 Hz |
|
- | 82 | typedef enum |
|
- | 83 | { |
|
- | 84 | GPS_FLIGHT_MODE_UNDEF, |
|
- | 85 | GPS_FLIGHT_MODE_FREE, |
|
- | 86 | GPS_FLIGHT_MODE_AID, |
|
- | 87 | GPS_FLIGHT_MODE_WAYPOINT |
|
- | 88 | } GPS_FlightMode_t; |
|
- | 89 | ||
- | 90 | typedef struct |
|
- | 91 | { |
|
- | 92 | float Gain; |
|
- | 93 | float P; |
|
- | 94 | float I; |
|
- | 95 | float D; |
|
- | 96 | float A; |
|
- | 97 | float ACC; |
|
- | 98 | u8 MinSat; |
|
- | 99 | s8 StickThreshold; |
|
- | 100 | float WindCorrection; |
|
- | 101 | s32 OperatingRadius; |
|
- | 102 | u32 NaviAngleLimitation; |
|
- | 103 | GPS_FlightMode_t FlightMode; |
|
- | 104 | } __attribute__((packed)) GPS_Parameter_t; |
|
- | 105 | ||
- | 106 | typedef struct |
|
- | 107 | { |
|
- | 108 | Status_t Status; // invalid, newdata, processed |
|
- | 109 | s32 North; // in cm |
|
- | 110 | s32 East; // in cm |
|
- | 111 | s32 Bearing; // in deg |
|
- | 112 | s32 Distance; // in cm |
|
- | 113 | } __attribute__((packed)) GPS_Deviation_t; |
|
- | 114 | GPS_Deviation_t TargetDeviation; |
|
- | 115 | ||
- | 116 | GPS_Stick_t GPS_Stick; |
|
- | 117 | GPS_Parameter_t GPS_Parameter; |
|
- | 118 | ||
- | 119 | // the gps reference positions |
|
- | 120 | GPS_Pos_t GPS_HoldPosition = {0,0,0, INVALID}; // the hold position |
|
- | 121 | GPS_Pos_t GPS_HomePosition = {0,0,0, INVALID}; // the home position |
|
- | 122 | GPS_Pos_t * GPS_pTargetPosition = NULL; // pointer to the actual target position |
|
- | 123 | Waypoint_t* GPS_pWaypoint = NULL; // pointer to the actual waypoint |
|
- | 124 | ||
- | 125 | //------------------------------------------------------------- |
|
- | 126 | // Update GPSParamter |
|
- | 127 | void GPS_UpdateParameter(void) |
|
- | 128 | { |
|
- | 129 | static GPS_FlightMode_t FlightMode_Old = GPS_FLIGHT_MODE_UNDEF; |
|
- | 130 | // in case of bad receiving conditions |
|
- | 131 | if(FC.RC_Quality < 100) |
|
- | 132 | { // set fixed parameter |
|
- | 133 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
|
- | 134 | GPS_Parameter.Gain = (float) 100; |
|
- | 135 | GPS_Parameter.P = (float) 90; |
|
- | 136 | GPS_Parameter.I = (float) 90; |
|
- | 137 | GPS_Parameter.D = (float) 90; |
|
- | 138 | GPS_Parameter.A = (float) 90; |
|
- | 139 | GPS_Parameter.ACC = (float) 0; |
|
- | 140 | GPS_Parameter.MinSat = 6; |
|
- | 141 | GPS_Parameter.StickThreshold = 8; |
|
- | 142 | GPS_Parameter.WindCorrection = 0.0; |
|
- | 143 | GPS_Parameter.OperatingRadius = 0; // forces the aircraft to fly to home positon |
|
- | 144 | GPS_Parameter.NaviAngleLimitation = 125; |
|
- | 145 | } |
|
- | 146 | else |
|
- | 147 | { |
|
- | 148 | // update parameter from FC |
|
- | 149 | if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
|
- | 150 | else |
|
- | 151 | { |
|
- | 152 | if (Parameter.NaviGpsModeControl < 50) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID; |
|
- | 153 | else if(Parameter.NaviGpsModeControl < 180) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
|
- | 154 | else GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT; |
|
- | 155 | } |
|
- | 156 | GPS_Parameter.Gain = (float)Parameter.NaviGpsGain; |
|
- | 157 | GPS_Parameter.P = (float)Parameter.NaviGpsP; |
|
- | 158 | GPS_Parameter.I = (float)Parameter.NaviGpsI; |
|
- | 159 | GPS_Parameter.D = (float)Parameter.NaviGpsD; |
|
- | 160 | GPS_Parameter.A = (float)Parameter.NaviGpsD; |
|
- | 161 | GPS_Parameter.ACC = (float)Parameter.NaviGpsACC; |
|
- | 162 | GPS_Parameter.MinSat = (u8)Parameter.NaviGpsMinSat; |
|
- | 163 | GPS_Parameter.StickThreshold = (s8)Parameter.NaviStickThreshold; |
|
- | 164 | GPS_Parameter.WindCorrection = (float)Parameter.NaviWindCorrection; |
|
- | 165 | GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm |
|
- | 166 | GPS_Parameter.NaviAngleLimitation = (u32) Parameter.NaviAngleLimitation * 2; |
|
- | 167 | } |
|
- | 168 | // FlightMode changed? |
|
- | 169 | if(GPS_Parameter.FlightMode != FlightMode_Old) BeepTime = 100; // beep to indicate that mode has switched |
|
- | 170 | FlightMode_Old = GPS_Parameter.FlightMode; |
|
- | 171 | } |
|
- | 172 | ||
- | 173 | //------------------------------------------------------------- |
|
- | 174 | // This function defines a good GPS signal condition |
|
- | 175 | u8 GPS_IsSignalOK(void) |
|
- | 176 | { |
|
- | 177 | if( (GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.NumOfSats >= GPS_Parameter.MinSat)) return(1); |
|
- | 178 | else return(0); |
|
- | 179 | } |
|
- | 180 | ||
- | 181 | //------------------------------------------------------------ |
|
- | 182 | // Checks for manual control action |
|
- | 183 | u8 GPS_IsManuallyControlled(void) |
|
- | 184 | { |
|
- | 185 | if( ( (abs(FC.StickNick) > GPS_Parameter.StickThreshold) || (abs(FC.StickRoll) > GPS_Parameter.StickThreshold)) && (GPS_Parameter.StickThreshold > 0)) return 1; |
|
- | 186 | else return 0; |
|
- | 187 | } |
|
- | 188 | ||
- | 189 | //------------------------------------------------------------ |
|
- | 190 | // copy GPS position from source position to target position |
|
- | 191 | u8 GPS_CopyPosition(GPS_Pos_t * pGPSPosSrc, GPS_Pos_t* pGPSPosTgt) |
|
- | 192 | { |
|
- | 193 | u8 retval = 0; |
|
- | 194 | if((pGPSPosSrc == NULL) || (pGPSPosTgt == NULL)) return(retval); // bad pointer |
|
- | 195 | // copy only valid positions |
|
- | 196 | if(pGPSPosSrc->Status != INVALID) |
|
- | 197 | { |
|
- | 198 | // if the source GPS position is not invalid |
|
- | 199 | pGPSPosTgt->Longitude = pGPSPosSrc->Longitude; |
|
- | 200 | pGPSPosTgt->Latitude = pGPSPosSrc->Latitude; |
|
- | 201 | pGPSPosTgt->Altitude = pGPSPosSrc->Altitude; |
|
- | 202 | pGPSPosTgt->Status = NEWDATA; // mark data in target position as new |
|
- | 203 | retval = 1; |
|
- | 204 | } |
|
- | 205 | return(retval); |
|
- | 206 | } |
|
Line -... | Line 207... | ||
- | 207 | ||
- | 208 | //------------------------------------------------------------ |
|
58 | 209 | // clear position data |
|
- | 210 | u8 GPS_ClearPosition(GPS_Pos_t * pGPSPos) |
|
- | 211 | { |
|
- | 212 | u8 retval = FALSE; |
|
- | 213 | if(pGPSPos == NULL) return(retval); // bad pointer |
|
- | 214 | else |
|
59 | u8 OsdBar; // Direction home for OSD |
215 | { |
- | 216 | pGPSPos->Longitude = 0; |
|
- | 217 | pGPSPos->Latitude = 0; |
|
- | 218 | pGPSPos->Altitude = 0; |
|
60 | s16 OsdDistance; // Distance home |
219 | pGPSPos->Status = INVALID; |
- | 220 | retval = TRUE; |
|
61 | s16 GPS_Nick; |
221 | } |
- | 222 | return (retval); |
|
Line 62... | Line -... | ||
62 | s16 GPS_Roll; |
- | |
Line 63... | Line 223... | ||
63 | 223 | } |
|
64 | struct str_GPSParameter GPSParameter; |
224 | |
- | 225 | ||
65 | 226 | //------------------------------------------------------------ |
|
66 | #define GPS_MODE_FREE 0 |
227 | void GPS_Neutral() |
67 | #define GPS_MODE_AID 1 |
228 | { |
- | 229 | GPS_Stick.Nick = 0; |
|
Line 68... | Line 230... | ||
68 | #define GPS_MODE_HOLD 2 |
230 | GPS_Stick.Roll = 0; |
69 | #define GPS_MODE_HOME 3 |
- | |
70 | u32 GpsFlightMode = GPS_MODE_FREE; |
231 | GPS_Stick.Yaw = 0; |
71 | 232 | } |
|
- | 233 | ||
- | 234 | //------------------------------------------------------------ |
|
72 | //------------------------------------------------------------ |
235 | void GPS_Init(void) |
- | 236 | { |
|
- | 237 | SerialPutString("\r\n GPS init..."); |
|
73 | // Init variables or send configuration to GPS module |
238 | UBX_Init(); |
74 | void GPS_Init(void) |
239 | GPS_Neutral(); |
- | 240 | GPS_ClearPosition(&GPS_HoldPosition); |
|
75 | { |
241 | GPS_ClearPosition(&GPS_HomePosition); |
- | 242 | GPS_pTargetPosition = NULL; |
|
- | 243 | WPList_Init(); |
|
Line 76... | Line 244... | ||
76 | GPS_Nick = 0; |
244 | GPS_pWaypoint = WPList_Begin(); |
77 | GPS_Roll = 0; |
245 | GPS_UpdateParameter(); |
78 | OsdDistance = 0; |
246 | SerialPutString("ok"); |
- | 247 | } |
|
79 | OsdBar = 0; |
248 | |
80 | 249 | //------------------------------------------------------------ |
|
81 | GPSParameter.P = 100; |
250 | // calculate the bearing to target position from its deviation |
82 | GPSParameter.I = 100; |
251 | s32 DirectionToTarget_N_E(float northdev, float eastdev) |
83 | GPSParameter.D = 100; |
252 | { |
- | 253 | s32 bearing; |
|
- | 254 | bearing = (s32)(atan2(northdev, eastdev) / M_PI_180); |
|
84 | GPSParameter.A = 100; |
255 | bearing = (270L - bearing)%360L; |
- | 256 | return(bearing); |
|
- | 257 | } |
|
- | 258 | ||
- | 259 | ||
- | 260 | //------------------------------------------------------------ |
|
- | 261 | // Rescale xy-vector length if length limit is violated |
|
- | 262 | void GPS_LimitXY(s32 *x, s32 *y, s32 limit) |
|
- | 263 | { |
|
- | 264 | s32 dist; |
|
- | 265 | dist = (s32)hypot(*x,*y); // the length of the vector |
|
- | 266 | if ((dist != 0L) && (dist > limit)) |
|
- | 267 | // if vector length is larger than the given limit |
|
Line -... | Line 268... | ||
- | 268 | { // scale vector compontents so that the length is cut off to limit |
|
- | 269 | *x = (*x * limit) / dist; |
|
85 | GPSParameter.ACC = 100; |
270 | *y = (*y * limit) / dist; |
86 | GPSParameter.ModeSchalter = 100; |
271 | } |
87 | GPSParameter.Amplification = 100;; |
272 | } |
- | 273 | ||
- | 274 | //------------------------------------------------------------ |
|
- | 275 | // transform the integer deg into float radians |
|
88 | } |
276 | inline double RadiansFromGPS(s32 deg) |
89 | //------------------------------------------------------------ |
- | |
90 | 277 | { |
|
91 | u8 Navigation(void) |
278 | return ((double)deg * 1e-7f * M_PI_180); // 1E-7 because deg is the value in ° * 1E7 |
92 | { |
279 | } |
93 | static char GpsFix = 0, NewGpsMode = 0; |
280 | |
Line -... | Line 281... | ||
- | 281 | //------------------------------------------------------------ |
|
94 | static u32 beep_rythm; |
282 | // transform the integer deg into float deg |
95 | u32 tmp_long; |
283 | inline double DegFromGPS(s32 deg) |
- | 284 | { |
|
- | 285 | return ((double)deg * 1e-7f); // 1E-7 because deg is the value in ° * 1E7 |
|
- | 286 | } |
|
96 | if(NewGPSDataAvail) // there are new data from gps module |
287 | |
- | 288 | //------------------------------------------------------------ |
|
- | 289 | // calculate the deviation from the current position to the target position |
|
- | 290 | u8 GPS_CalculateDeviation(GPS_Pos_t * pCurrentPos, GPS_Pos_t * pTargetPos) |
|
- | 291 | { |
|
97 | { |
292 | double temp1, temp2; |
98 | NewGPSDataAvail = 0; |
293 | // if given pointer is NULL |
99 | beep_rythm++; |
294 | if((pCurrentPos == NULL) || (pTargetPos == NULL)) goto baddata; |
100 | 295 | // if positions are invalid |
|
- | 296 | if((pCurrentPos->Status == INVALID) || (pTargetPos->Status == INVALID)) goto baddata; |
|
- | 297 | ||
101 | GPSParameter.ModeSchalter = Parameter_UserParam1; |
298 | // The deviation from the current to the target position along north and east direction is |
- | 299 | // simple the lat/lon difference. To convert that angular deviation into an |
|
- | 300 | // arc length the spherical projection has to be considered. |
|
- | 301 | // The mean earth radius is 6371km. Therfore the arc length per latitude degree |
|
- | 302 | // is always 6371km * 2 * Pi / 360deg = 111.2 km/deg. |
|
102 | GPSParameter.Amplification = (float) Parameter_UserParam2 / (100.0); |
303 | // The arc length per longitude degree depends on the correspondig latitude and |
- | 304 | // is 111.2km * cos(latitude). |
|
- | 305 | ||
- | 306 | // calculate the shortest longitude deviation from target |
|
- | 307 | temp1 = DegFromGPS(pCurrentPos->Longitude) - DegFromGPS(pTargetPos->Longitude); |
|
- | 308 | // outside an angular difference of -180 deg ... +180 deg its shorter to go the other way around |
|
- | 309 | // In our application we wont fly more than 20.000 km but along the date line this is important. |
|
- | 310 | if(temp1 > 180.0f) temp1 -= 360.0f; |
|
103 | GPSParameter.P = (float) Parameter_UserParam3; |
311 | else if (temp1 < -180.0f) temp1 += 360.0f; |
104 | GPSParameter.I = (float) Parameter_UserParam4; |
312 | temp1 *= cos(RadiansFromGPS(pTargetPos->Latitude)); |
- | 313 | // calculate latitude deviation from target |
|
- | 314 | // this is allways within -180 deg ... 180 deg |
|
- | 315 | temp2 = DegFromGPS(pCurrentPos->Latitude) - DegFromGPS(pTargetPos->Latitude); |
|
- | 316 | // deviation from target position in cm |
|
- | 317 | // i.e. the distance to walk from the target in northern and eastern direction to reach the current position |
|
- | 318 | ||
- | 319 | TargetDeviation.Status = INVALID; |
|
- | 320 | TargetDeviation.North = (s32)(11119492.7f * temp2); |
|
105 | GPSParameter.D = (float) Parameter_UserParam5; |
321 | TargetDeviation.East = (s32)(11119492.7f * temp1); |
- | 322 | // If the position deviation is small enough to neglect the earth curvature |
|
- | 323 | // (this is for our application always fulfilled) the distance to target |
|
- | 324 | // can be calculated by the pythagoras of north and east deviation. |
|
106 | GPSParameter.A = (float) Parameter_UserParam6; |
325 | TargetDeviation.Distance = (s32)(11119492.7f * hypot(temp1, temp2)); |
107 | GPSParameter.ACC = (float) Parameter_UserParam7; |
326 | if (TargetDeviation.Distance == 0L) TargetDeviation.Bearing = 0L; |
108 | 327 | else TargetDeviation.Bearing = DirectionToTarget_N_E(temp2, temp1); |
|
109 | if(SenderOkay < 100) // Empfangsausfall |
328 | TargetDeviation.Status = NEWDATA; |
110 | { |
329 | return TRUE; |
- | 330 | ||
111 | GPSParameter.ModeSchalter = 0; |
331 | baddata: |
Line -... | Line 332... | ||
- | 332 | TargetDeviation.North = 0L; |
|
- | 333 | TargetDeviation.East = 0L; |
|
- | 334 | TargetDeviation.Distance = 0L; |
|
- | 335 | TargetDeviation.Bearing = 0L; |
|
- | 336 | TargetDeviation.Status = INVALID; |
|
- | 337 | return FALSE; |
|
- | 338 | } |
|
- | 339 | ||
- | 340 | //------------------------------------------------------------ |
|
- | 341 | void GPS_Navigation(void) |
|
- | 342 | { |
|
- | 343 | static u32 beep_rythm; |
|
- | 344 | static u32 GPSDataTimeout = 0; |
|
- | 345 | ||
- | 346 | // pointer to current target position |
|
112 | GPSParameter.Amplification = 100; |
347 | static GPS_Pos_t * pTargetPositionOld = NULL; |
113 | GPSParameter.P = (float) 90; |
348 | static Waypoint_t* GPS_pWaypointOld = NULL; |
114 | GPSParameter.I = (float) 90; |
349 | |
- | 350 | 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 |
|
- | 351 | static s32 OperatingRadiusOld = -1; |
|
- | 352 | static u8 WPArrived = FALSE; |
|
- | 353 | static u32 WPTime = 0; |
|
- | 354 | ||
- | 355 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 356 | //+ Check for new data from GPS-receiver |
|
- | 357 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 358 | switch(GPSData.Status) |
|
- | 359 | { |
|
- | 360 | case INVALID: // no gps data available |
|
- | 361 | // do nothing |
|
- | 362 | break; |
|
- | 363 | ||
- | 364 | case PROCESSED: // the current data have been allready processed |
|
- | 365 | // if no new data are available within the timeout switch to invalid state. |
|
- | 366 | if(CheckDelay(GPSDataTimeout)) GPSData.Status = INVALID; |
|
- | 367 | // wait for new gps data |
|
- | 368 | break; |
|
- | 369 | ||
- | 370 | case NEWDATA: // handle new gps data |
|
- | 371 | ||
- | 372 | // update GPS Parameter from FC-Data via SPI interface |
|
- | 373 | GPS_UpdateParameter(); |
|
- | 374 | ||
- | 375 | // wait maximum of 3 times the normal data update time before data timemout |
|
- | 376 | GPSDataTimeout = SetDelay(3 * GPS_UPDATETIME_MS); |
|
- | 377 | beep_rythm++; |
|
- | 378 | ||
- | 379 | // debug |
|
- | 380 | DebugOut.Analog[21] = (u16)GPSData.Speed_North; |
|
- | 381 | DebugOut.Analog[22] = (u16)GPSData.Speed_East; |
|
- | 382 | DebugOut.Analog[31] = (u16)GPSData.NumOfSats; |
|
- | 383 | ||
- | 384 | // If GPS signal condition is sufficient for a reliable position measurement |
|
- | 385 | if(GPS_IsSignalOK()) |
|
- | 386 | { |
|
- | 387 | // if the MK is starting or the home position is invalid then store the home position |
|
- | 388 | if((FC.MKFlags & MKFLAG_START) || (GPS_HomePosition.Status == INVALID)) |
|
- | 389 | { // try to update the home position from the current position |
|
- | 390 | if(GPS_CopyPosition(&(GPSData.Position), &GPS_HomePosition)) |
|
- | 391 | { |
|
- | 392 | BeepTime = 700; // beep on success |
|
115 | GPSParameter.D = (float) 90; |
393 | GPS_CopyPosition(&GPS_HomePosition, &(NaviData.HomePosition)); |
- | 394 | } |
|
- | 395 | GPS_pWaypoint = WPList_Begin(); // go to start of waypoint list, return NULL of the list is empty |
|
- | 396 | } |
|
- | 397 | ||
- | 398 | /* The selected flight mode influences the target position pointer and therefore the behavior */ |
|
- | 399 | ||
- | 400 | // check for current flight mode and set the target pointer GPS_pTargetPosition respectively |
|
116 | GPSParameter.A = (float) 90; |
401 | switch(GPS_Parameter.FlightMode) |
- | 402 | { |
|
- | 403 | // the GPS control is deactived |
|
- | 404 | case GPS_FLIGHT_MODE_FREE: |
|
- | 405 | NaviData.NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH); |
|
- | 406 | NaviData.NCFlags |= NC_FLAG_FREE; |
|
- | 407 | ||
- | 408 | // update hold position |
|
- | 409 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
|
- | 410 | // no target position |
|
- | 411 | GPS_pTargetPosition = NULL; |
|
- | 412 | break; |
|
- | 413 | ||
- | 414 | // the GPS supports the position hold, if the pilot takes no action |
|
- | 415 | case GPS_FLIGHT_MODE_AID: |
|
- | 416 | NaviData.NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_CH); |
|
- | 417 | NaviData.NCFlags |= NC_FLAG_PH; |
|
117 | GPSParameter.ACC = (float) 90; |
418 | // reset WPList to begin |
118 | } |
- | |
119 | - | ||
120 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
121 | //+ GPS-Mode |
419 | GPS_pWaypoint = WPList_Begin(); |
122 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
420 | |
123 | tmp_long = GpsFlightMode; |
- | |
124 | if(GPSParameter.ModeSchalter < 20) GpsFlightMode = GPS_MODE_AID; |
421 | if(GPS_IsManuallyControlled()) |
125 | else |
- | |
- | 422 | { |
|
Line -... | Line 423... | ||
- | 423 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
|
- | 424 | GPS_pTargetPosition = NULL; |
|
- | 425 | } |
|
- | 426 | else |
|
- | 427 | { |
|
Line -... | Line 428... | ||
- | 428 | GPS_pTargetPosition = &GPS_HoldPosition; |
|
- | 429 | } |
|
- | 430 | break; |
|
126 | if(GPSParameter.ModeSchalter < 200) GpsFlightMode = GPS_MODE_FREE; |
431 | |
- | 432 | // the GPS control is directed to a target position |
|
- | 433 | // given by a waypoint or by the home position |
|
- | 434 | case GPS_FLIGHT_MODE_WAYPOINT: |
|
- | 435 | NaviData.NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH); |
|
- | 436 | NaviData.NCFlags |= NC_FLAG_CH; |
|
- | 437 | ||
127 | else GpsFlightMode = GPS_MODE_HOME; |
438 | // waypoint trigger logic |
128 | if(GpsFlightMode != tmp_long) // Mode changed |
439 | if(GPS_pWaypoint != NULL) // waypoint exist |
129 | { |
440 | { |
- | 441 | if(GPS_pWaypoint->Position.Status == INVALID) // should never happen |
|
- | 442 | { |
|
- | 443 | GPS_pWaypoint = WPList_Next(); // goto to next WP |
|
- | 444 | WPArrived = FALSE; |
|
- | 445 | BeepTime = 255; |
|
- | 446 | } |
|
- | 447 | else // waypoint position is valid |
|
- | 448 | { |
|
- | 449 | // check if the pointer to the waypoint has been changed or the data have been updated |
|
- | 450 | if((GPS_pWaypoint != GPS_pWaypointOld) || (GPS_pWaypoint->Position.Status == NEWDATA)) |
|
- | 451 | { |
|
- | 452 | GPS_pWaypointOld = GPS_pWaypoint; |
|
- | 453 | // reset the arrived bit to break a pending HoldTime of the old WP |
|
- | 454 | WPArrived = FALSE; |
|
- | 455 | } |
|
- | 456 | ||
- | 457 | if(TargetDeviation.Status != INVALID) |
|
- | 458 | { // if the waypoint was not catched and the target area has been reached |
|
- | 459 | if(!WPArrived && (TargetDeviation.Distance < (GPS_pWaypoint->ToleranceRadius * 100))) |
|
- | 460 | { |
|
- | 461 | WPArrived = TRUE; |
|
- | 462 | WPTime = SetDelay(GPS_pWaypoint->HoldTime * 1000); // set hold time stamp |
|
- | 463 | } |
|
- | 464 | } |
|
- | 465 | // if WP has been reached once, wait hold time before trigger to next one |
|
- | 466 | if(WPArrived) |
|
- | 467 | { |
|
- | 468 | /* ToDo: Adjust GPS_pWaypoint->Heading, GPS_pWaypoint->Event handling */ |
|
130 | BeepTime = 100; |
469 | if(CheckDelay(WPTime)) |
131 | NewGpsMode = 1; |
470 | { |
132 | } |
471 | GPS_pWaypoint = WPList_Next(); // goto to next waypoint, return NULL if end of list has been reached |
133 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
134 | 472 | WPArrived = FALSE; // which is not arrived |
|
135 | 473 | } |
|
- | 474 | } // EOF if(WPArrived) |
|
- | 475 | } |
|
- | 476 | } // EOF waypoint trigger logic |
|
- | 477 | ||
- | 478 | if(GPS_pWaypoint != NULL) // Waypoint exist |
|
- | 479 | { |
|
- | 480 | // update the hold position |
|
- | 481 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
|
- | 482 | GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
|
- | 483 | } |
|
- | 484 | else // no waypoint info available, i.e. the WPList is empty or the end of the list has been reached |
|
- | 485 | { |
|
- | 486 | // fly back to home postion |
|
- | 487 | if(GPS_HomePosition.Status == INVALID) |
|
- | 488 | { |
|
- | 489 | GPS_pTargetPosition = &GPS_HoldPosition; // fall back to hold mode if home position is not available |
|
- | 490 | BeepTime = 255; // beep to indicate missin home position |
|
- | 491 | } |
|
- | 492 | else // the home position is valid |
|
- | 493 | { |
|
- | 494 | // update the hold position |
|
- | 495 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
|
- | 496 | // set target to home position |
|
- | 497 | GPS_pTargetPosition = &GPS_HomePosition; |
|
- | 498 | } |
|
- | 499 | } |
|
- | 500 | break; |
|
- | 501 | ||
- | 502 | case GPS_FLIGHT_MODE_UNDEF: |
|
- | 503 | default: |
|
- | 504 | // update hold position |
|
- | 505 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
|
- | 506 | // no target position |
|
136 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
507 | GPS_pTargetPosition = NULL; |
- | 508 | break; |
|
- | 509 | ||
- | 510 | }// EOF GPS Mode Handling |
|
- | 511 | ||
- | 512 | ||
- | 513 | /* Calculation of range target based on the real target */ |
|
- | 514 | ||
- | 515 | // if no target position exist clear the ranged target position |
|
- | 516 | if(GPS_pTargetPosition == NULL) GPS_ClearPosition(&RangedTargetPosition); |
|
137 | //+ Fix okay |
517 | else |
- | 518 | { // if the target position has been changed or the value has been updated or the OperatingRadius has changed |
|
- | 519 | if((GPS_pTargetPosition != pTargetPositionOld) || (GPS_pTargetPosition->Status == NEWDATA) || (GPS_Parameter.OperatingRadius != OperatingRadiusOld) ) |
|
- | 520 | { |
|
- | 521 | // calculate deviation of new target position from home position |
|
- | 522 | if(GPS_CalculateDeviation(GPS_pTargetPosition, &GPS_HomePosition)) |
|
- | 523 | { |
|
- | 524 | // check distance from home position |
|
- | 525 | if(TargetDeviation.Distance > GPS_Parameter.OperatingRadius) |
|
- | 526 | { |
|
- | 527 | //calculate ranged target position to be within the operation radius area |
|
- | 528 | NaviData.NCFlags |= NC_FLAG_RANGE_LIMIT; |
|
- | 529 | TargetDeviation.Distance = GPS_Parameter.OperatingRadius; |
|
- | 530 | GPS_LimitXY(&(TargetDeviation.East), &(TargetDeviation.North), TargetDeviation.Distance); |
|
- | 531 | ||
- | 532 | RangedTargetPosition.Status = INVALID; |
|
- | 533 | RangedTargetPosition.Latitude = GPS_HomePosition.Latitude; |
|
- | 534 | RangedTargetPosition.Latitude += (s32)((float)TargetDeviation.North / 1.11194927f); |
|
- | 535 | RangedTargetPosition.Longitude = GPS_HomePosition.Longitude; |
|
- | 536 | RangedTargetPosition.Longitude += (s32)((float)TargetDeviation.East / (1.11194927f * cos(RadiansFromGPS(GPS_HomePosition.Latitude))) ); |
|
138 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
537 | RangedTargetPosition.Altitude = GPS_pTargetPosition->Altitude; |
- | 538 | RangedTargetPosition.Status = NEWDATA; |
|
- | 539 | } |
|
- | 540 | else |
|
- | 541 | { // the target is located within the operation radius area |
|
- | 542 | // simple copy the loaction to the ranged target position |
|
- | 543 | GPS_CopyPosition(GPS_pTargetPosition, &RangedTargetPosition); |
|
- | 544 | NaviData.NCFlags &= ~NC_FLAG_RANGE_LIMIT; |
|
- | 545 | } |
|
- | 546 | } |
|
- | 547 | else |
|
- | 548 | { // deviation could not be determined |
|
- | 549 | GPS_ClearPosition(&RangedTargetPosition); |
|
139 | if((GPS_Data.Flags & GPS_FIX) && ((GPS_Data.Used_Sat >= GPS_SAT_MIN) || GpsFix)) |
550 | } |
- | 551 | GPS_pTargetPosition->Status = PROCESSED; // mark current target as processed! |
|
140 | { |
552 | } |
141 | GpsFix = 1; // hysteresis |
553 | } |
- | 554 | OperatingRadiusOld = GPS_Parameter.OperatingRadius; |
|
142 | // here is a good place to put your GPS code... |
555 | // remember last target position pointer |
143 | GPS_Nick = 0; // do nothing |
556 | pTargetPositionOld = GPS_pTargetPosition; |
144 | GPS_Roll = 0; // do nothing |
557 | |
145 | } |
558 | /* Calculate position deviation from ranged target */ |
- | 559 | ||
- | 560 | // calculate deviation of current position to ranged target position in cm |
|
- | 561 | if(GPS_CalculateDeviation(&(GPSData.Position), &RangedTargetPosition)) |
|
146 | else |
562 | { |
- | 563 | // implement your control code here based |
|
- | 564 | // in the info available in the TargetDeviation, GPSData and FromFlightCtrl.GyroHeading |
|
- | 565 | GPS_Stick.Nick = 0; |
|
- | 566 | GPS_Stick.Roll = 0; |
|
- | 567 | GPS_Stick.Yaw = 0; |
|
- | 568 | } |
|
- | 569 | else // deviation could not be calculated |
|
- | 570 | { // do nothing on gps sticks! |
|
- | 571 | GPS_Neutral(); |
|
- | 572 | } |
|
- | 573 | ||
- | 574 | }// eof if GPSSignal is OK |
|
- | 575 | else // GPSSignal not OK |
|
- | 576 | { |
|
- | 577 | GPS_Neutral(); |
|
- | 578 | // beep if signal is not sufficient |
|
- | 579 | if(GPS_Parameter.FlightMode != GPS_FLIGHT_MODE_FREE) |
|
- | 580 | { |
|
- | 581 | if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
|
- | 582 | else if (GPSData.NumOfSats < GPS_Parameter.MinSat && !(beep_rythm % 5)) BeepTime = 10; |
|
- | 583 | } |
|
- | 584 | } |
|
- | 585 | GPSData.Status = PROCESSED; // mark as processed |
|
- | 586 | break; |
|
- | 587 | } |
|
- | 588 | ||
- | 589 | DebugOut.Analog[27] = (s16)TargetDeviation.North; |
|
- | 590 | DebugOut.Analog[28] = (s16)TargetDeviation.East; |
|
- | 591 | DebugOut.Analog[29] = GPS_Stick.Nick; |
|
- | 592 | DebugOut.Analog[30] = GPS_Stick.Roll; |
|
- | 593 | ||
- | 594 | // update navi data, send back to ground station |
|
- | 595 | GPS_CopyPosition(&(GPSData.Position), &(NaviData.CurrentPosition)); |
|
- | 596 | GPS_CopyPosition(&RangedTargetPosition, &(NaviData.TargetPosition)); |
|
- | 597 | NaviData.SatsInUse = GPSData.NumOfSats; |
|
- | 598 | NaviData.TargetPositionDeviation.Distance = TargetDeviation.Distance; |
|
- | 599 | NaviData.TargetPositionDeviation.Bearing = TargetDeviation.Bearing; |
|
147 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
600 | NaviData.UBat = FC.UBat; |
148 | //+ No Fix |
601 | NaviData.GroundSpeed = (u16)GPSData.Speed_Ground; |