Rev 99 | Rev 103 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 99 | Rev 102 | ||
|---|---|---|---|
| Line 93... | Line 93... | ||
| 93 | float P; |
93 | float P; |
| 94 | float I; |
94 | float I; |
| 95 | float D; |
95 | float D; |
| 96 | float A; |
96 | float A; |
| 97 | float ACC; |
97 | float ACC; |
| 98 | u32 P_Limit; |
98 | s32 P_Limit; |
| 99 | u32 I_Limit; |
99 | s32 I_Limit; |
| 100 | u32 D_Limit; |
100 | s32 D_Limit; |
| 101 | u32 PID_Limit; |
101 | s32 PID_Limit; |
| 102 | u32 BrakingDuration; |
102 | u32 BrakingDuration; |
| 103 | u8 MinSat; |
103 | u8 MinSat; |
| 104 | s8 StickThreshold; |
104 | s8 StickThreshold; |
| 105 | float WindCorrection; |
105 | float WindCorrection; |
| 106 | float SpeedCompensation; |
106 | float SpeedCompensation; |
| 107 | u32 OperatingRadius; |
107 | s32 OperatingRadius; |
| 108 | GPS_FlightMode_t FlightMode; |
108 | GPS_FlightMode_t FlightMode; |
| 109 | } __attribute__((packed)) GPS_Parameter_t; |
109 | } __attribute__((packed)) GPS_Parameter_t; |
| Line 110... | Line 110... | ||
| 110 | 110 | ||
| 111 | typedef struct |
111 | typedef struct |
| 112 | { |
112 | { |
| 113 | u8 Status; // invalid, newdata, processed |
113 | u8 Status; // invalid, newdata, processed |
| 114 | s32 North; // in cm |
114 | s32 North; // in cm |
| 115 | s32 East; // in cm |
115 | s32 East; // in cm |
| 116 | s32 Bearing; // in deg |
116 | s32 Bearing; // in deg |
| 117 | u32 Distance; // in cm |
117 | s32 Distance; // in cm |
| 118 | } __attribute__((packed)) GPS_Deviation_t; |
118 | } __attribute__((packed)) GPS_Deviation_t; |
| 119 | GPS_Deviation_t CurrentTargetDeviation; // Deviation from Target |
119 | GPS_Deviation_t CurrentTargetDeviation; // Deviation from Target |
| 120 | GPS_Deviation_t CurrentHomeDeviation; // Deviation from Home |
120 | GPS_Deviation_t CurrentHomeDeviation; // Deviation from Home |
| Line 186... | Line 186... | ||
| 186 | GPS_Parameter.P = (float)Parameter.NaviGpsP; |
186 | GPS_Parameter.P = (float)Parameter.NaviGpsP; |
| 187 | GPS_Parameter.I = (float)Parameter.NaviGpsI; |
187 | GPS_Parameter.I = (float)Parameter.NaviGpsI; |
| 188 | GPS_Parameter.D = (float)Parameter.NaviGpsD; |
188 | GPS_Parameter.D = (float)Parameter.NaviGpsD; |
| 189 | GPS_Parameter.A = (float)Parameter.NaviGpsD; |
189 | GPS_Parameter.A = (float)Parameter.NaviGpsD; |
| 190 | GPS_Parameter.ACC = (float)Parameter.NaviGpsACC; |
190 | GPS_Parameter.ACC = (float)Parameter.NaviGpsACC; |
| 191 | GPS_Parameter.P_Limit = (u32)Parameter.NaviGpsPLimit; |
191 | GPS_Parameter.P_Limit = (s32)Parameter.NaviGpsPLimit; |
| 192 | GPS_Parameter.I_Limit = (u32)Parameter.NaviGpsILimit; |
192 | GPS_Parameter.I_Limit = (s32)Parameter.NaviGpsILimit; |
| 193 | GPS_Parameter.D_Limit = (u32)Parameter.NaviGpsDLimit; |
193 | GPS_Parameter.D_Limit = (s32)Parameter.NaviGpsDLimit; |
| 194 | GPS_Parameter.PID_Limit = 2* (u32)Parameter.NaviAngleLimitation; |
194 | GPS_Parameter.PID_Limit = 2* (u32)Parameter.NaviAngleLimitation; |
| 195 | GPS_Parameter.BrakingDuration = (u32)Parameter.NaviPH_LoginTime; |
195 | GPS_Parameter.BrakingDuration = (u32)Parameter.NaviPH_LoginTime; |
| 196 | GPS_Parameter.SpeedCompensation = (float)Parameter.NaviSpeedCompensation; |
196 | GPS_Parameter.SpeedCompensation = (float)Parameter.NaviSpeedCompensation; |
| 197 | GPS_Parameter.MinSat = (u8)Parameter.NaviGpsMinSat; |
197 | GPS_Parameter.MinSat = (u8)Parameter.NaviGpsMinSat; |
| 198 | GPS_Parameter.StickThreshold = (s8)Parameter.NaviStickThreshold; |
198 | GPS_Parameter.StickThreshold = (s8)Parameter.NaviStickThreshold; |
| 199 | GPS_Parameter.WindCorrection = (float)Parameter.NaviWindCorrection; |
199 | GPS_Parameter.WindCorrection = (float)Parameter.NaviWindCorrection; |
| 200 | GPS_Parameter.OperatingRadius = (u32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm |
200 | GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm |
| 201 | } |
201 | } |
| 202 | // FlightMode changed? |
202 | // FlightMode changed? |
| 203 | if(GPS_Parameter.FlightMode != FlightMode_Old) BeepTime = 100; // beep to indicate that mode has switched |
203 | if(GPS_Parameter.FlightMode != FlightMode_Old) BeepTime = 100; // beep to indicate that mode has switched |
| 204 | FlightMode_Old = GPS_Parameter.FlightMode; |
204 | FlightMode_Old = GPS_Parameter.FlightMode; |
| 205 | } |
205 | } |
| Line 214... | Line 214... | ||
| 214 | 214 | ||
| 215 | //------------------------------------------------------------ |
215 | //------------------------------------------------------------ |
| 216 | // Checks for manual control action |
216 | // Checks for manual control action |
| 217 | u8 GPS_IsManuallyControlled(void) |
217 | u8 GPS_IsManuallyControlled(void) |
| 218 | { |
218 | { |
| - | 219 | if( ( (abs(FC.StickNick) > GPS_Parameter.StickThreshold) || (abs(FC.StickRoll) > GPS_Parameter.StickThreshold)) && (GPS_Parameter.StickThreshold > 0) && (FC.RC_Quality > 150) ) |
|
| - | 220 | { |
|
| - | 221 | NCFlags |= NC_FLAG_MANUAL_CONTROL; |
|
| - | 222 | return(1); |
|
| - | 223 | } |
|
| - | 224 | else |
|
| - | 225 | { |
|
| 219 | if( ( (abs(FC.StickNick) > GPS_Parameter.StickThreshold) || (abs(FC.StickRoll) > GPS_Parameter.StickThreshold)) && (GPS_Parameter.StickThreshold > 0)) return 1; |
226 | NCFlags &= ~NC_FLAG_MANUAL_CONTROL; |
| - | 227 | return(0); |
|
| 220 | else return 0; |
228 | } |
| Line 221... | Line 229... | ||
| 221 | } |
229 | } |
| 222 | 230 | ||
| 223 | //------------------------------------------------------------ |
231 | //------------------------------------------------------------ |
| Line 291... | Line 299... | ||
| 291 | } |
299 | } |
| Line 292... | Line 300... | ||
| 292 | 300 | ||
| 293 | 301 | ||
| 294 | //------------------------------------------------------------ |
302 | //------------------------------------------------------------ |
| 295 | // Rescale xy-vector length if length limit is violated |
303 | // Rescale xy-vector length if length limit is violated |
| 296 | // return vector length after scaling |
304 | // returns vector len after scaling |
| 297 | u32 GPS_LimitXY(s32 *x, s32 *y, u32 limit) |
305 | s32 GPS_LimitXY(s32 *x, s32 *y, s32 limit) |
| Line 298... | Line 306... | ||
| 298 | { |
306 | { |
| 299 | u32 dist; |
307 | s32 dist; |
| 300 | 308 | ||
| 301 | dist = (u32)hypot(*x,*y); // the length of the vector |
309 | dist = (s32)hypot(*x,*y); // the length of the vector |
| 302 | if (dist > limit) |
310 | if (dist > limit) |
| 303 | // if vector length is larger than the given limit |
311 | // if vector length is larger than the given limit |
| 304 | { // scale vector compontents so that the length is cut off to limit |
312 | { // scale vector compontents so that the length is cut off to limit |
| 305 | *x = (s32)(((double)(*x) * (double)limit) / dist); |
313 | *x = (s32)(( (double)(*x) * (double)limit ) / (double)dist); |
| 306 | *y = (s32)(((double)(*y) * (double)limit) / dist); |
314 | *y = (s32)(( (double)(*y) * (double)limit ) / (double)dist); |
| 307 | dist = limit; |
315 | dist = limit; |
| Line 358... | Line 366... | ||
| 358 | pDeviationFromTarget->North = (s32)(11119492.7f * temp2); |
366 | pDeviationFromTarget->North = (s32)(11119492.7f * temp2); |
| 359 | pDeviationFromTarget->East = (s32)(11119492.7f * temp1); |
367 | pDeviationFromTarget->East = (s32)(11119492.7f * temp1); |
| 360 | // If the position deviation is small enough to neglect the earth curvature |
368 | // If the position deviation is small enough to neglect the earth curvature |
| 361 | // (this is for our application always fulfilled) the distance to target |
369 | // (this is for our application always fulfilled) the distance to target |
| 362 | // can be calculated by the pythagoras of north and east deviation. |
370 | // can be calculated by the pythagoras of north and east deviation. |
| 363 | pDeviationFromTarget->Distance = (u32)(11119492.7f * hypot(temp1, temp2)); |
371 | pDeviationFromTarget->Distance = (s32)(11119492.7f * hypot(temp1, temp2)); |
| 364 | if (pDeviationFromTarget->Distance == 0L) pDeviationFromTarget->Bearing = 0L; |
372 | if (pDeviationFromTarget->Distance == 0L) pDeviationFromTarget->Bearing = 0L; |
| 365 | else pDeviationFromTarget->Bearing = DirectionToTarget_N_E(temp2, temp1); |
373 | else pDeviationFromTarget->Bearing = DirectionToTarget_N_E(temp2, temp1); |
| 366 | pDeviationFromTarget->Status = NEWDATA; |
374 | pDeviationFromTarget->Status = NEWDATA; |
| 367 | return TRUE; |
375 | return TRUE; |
| Line 384... | Line 392... | ||
| 384 | // pointer to current target position |
392 | // pointer to current target position |
| 385 | static GPS_Pos_t * pTargetPositionOld = NULL; |
393 | static GPS_Pos_t * pTargetPositionOld = NULL; |
| 386 | static Waypoint_t* GPS_pWaypointOld = NULL; |
394 | static Waypoint_t* GPS_pWaypointOld = NULL; |
| Line 387... | Line 395... | ||
| 387 | 395 | ||
| 388 | 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 |
396 | 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 |
| 389 | static u32 OperatingRadiusOld = -1; |
397 | static s32 OperatingRadiusOld = -1; |
| Line 390... | Line 398... | ||
| 390 | static u32 WPTime = 0; |
398 | static u32 WPTime = 0; |
| 391 | 399 | ||
| 392 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
400 | //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 519... | Line 527... | ||
| 519 | if(GPS_pWaypoint != NULL) // Waypoint exist |
527 | if(GPS_pWaypoint != NULL) // Waypoint exist |
| 520 | { |
528 | { |
| 521 | // update the hold position |
529 | // update the hold position |
| 522 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
530 | GPS_CopyPosition(&(GPSData.Position), &GPS_HoldPosition); |
| 523 | GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
531 | GPS_pTargetPosition = &(GPS_pWaypoint->Position); |
| 524 | GPS_TargetRadius = (u32)(GPS_pWaypoint->ToleranceRadius) * 100L; |
532 | GPS_TargetRadius = (s32)(GPS_pWaypoint->ToleranceRadius) * 100L; |
| Line 525... | Line 533... | ||
| 525 | 533 | ||
| 526 | } |
534 | } |
| 527 | else // no waypoint info available, i.e. the WPList is empty or the end of the list has been reached |
535 | else // no waypoint info available, i.e. the WPList is empty or the end of the list has been reached |
| 528 | { |
536 | { |
| Line 575... | Line 583... | ||
| 575 | if(TargetHomeDeviation.Distance > GPS_Parameter.OperatingRadius) |
583 | if(TargetHomeDeviation.Distance > GPS_Parameter.OperatingRadius) |
| 576 | { |
584 | { |
| 577 | //calculate ranged target position to be within the operation radius area |
585 | //calculate ranged target position to be within the operation radius area |
| 578 | NCFlags |= NC_FLAG_RANGE_LIMIT; |
586 | NCFlags |= NC_FLAG_RANGE_LIMIT; |
| Line 579... | Line -... | ||
| 579 | - | ||
| 580 | TargetHomeDeviation.North *= GPS_Parameter.OperatingRadius; |
587 | |
| 581 | TargetHomeDeviation.North /= TargetHomeDeviation.Distance; |
- | |
| 582 | TargetHomeDeviation.East *= GPS_Parameter.OperatingRadius; |
588 | TargetHomeDeviation.North = (s32)(((float)TargetHomeDeviation.North * (float)GPS_Parameter.OperatingRadius) / (float)TargetHomeDeviation.Distance); |
| 583 | TargetHomeDeviation.East /= TargetHomeDeviation.Distance; |
589 | TargetHomeDeviation.East = (s32)(((float)TargetHomeDeviation.East * (float)GPS_Parameter.OperatingRadius) / (float)TargetHomeDeviation.Distance); |
| Line 584... | Line 590... | ||
| 584 | TargetHomeDeviation.Distance = GPS_Parameter.OperatingRadius; |
590 | TargetHomeDeviation.Distance = GPS_Parameter.OperatingRadius; |
| 585 | 591 | ||
| 586 | RangedTargetPosition.Status = INVALID; |
592 | RangedTargetPosition.Status = INVALID; |
| 587 | RangedTargetPosition.Latitude = GPS_HomePosition.Latitude; |
593 | RangedTargetPosition.Latitude = GPS_HomePosition.Latitude; |