Rev 2071 | Rev 2076 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2071 | Rev 2074 | ||
---|---|---|---|
Line 11... | Line 11... | ||
11 | #include "attitude.h" |
11 | #include "attitude.h" |
12 | #include "dongfangMath.h" |
12 | #include "dongfangMath.h" |
13 | #include "attitude.h" |
13 | #include "attitude.h" |
Line 14... | Line 14... | ||
14 | 14 | ||
15 | typedef enum { |
15 | typedef enum { |
16 | GPS_FLIGHT_MODE_UNDEF, |
16 | NAVI_FLIGHT_MODE_UNDEF, |
17 | GPS_FLIGHT_MODE_FREE, |
17 | NAVI_FLIGHT_MODE_FREE, |
18 | GPS_FLIGHT_MODE_AID, |
18 | NAVI_FLIGHT_MODE_AID, |
19 | GPS_FLIGHT_MODE_HOME, |
19 | NAVI_FLIGHT_MODE_HOME, |
Line -... | Line 20... | ||
- | 20 | } FlightMode_t; |
|
- | 21 | ||
- | 22 | typedef enum { |
|
- | 23 | NAVI_STATUS_FREEFLIGHT=0, |
|
- | 24 | NAVI_STATUS_INVALID_GPS=1, |
|
- | 25 | NAVI_STATUS_BAD_GPS_SIGNAL=2, |
|
- | 26 | NAVI_STATUS_MANUAL_OVERRIDE=5, |
|
- | 27 | NAVI_STATUS_POSITION_HOLD=7, |
|
- | 28 | NAVI_STATUS_RTH=8, |
|
- | 29 | NAVI_STATUS_HOLD_POSITION_INVALID=9, |
|
- | 30 | NAVI_STATUS_RTH_FALLBACK_ON_HOLD=10, |
|
- | 31 | NAVI_STATUS_RTH_POSITION_INVALID=11, |
|
- | 32 | NAVI_STATUS_GPS_TIMEOUT=12 |
|
20 | } FlightMode_t; |
33 | } NaviStatus_t; |
21 | 34 | ||
22 | #define GPS_POSINTEGRAL_LIMIT 32000 |
35 | #define GPS_POSINTEGRAL_LIMIT 32000 |
Line 23... | Line 36... | ||
23 | #define LOG_NAVI_STICK_GAIN 3 |
36 | #define LOG_NAVI_STICK_GAIN 3 |
Line 33... | Line 46... | ||
33 | // GPS coordinates for hold position |
46 | // GPS coordinates for hold position |
34 | GPS_Pos_t holdPosition = { 0, 0, 0, INVALID }; |
47 | GPS_Pos_t holdPosition = { 0, 0, 0, INVALID }; |
35 | // GPS coordinates for home position |
48 | // GPS coordinates for home position |
36 | GPS_Pos_t homePosition = { 0, 0, 0, INVALID }; |
49 | GPS_Pos_t homePosition = { 0, 0, 0, INVALID }; |
37 | // the current flight mode |
50 | // the current flight mode |
38 | FlightMode_t flightMode = GPS_FLIGHT_MODE_UNDEF; |
51 | FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF; |
39 | int16_t naviSticks[2] = {0,0}; |
52 | int16_t naviSticks[2] = {0,0}; |
Line -... | Line 53... | ||
- | 53 | ||
- | 54 | uint8_t naviStatus; |
|
40 | 55 | ||
41 | // --------------------------------------------------------------------------------- |
56 | // --------------------------------------------------------------------------------- |
42 | void navi_updateFlightMode(void) { |
57 | void navi_updateFlightMode(void) { |
Line 43... | Line 58... | ||
43 | static FlightMode_t flightModeOld = GPS_FLIGHT_MODE_UNDEF; |
58 | static FlightMode_t flightModeOld = NAVI_FLIGHT_MODE_UNDEF; |
44 | 59 | ||
45 | if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
60 | if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
46 | flightMode = GPS_FLIGHT_MODE_FREE; |
61 | flightMode = NAVI_FLIGHT_MODE_FREE; |
47 | } else { |
62 | } else { |
48 | if (dynamicParams.naviMode < 50) |
63 | if (dynamicParams.naviMode < 50) |
49 | flightMode = GPS_FLIGHT_MODE_FREE; |
64 | flightMode = NAVI_FLIGHT_MODE_FREE; |
50 | else if (dynamicParams.naviMode < 180) |
65 | else if (dynamicParams.naviMode < 180) |
51 | flightMode = GPS_FLIGHT_MODE_AID; |
66 | flightMode = NAVI_FLIGHT_MODE_AID; |
52 | else |
67 | else |
Line 53... | Line 68... | ||
53 | flightMode = GPS_FLIGHT_MODE_HOME; |
68 | flightMode = NAVI_FLIGHT_MODE_HOME; |
54 | } |
69 | } |
55 | 70 | ||
Line 258... | Line 273... | ||
258 | 273 | ||
259 | // store home position if start of flight flag is set |
274 | // store home position if start of flight flag is set |
260 | if (MKFlags & MKFLAG_CALIBRATE) { |
275 | if (MKFlags & MKFLAG_CALIBRATE) { |
261 | MKFlags &= ~(MKFLAG_CALIBRATE); |
276 | MKFlags &= ~(MKFLAG_CALIBRATE); |
262 | if (navi_writeCurrPositionTo(&homePosition)) { |
277 | if (navi_writeCurrPositionTo(&homePosition)) { |
263 | // shift north to simulate an offset. |
278 | // shift north to simulate an offset. |
264 | // homePosition.latitude += 10000L; |
279 | // homePosition.latitude += 10000L; |
265 | beep(500); |
280 | beep(500); |
266 | } |
281 | } |
Line 267... | Line 282... | ||
267 | } |
282 | } |
268 | 283 | ||
269 | switch (GPSInfo.status) { |
284 | switch (GPSInfo.status) { |
- | 285 | case INVALID: // invalid gps data |
|
270 | case INVALID: // invalid gps data |
286 | navi_setNeutral(); |
271 | navi_setNeutral(); |
287 | naviStatus = NAVI_STATUS_INVALID_GPS; |
272 | if (flightMode != GPS_FLIGHT_MODE_FREE) { |
288 | if (flightMode != NAVI_FLIGHT_MODE_FREE) { |
273 | beep(1); // beep if signal is neccesary |
289 | beep(1); // beep if signal is neccesary |
274 | } |
290 | } |
275 | break; |
291 | break; |
Line 280... | Line 296... | ||
280 | // if no new data arrived within timeout set current data invalid |
296 | // if no new data arrived within timeout set current data invalid |
281 | // and therefore disable GPS |
297 | // and therefore disable GPS |
282 | else { |
298 | else { |
283 | navi_setNeutral(); |
299 | navi_setNeutral(); |
284 | GPSInfo.status = INVALID; |
300 | GPSInfo.status = INVALID; |
- | 301 | naviStatus = NAVI_STATUS_GPS_TIMEOUT; |
|
285 | } |
302 | } |
286 | break; |
303 | break; |
287 | case NEWDATA: // new valid data from gps device |
304 | case NEWDATA: // new valid data from gps device |
288 | // if the gps data quality is good |
305 | // if the gps data quality is good |
289 | beep_rythm++; |
306 | beep_rythm++; |
290 | if (navi_isGPSSignalOK()) { |
307 | if (navi_isGPSSignalOK()) { |
291 | switch (flightMode) { // check what's to do |
308 | switch (flightMode) { // check what's to do |
292 | case GPS_FLIGHT_MODE_FREE: |
309 | case NAVI_FLIGHT_MODE_FREE: |
293 | // update hold position to current gps position |
310 | // update hold position to current gps position |
294 | navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad |
311 | navi_writeCurrPositionTo(&holdPosition); // can get invalid if gps signal is bad |
295 | // disable gps control |
312 | // disable gps control |
296 | navi_setNeutral(); |
313 | navi_setNeutral(); |
- | 314 | naviStatus = NAVI_STATUS_FREEFLIGHT; |
|
297 | break; |
315 | break; |
Line 298... | Line 316... | ||
298 | 316 | ||
299 | case GPS_FLIGHT_MODE_AID: |
317 | case NAVI_FLIGHT_MODE_AID: |
300 | if (holdPosition.status != INVALID) { |
318 | if (holdPosition.status != INVALID) { |
301 | if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
319 | if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
302 | // update hold point to current gps position |
320 | // update hold point to current gps position |
303 | navi_writeCurrPositionTo(&holdPosition); |
321 | navi_writeCurrPositionTo(&holdPosition); |
304 | // disable gps control |
322 | // disable gps control |
305 | navi_setNeutral(); |
323 | navi_setNeutral(); |
- | 324 | GPS_P_Delay = 0; |
|
306 | GPS_P_Delay = 0; |
325 | naviStatus = NAVI_STATUS_MANUAL_OVERRIDE; |
307 | } else { // GPS control active |
326 | } else { // GPS control active |
308 | if (GPS_P_Delay < 7) { |
327 | if (GPS_P_Delay < 7) { |
309 | // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
328 | // delayed activation of P-Part for 8 cycles (8*0.25s = 2s) |
310 | GPS_P_Delay++; |
329 | GPS_P_Delay++; |
311 | navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position |
330 | navi_writeCurrPositionTo(&holdPosition); // update hold point to current gps position |
- | 331 | navi_PIDController(NULL); // activates only the D-Part |
|
312 | navi_PIDController(NULL); // activates only the D-Part |
332 | naviStatus = NAVI_STATUS_POSITION_HOLD; |
313 | } else |
333 | } else { |
- | 334 | navi_PIDController(&holdPosition); // activates the P&D-Part |
|
- | 335 | naviStatus = NAVI_STATUS_POSITION_HOLD; |
|
314 | navi_PIDController(&holdPosition); // activates the P&D-Part |
336 | } |
315 | } |
337 | } |
316 | } else { // invalid Hold Position |
338 | } else { // invalid Hold Position |
317 | // try to catch a valid hold position from gps data input |
339 | // try to catch a valid hold position from gps data input |
318 | navi_writeCurrPositionTo(&holdPosition); |
340 | navi_writeCurrPositionTo(&holdPosition); |
- | 341 | navi_setNeutral(); |
|
319 | navi_setNeutral(); |
342 | naviStatus = NAVI_STATUS_HOLD_POSITION_INVALID; |
320 | } |
343 | } |
Line 321... | Line 344... | ||
321 | break; |
344 | break; |
322 | 345 | ||
323 | case GPS_FLIGHT_MODE_HOME: |
346 | case NAVI_FLIGHT_MODE_HOME: |
324 | if (homePosition.status != INVALID) { |
347 | if (homePosition.status != INVALID) { |
325 | // update hold point to current gps position |
348 | // update hold point to current gps position |
326 | // to avoid a flight back if home comming is deactivated |
349 | // to avoid a flight back if home comming is deactivated |
327 | navi_writeCurrPositionTo(&holdPosition); |
350 | navi_writeCurrPositionTo(&holdPosition); |
328 | if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
351 | if (navi_isManuallyControlled(PRTY)) { // MK controlled by user |
329 | navi_setNeutral(); |
352 | navi_setNeutral(); |
330 | } else {// GPS control active |
353 | } else {// GPS control active |
- | 354 | navi_PIDController(&homePosition); |
|
331 | navi_PIDController(&homePosition); |
355 | } |
332 | } |
356 | naviStatus = NAVI_STATUS_RTH; |
333 | } else { |
357 | } else { |
334 | // bad home position |
358 | // bad home position |
335 | beep(50); // signal invalid home position |
- | |
336 | // try to hold at least the position as a fallback option |
359 | beep(50); // signal invalid home position |
337 | 360 | // try to hold at least the position as a fallback option |
|
338 | if (holdPosition.status != INVALID) { |
361 | if (holdPosition.status != INVALID) { |
339 | if (navi_isManuallyControlled(PRTY)) { |
362 | if (navi_isManuallyControlled(PRTY)) { |
- | 363 | // MK controlled by user |
|
340 | // MK controlled by user |
364 | navi_setNeutral(); |
341 | navi_setNeutral(); |
365 | naviStatus = NAVI_STATUS_MANUAL_OVERRIDE; |
342 | } else { |
366 | } else { |
- | 367 | // GPS control active |
|
343 | // GPS control active |
368 | navi_PIDController(&holdPosition); |
344 | navi_PIDController(&holdPosition); |
369 | naviStatus = NAVI_STATUS_RTH_FALLBACK_ON_HOLD; |
345 | } |
370 | } |
- | 371 | } else { // try to catch a valid hold position |
|
346 | } else { // try to catch a valid hold position |
372 | navi_writeCurrPositionTo(&holdPosition); |
347 | navi_writeCurrPositionTo(&holdPosition); |
373 | naviStatus = NAVI_STATUS_RTH_POSITION_INVALID; |
348 | navi_setNeutral(); |
374 | navi_setNeutral(); |
349 | } |
375 | } |
350 | } |
376 | } |
Line 355... | Line 381... | ||
355 | } // eof switch GPS_Task |
381 | } // eof switch GPS_Task |
356 | } // eof gps data quality is good |
382 | } // eof gps data quality is good |
357 | else { // gps data quality is bad |
383 | else { // gps data quality is bad |
358 | // disable gps control |
384 | // disable gps control |
359 | navi_setNeutral(); |
385 | navi_setNeutral(); |
- | 386 | naviStatus = NAVI_STATUS_BAD_GPS_SIGNAL; |
|
360 | if (flightMode != GPS_FLIGHT_MODE_FREE) { |
387 | if (flightMode != NAVI_FLIGHT_MODE_FREE) { |
361 | // beep if signal is not sufficient |
388 | // beep if signal is not sufficient |
362 | if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) |
389 | if (!(GPSInfo.flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) |
363 | beep(100); |
390 | beep(100); |
364 | else if (GPSInfo.satnum < staticParams.GPSMininumSatellites |
391 | else if (GPSInfo.satnum < staticParams.GPSMininumSatellites |
365 | && !(beep_rythm % 5)) |
392 | && !(beep_rythm % 5)) |
Line 371... | Line 398... | ||
371 | break; |
398 | break; |
372 | } // eof GPSInfo.status |
399 | } // eof GPSInfo.status |
Line 373... | Line 400... | ||
373 | 400 | ||
374 | PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH]; |
401 | PRTY[CONTROL_PITCH] += naviSticks[CONTROL_PITCH]; |
- | 402 | PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL]; |
|
- | 403 | ||
- | 404 | debugOut.analog[16] = flightMode; |
|
- | 405 | debugOut.analog[17] = naviStatus; |
|
- | 406 | ||
- | 407 | debugOut.analog[18] = naviSticks[CONTROL_PITCH]; |
|
375 | PRTY[CONTROL_ROLL] += naviSticks[CONTROL_ROLL]; |
408 | debugOut.analog[19] = naviSticks[CONTROL_ROLL]; |