Rev 131 | Rev 146 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 131 | Rev 133 | ||
---|---|---|---|
Line 132... | Line 132... | ||
132 | 132 | ||
133 | //------------------------------------------------------------- |
133 | //------------------------------------------------------------- |
134 | // Update GPSParamter |
134 | // Update GPSParamter |
135 | void GPS_UpdateParameter(void) |
135 | void GPS_UpdateParameter(void) |
- | 136 | { |
|
- | 137 | #define SWITCH_DELAY 500 |
|
136 | { |
138 | static u32 SwitchDelay = 0; |
- | 139 | static GPS_FlightMode_t FlightMode_Old = GPS_FLIGHT_MODE_UNDEF; |
|
137 | static GPS_FlightMode_t FlightMode_Old = GPS_FLIGHT_MODE_UNDEF; |
140 | |
138 | // in case of bad receiving conditions |
141 | // in case of bad receiving conditions |
139 | if(FC.RC_Quality < 100) |
142 | if(FC.RC_Quality < 100) |
140 | { // set fixed parameter |
143 | { // set fixed parameter |
141 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT; |
144 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT; |
Line 163... | Line 166... | ||
163 | if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
166 | if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
164 | else |
167 | else |
165 | { |
168 | { |
166 | if(Parameter.NaviGpsModeControl < 50) |
169 | if(Parameter.NaviGpsModeControl < 50) |
167 | { |
170 | { |
- | 171 | if(FlightMode_Old == GPS_FLIGHT_MODE_FREE) SetDelay(SWITCH_DELAY); |
|
- | 172 | if(CheckDelay(SwitchDelay)) |
|
- | 173 | { |
|
168 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
174 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE; |
169 | NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH); |
175 | NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH); |
170 | NCFlags |= NC_FLAG_FREE; |
176 | NCFlags |= NC_FLAG_FREE; |
- | 177 | } |
|
171 | } |
178 | } |
172 | else if(Parameter.NaviGpsModeControl < 180) |
179 | else if(Parameter.NaviGpsModeControl < 180) |
173 | { |
180 | { |
- | 181 | if(FlightMode_Old == GPS_FLIGHT_MODE_AID) SetDelay(SWITCH_DELAY); |
|
- | 182 | if(CheckDelay(SwitchDelay)) |
|
- | 183 | { |
|
174 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID; |
184 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID; |
175 | NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_CH); |
185 | NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_CH); |
176 | NCFlags |= NC_FLAG_PH; |
186 | NCFlags |= NC_FLAG_PH; |
- | 187 | } |
|
177 | } |
188 | } |
178 | else |
189 | else |
179 | { |
190 | { |
- | 191 | if(FlightMode_Old == GPS_FLIGHT_MODE_WAYPOINT) SetDelay(SWITCH_DELAY); |
|
- | 192 | if(CheckDelay(SwitchDelay)) |
|
- | 193 | { |
|
180 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT; |
194 | GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT; |
181 | NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH); |
195 | NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH); |
182 | NCFlags |= NC_FLAG_CH; |
196 | NCFlags |= NC_FLAG_CH; |
- | 197 | } |
|
183 | } |
198 | } |
184 | } |
199 | } |
185 | GPS_Parameter.Gain = (float)Parameter.NaviGpsGain; |
200 | GPS_Parameter.Gain = (float)Parameter.NaviGpsGain; |
186 | GPS_Parameter.P = (float)Parameter.NaviGpsP; |
201 | GPS_Parameter.P = (float)Parameter.NaviGpsP; |
187 | GPS_Parameter.I = (float)Parameter.NaviGpsI; |
202 | GPS_Parameter.I = (float)Parameter.NaviGpsI; |
Line 200... | Line 215... | ||
200 | GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm |
215 | GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm |
201 | } |
216 | } |
202 | // FlightMode changed? |
217 | // FlightMode changed? |
203 | if(GPS_Parameter.FlightMode != FlightMode_Old) |
218 | if(GPS_Parameter.FlightMode != FlightMode_Old) |
204 | { |
219 | { |
205 | BeepTime = 100; // beep to indicate that mode has been switched |
220 | BeepTime = 200; // beep to indicate that mode has been switched |
206 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
221 | NCFlags &= ~NC_FLAG_TARGET_REACHED; |
207 | // if the mode has changed to free |
222 | // if the mode has changed to free |
208 | if(GPS_Parameter.FlightMode == GPS_FLIGHT_MODE_FREE) WPList_Clear(); // clear WPList if mode has changed to Free |
223 | if(GPS_Parameter.FlightMode == GPS_FLIGHT_MODE_FREE) WPList_Clear(); // clear WPList if mode has changed to Free |
209 | } |
224 | } |
210 | FlightMode_Old = GPS_Parameter.FlightMode; |
225 | FlightMode_Old = GPS_Parameter.FlightMode; |
Line 487... | Line 502... | ||
487 | GPS_pTargetPosition = NULL; |
502 | GPS_pTargetPosition = NULL; |
488 | GPS_TargetRadius = 0; |
503 | GPS_TargetRadius = 0; |
489 | } |
504 | } |
490 | else |
505 | else |
491 | { |
506 | { |
- | 507 | /* |
|
492 | #define PH_MOVE_THRESHOLD 8 |
508 | #define PH_MOVE_THRESHOLD 8 |
493 | if( ((abs(FC.StickNick) > PH_MOVE_THRESHOLD) || (abs(FC.StickRoll) > PH_MOVE_THRESHOLD) ) && (FC.RC_Quality > 150)) |
509 | if( ((abs(FC.StickNick) > PH_MOVE_THRESHOLD) || (abs(FC.StickRoll) > PH_MOVE_THRESHOLD) ) && (FC.RC_Quality > 150)) |
494 | { // indirect control by moving the hold position with rc sticks |
510 | { // indirect control by moving the hold position with rc sticks |
495 | // rc sticks have a range of +/-127 counts |
511 | // rc sticks have a range of +/-127 counts |
496 | // GPS Coordinates are in 1-E7 deg, i.e. 1 counts is 1E-7°/360°*40E+8cm = 1.11 cm |
512 | // GPS Coordinates are in 1-E7 deg, i.e. 1 counts is 1E-7°/360°*40E+8cm = 1.11 cm |
497 | // normal manual activtion threshold is 8 counts that results in a change of the hold position |
513 | // normal manual activtion threshold is 8 counts that results in a change of the hold position |
498 | // of min 8*1.11cm * 5Hz = 8.88 = 44.4cm/s an max 127*1.11cm *5Hz = 705cm/s |
514 | // of min 8*1.11cm * 5Hz = 8.88 = 44.4cm/s an max 127*1.11cm *5Hz = 705cm/s |
499 | GPS_HoldPosition.Latitude += (s32)(cos_h*FC.StickNick + sin_h*FC.StickRoll); |
515 | GPS_HoldPosition.Latitude += (s32)(cos_h*FC.StickNick + sin_h*FC.StickRoll); |
500 | GPS_HoldPosition.Longitude += (s32)((sin_h*FC.StickNick - cos_h*FC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) ); |
516 | GPS_HoldPosition.Longitude += (s32)((sin_h*FC.StickNick - cos_h*FC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) ); |
501 | } |
517 | }*/ |
- | 518 | // set target position |
|
502 | GPS_pTargetPosition = &GPS_HoldPosition; |
519 | GPS_pTargetPosition = &GPS_HoldPosition; |
503 | GPS_TargetRadius = 100; // 1 meter |
520 | GPS_TargetRadius = 100; // 1 meter |
504 | } |
521 | } |
505 | break; |
522 | break; |