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