Rev 102 | Rev 105 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 102 | Rev 103 | ||
---|---|---|---|
Line 91... | Line 91... | ||
91 | { |
91 | { |
92 | float Gain; |
92 | float Gain; |
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 | s32 P_Limit; |
98 | s32 P_Limit; |
99 | s32 I_Limit; |
99 | s32 I_Limit; |
100 | s32 D_Limit; |
100 | s32 D_Limit; |
101 | s32 PID_Limit; |
101 | s32 PID_Limit; |
Line 109... | Line 109... | ||
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 | s32 Distance; // in cm |
117 | s32 Distance; // in cm |
118 | } __attribute__((packed)) GPS_Deviation_t; |
118 | } __attribute__((packed)) GPS_Deviation_t; |
Line 198... | Line 198... | ||
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 = (s32)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) |
|
- | 204 | { |
|
203 | if(GPS_Parameter.FlightMode != FlightMode_Old) BeepTime = 100; // beep to indicate that mode has switched |
205 | BeepTime = 100; // beep to indicate that mode has been switched |
- | 206 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
|
- | 207 | } |
|
204 | FlightMode_Old = GPS_Parameter.FlightMode; |
208 | FlightMode_Old = GPS_Parameter.FlightMode; |
205 | } |
209 | } |
Line 206... | Line 210... | ||
206 | 210 | ||
207 | //------------------------------------------------------------- |
211 | //------------------------------------------------------------- |
Line 223... | Line 227... | ||
223 | } |
227 | } |
224 | else |
228 | else |
225 | { |
229 | { |
226 | NCFlags &= ~NC_FLAG_MANUAL_CONTROL; |
230 | NCFlags &= ~NC_FLAG_MANUAL_CONTROL; |
227 | return(0); |
231 | return(0); |
228 | } |
232 | } |
229 | } |
233 | } |
Line 230... | Line 234... | ||
230 | 234 | ||
231 | //------------------------------------------------------------ |
235 | //------------------------------------------------------------ |
232 | // copy GPS position from source position to target position |
236 | // copy GPS position from source position to target position |
Line 307... | Line 311... | ||
307 | s32 dist; |
311 | s32 dist; |
Line 308... | Line 312... | ||
308 | 312 | ||
309 | dist = (s32)hypot(*x,*y); // the length of the vector |
313 | dist = (s32)hypot(*x,*y); // the length of the vector |
310 | if (dist > limit) |
314 | if (dist > limit) |
311 | // if vector length is larger than the given limit |
315 | // if vector length is larger than the given limit |
312 | { // scale vector compontents so that the length is cut off to limit |
316 | { // scale vector compontents so that the length is cut off to limit |
313 | *x = (s32)(( (double)(*x) * (double)limit ) / (double)dist); |
317 | *x = (s32)(( (double)(*x) * (double)limit ) / (double)dist); |
314 | *y = (s32)(( (double)(*y) * (double)limit ) / (double)dist); |
318 | *y = (s32)(( (double)(*y) * (double)limit ) / (double)dist); |
315 | dist = limit; |
319 | dist = limit; |
316 | } |
320 | } |
Line 445... | Line 449... | ||
445 | } |
449 | } |
Line 446... | Line 450... | ||
446 | 450 | ||
Line 447... | Line 451... | ||
447 | /* The selected flight mode influences the target position pointer and therefore the behavior */ |
451 | /* The selected flight mode influences the target position pointer and therefore the behavior */ |
448 | 452 | ||
449 | // check for current flight mode and set the target pointer GPS_pTargetPosition respectively |
453 | // check for current flight mode and set the target pointer GPS_pTargetPosition respectively |
450 | switch(GPS_Parameter.FlightMode) |
454 | switch(GPS_Parameter.FlightMode) |
451 | { |
455 | { |
Line 452... | Line 456... | ||
452 | // the GPS control is deactived |
456 | // the GPS control is deactived |
Line 593... | Line 597... | ||
593 | RangedTargetPosition.Latitude = GPS_HomePosition.Latitude; |
597 | RangedTargetPosition.Latitude = GPS_HomePosition.Latitude; |
594 | RangedTargetPosition.Latitude += (s32)((float)TargetHomeDeviation.North / 1.11194927f); |
598 | RangedTargetPosition.Latitude += (s32)((float)TargetHomeDeviation.North / 1.11194927f); |
595 | RangedTargetPosition.Longitude = GPS_HomePosition.Longitude; |
599 | RangedTargetPosition.Longitude = GPS_HomePosition.Longitude; |
596 | RangedTargetPosition.Longitude += (s32)((float)TargetHomeDeviation.East / (1.11194927f * cos(RadiansFromGPS(GPS_HomePosition.Latitude))) ); |
600 | RangedTargetPosition.Longitude += (s32)((float)TargetHomeDeviation.East / (1.11194927f * cos(RadiansFromGPS(GPS_HomePosition.Latitude))) ); |
597 | RangedTargetPosition.Altitude = GPS_pTargetPosition->Altitude; |
601 | RangedTargetPosition.Altitude = GPS_pTargetPosition->Altitude; |
598 | RangedTargetPosition.Status = NEWDATA; |
602 | RangedTargetPosition.Status = NEWDATA; |
599 | } |
603 | } |
600 | else |
604 | else |
601 | { // the target is located within the operation radius area |
605 | { // the target is located within the operation radius area |
602 | // simple copy the loaction to the ranged target position |
606 | // simple copy the loaction to the ranged target position |
603 | GPS_CopyPosition(GPS_pTargetPosition, &RangedTargetPosition); |
607 | GPS_CopyPosition(GPS_pTargetPosition, &RangedTargetPosition); |
Line 631... | Line 635... | ||
631 | GPS_Stick.Yaw = 0; |
635 | GPS_Stick.Yaw = 0; |
632 | } |
636 | } |
633 | else // deviation could not be calculated |
637 | else // deviation could not be calculated |
634 | { // do nothing on gps sticks! |
638 | { // do nothing on gps sticks! |
635 | GPS_Neutral(); |
639 | GPS_Neutral(); |
- | 640 | NCFlags &= ~NC_FLAG_TARGET_REACHED; // clear target reached |
|
636 | } |
641 | } |
Line 637... | Line 642... | ||
637 | 642 | ||
638 | }// eof if GPSSignal is OK |
643 | }// eof if GPSSignal is OK |
639 | else // GPSSignal not OK |
644 | else // GPSSignal not OK |
Line 644... | Line 649... | ||
644 | { |
649 | { |
645 | if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
650 | if(!(GPSData.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100; |
646 | else if (GPSData.NumOfSats < GPS_Parameter.MinSat && !(beep_rythm % 5)) BeepTime = 10; |
651 | else if (GPSData.NumOfSats < GPS_Parameter.MinSat && !(beep_rythm % 5)) BeepTime = 10; |
647 | } |
652 | } |
648 | } |
653 | } |
649 | GPSData.Status = PROCESSED; // mark as processed |
654 | GPSData.Status = PROCESSED; // mark as processed |
650 | break; |
655 | break; |
651 | } |
656 | } |
Line 652... | Line 657... | ||
652 | 657 | ||
653 | DebugOut.Analog[6] = NCFlags; |
658 | DebugOut.Analog[6] = NCFlags; |
Line 676... | Line 681... | ||
676 | NaviData.NCFlags = NCFlags; |
681 | NaviData.NCFlags = NCFlags; |
677 | NaviData.OperatingRadius = Parameter.NaviOperatingRadius; |
682 | NaviData.OperatingRadius = Parameter.NaviOperatingRadius; |
678 | NaviData.TopSpeed = (s16)GPSData.Speed_Top; // in cm/s |
683 | NaviData.TopSpeed = (s16)GPSData.Speed_Top; // in cm/s |
679 | NaviData.TargetHoldTime = (u8)(GetDelay(WPTime)/1000); // in s |
684 | NaviData.TargetHoldTime = (u8)(GetDelay(WPTime)/1000); // in s |
680 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
685 | //+++++++++++++++++++++++++++++++++++++++++++++++++++ |
681 | return; |
686 | return; |
682 | } |
687 | } |