Rev 2044 | Rev 2046 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2044 | Rev 2045 | ||
---|---|---|---|
Line 45... | Line 45... | ||
45 | void GPS_updateFlightMode(void) { |
45 | void GPS_updateFlightMode(void) { |
46 | static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF; |
46 | static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF; |
Line 47... | Line 47... | ||
47 | 47 | ||
48 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD |
48 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD |
49 | || MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
49 | || MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
50 | flightMode = GPS_FLIGHT_MODE_FREE; |
50 | flightMode = GPS_FLIGHT_MODE_HOME; |
51 | } else { |
51 | } else { |
52 | if (dynamicParams.directGPSMode < 50) |
- | |
53 | flightMode = GPS_FLIGHT_MODE_AID; |
- | |
54 | else if (dynamicParams.directGPSMode < 180) |
52 | if (dynamicParams.naviMode < 50) |
- | 53 | flightMode = GPS_FLIGHT_MODE_FREE; |
|
- | 54 | else if (dynamicParams.naviMode < 180) |
|
55 | flightMode = GPS_FLIGHT_MODE_FREE; |
55 | flightMode = GPS_FLIGHT_MODE_AID; |
56 | else |
56 | else |
57 | flightMode = GPS_FLIGHT_MODE_HOME; |
57 | flightMode = GPS_FLIGHT_MODE_HOME; |
Line 58... | Line 58... | ||
58 | } |
58 | } |
59 | 59 | ||
60 | if (flightMode != flightModeOld) { |
60 | if (flightMode != flightModeOld) { |
61 | beep(100); |
61 | beep(100); |
- | 62 | flightModeOld = flightMode; |
|
- | 63 | } |
|
62 | flightModeOld = flightMode; |
64 | |
Line 63... | Line 65... | ||
63 | } |
65 | debugOut.analog[31] = flightMode; |
64 | } |
66 | } |
65 | 67 | ||
Line 98... | Line 100... | ||
98 | else |
100 | else |
99 | return 1; |
101 | return 1; |
100 | } |
102 | } |
Line 101... | Line 103... | ||
101 | 103 | ||
102 | // set given position to current gps position |
104 | // set given position to current gps position |
103 | uint8_t GPS_setCurrPosition(GPS_Pos_t * pGPSPos) { |
105 | uint8_t GPS_writeCurrPositionTo(GPS_Pos_t * pGPSPos) { |
104 | if (pGPSPos == NULL) |
106 | if (pGPSPos == NULL) |
Line 105... | Line 107... | ||
105 | return 0; // bad pointer |
107 | return 0; // bad pointer |
106 | 108 | ||
Line 151... | Line 153... | ||
151 | || (pTargetPos != pLastTargetPos)) { |
153 | || (pTargetPos != pLastTargetPos)) { |
152 | // reset error integral |
154 | // reset error integral |
153 | GPSPosDevIntegral_North = 0; |
155 | GPSPosDevIntegral_North = 0; |
154 | GPSPosDevIntegral_East = 0; |
156 | GPSPosDevIntegral_East = 0; |
155 | // recalculate latitude projection |
157 | // recalculate latitude projection |
156 | cos_target_latitude = cos_360((int16_t) (pTargetPos->latitude / 10000000L)); |
158 | cos_target_latitude = cos_360(pTargetPos->latitude / 10000000L); |
157 | // remember last target pointer |
159 | // remember last target pointer |
158 | pLastTargetPos = pTargetPos; |
160 | pLastTargetPos = pTargetPos; |
159 | // mark data as processed |
161 | // mark data as processed |
160 | pTargetPos->status = PROCESSED; |
162 | pTargetPos->status = PROCESSED; |
161 | } |
163 | } |
Line 236... | Line 238... | ||
236 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> MATH_UNIT_FACTOR_LOG; |
238 | PID_Roll = (sincompass * PID_North - coscompass * PID_East) >> MATH_UNIT_FACTOR_LOG; |
Line 237... | Line 239... | ||
237 | 239 | ||
238 | // limit resulting GPS control vector |
240 | // limit resulting GPS control vector |
Line -... | Line 241... | ||
- | 241 | GPS_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
|
- | 242 | ||
- | 243 | debugOut.analog[27] = PID_Nick; |
|
239 | GPS_limitXY(&PID_Nick, &PID_Roll, GPS_STICK_LIMIT); |
244 | debugOut.analog[28] = PID_Roll; |
240 | 245 | ||
- | 246 | sticks[CONTROL_PITCH] += PID_Nick; |
|
241 | sticks[CONTROL_PITCH] += (int16_t) PID_Nick; |
247 | sticks[CONTROL_ROLL] += PID_Roll; |
242 | sticks[CONTROL_ROLL] += (int16_t) PID_Roll; |
248 | |
243 | } else { // invalid GPS data or bad compass reading |
249 | } else { // invalid GPS data or bad compass reading |
244 | // reset error integral |
250 | // reset error integral |
245 | GPSPosDevIntegral_North = 0; |
251 | GPSPosDevIntegral_North = 0; |
Line 254... | Line 260... | ||
254 | GPS_updateFlightMode(); |
260 | GPS_updateFlightMode(); |
Line 255... | Line 261... | ||
255 | 261 | ||
256 | // store home position if start of flight flag is set |
262 | // store home position if start of flight flag is set |
257 | if (MKFlags & MKFLAG_CALIBRATE) { |
263 | if (MKFlags & MKFLAG_CALIBRATE) { |
258 | MKFlags &= ~(MKFLAG_CALIBRATE); |
264 | MKFlags &= ~(MKFLAG_CALIBRATE); |
- | 265 | if (GPS_writeCurrPositionTo(&homePosition)) { |
|
259 | if (GPS_setCurrPosition(&homePosition)) |
266 | homePosition.latitude += 9000L; |
- | 267 | beep(500); |
|
260 | beep(500); |
268 | } |
Line 261... | Line 269... | ||
261 | } |
269 | } |
262 | 270 | ||
263 | switch (GPSInfo.status) { |
271 | switch (GPSInfo.status) { |
264 | case INVALID: // invalid gps data |
272 | case INVALID: // invalid gps data |
Line 281... | Line 289... | ||
281 | beep_rythm++; |
289 | beep_rythm++; |
282 | if (GPS_isSignalOK()) { |
290 | if (GPS_isSignalOK()) { |
283 | switch (flightMode) { // check what's to do |
291 | switch (flightMode) { // check what's to do |
284 | case GPS_FLIGHT_MODE_FREE: |
292 | case GPS_FLIGHT_MODE_FREE: |
285 | // update hold position to current gps position |
293 | // update hold position to current gps position |
286 | GPS_setCurrPosition(&holdPosition); // can get invalid if gps signal is bad |
294 | GPS_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad |
287 | // disable gps control |
295 | // disable gps control |
288 | break; |
296 | break; |
Line 289... | Line 297... | ||
289 | 297 | ||
290 | case GPS_FLIGHT_MODE_AID: |
298 | case GPS_FLIGHT_MODE_AID: |
291 | if (holdPosition.status != INVALID) { |
299 | if (holdPosition.status != INVALID) { |
292 | if (GPS_isManuallyControlled(sticks)) { // MK controlled by user |
300 | if (GPS_isManuallyControlled(sticks)) { // MK controlled by user |
293 | // update hold point to current gps position |
301 | // update hold point to current gps position |
294 | GPS_setCurrPosition(&holdPosition); |
302 | GPS_writeCurrPositionTo(&holdPosition); |
295 | // disable gps control |
303 | // disable gps control |
296 | GPS_P_Delay = 0; |
304 | GPS_P_Delay = 0; |
297 | } else { // GPS control active |
305 | } else { // GPS control active |
298 | if (GPS_P_Delay < 7) { |
306 | if (GPS_P_Delay < 7) { |
299 | // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
307 | // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
300 | GPS_P_Delay++; |
308 | GPS_P_Delay++; |
301 | GPS_setCurrPosition(&holdPosition); // update hold point to current gps position |
309 | GPS_writeCurrPositionTo(&holdPosition); // update hold point to current gps position |
302 | GPS_PIDController(NULL, sticks); // activates only the D-Part |
310 | GPS_PIDController(NULL, sticks); // activates only the D-Part |
303 | } else |
311 | } else |
304 | GPS_PIDController(&holdPosition, sticks); // activates the P&D-Part |
312 | GPS_PIDController(&holdPosition, sticks); // activates the P&D-Part |
305 | } |
313 | } |
306 | } else // invalid Hold Position |
314 | } else // invalid Hold Position |
307 | { // try to catch a valid hold position from gps data input |
315 | { // try to catch a valid hold position from gps data input |
308 | GPS_setCurrPosition(&holdPosition); |
316 | GPS_writeCurrPositionTo(&holdPosition); |
309 | } |
317 | } |
Line 310... | Line 318... | ||
310 | break; |
318 | break; |
311 | 319 | ||
312 | case GPS_FLIGHT_MODE_HOME: |
320 | case GPS_FLIGHT_MODE_HOME: |
313 | if (homePosition.status != INVALID) { |
321 | if (homePosition.status != INVALID) { |
314 | // update hold point to current gps position |
322 | // update hold point to current gps position |
315 | // to avoid a flight back if home comming is deactivated |
323 | // to avoid a flight back if home comming is deactivated |
316 | GPS_setCurrPosition(&holdPosition); |
324 | GPS_writeCurrPositionTo(&holdPosition); |
317 | if (GPS_isManuallyControlled(sticks)) // MK controlled by user |
325 | if (GPS_isManuallyControlled(sticks)) // MK controlled by user |
318 | { |
326 | { |
319 | } else {// GPS control active |
327 | } else {// GPS control active |
Line 330... | Line 338... | ||
330 | } else { |
338 | } else { |
331 | // GPS control active |
339 | // GPS control active |
332 | GPS_PIDController(&holdPosition, sticks); |
340 | GPS_PIDController(&holdPosition, sticks); |
333 | } |
341 | } |
334 | } else { // try to catch a valid hold position |
342 | } else { // try to catch a valid hold position |
335 | GPS_setCurrPosition(&holdPosition); |
343 | GPS_writeCurrPositionTo(&holdPosition); |
336 | } |
344 | } |
337 | } |
345 | } |
338 | break; // eof TSK_HOME |
346 | break; // eof TSK_HOME |
339 | default: // unhandled task |
347 | default: // unhandled task |
340 | break; // eof default |
348 | break; // eof default |