| 19,17 → 19,22 |
|---|
| NAVI_FLIGHT_MODE_HOME, |
| } FlightMode_t; |
| |
| /* |
| * This is not read from internally. It only serves to let a pilot/debugger |
| * know the status of the navigation system (thru a data connection). |
| */ |
| typedef enum { |
| NAVI_STATUS_NO_COMPASS=-1, |
| NAVI_STATUS_INVALID_GPS=-2, |
| NAVI_STATUS_BAD_GPS_SIGNAL=-3, |
| NAVI_STATUS_RTH_POSITION_INVALID=-4, |
| NAVI_STATUS_RTH_FALLBACK_ON_HOLD=-5, |
| NAVI_STATUS_GPS_TIMEOUT=-6, |
| NAVI_STATUS_FREEFLIGHT=0, |
| NAVI_STATUS_INVALID_GPS=1, |
| NAVI_STATUS_BAD_GPS_SIGNAL=2, |
| NAVI_STATUS_MANUAL_OVERRIDE=5, |
| NAVI_STATUS_POSITION_HOLD=7, |
| NAVI_STATUS_RTH=8, |
| NAVI_STATUS_HOLD_POSITION_INVALID=9, |
| NAVI_STATUS_RTH_FALLBACK_ON_HOLD=10, |
| NAVI_STATUS_RTH_POSITION_INVALID=11, |
| NAVI_STATUS_GPS_TIMEOUT=12 |
| NAVI_STATUS_POSITION_HOLD=1, |
| NAVI_STATUS_RTH=2, |
| NAVI_STATUS_MANUAL_OVERRIDE=3, |
| NAVI_STATUS_HOLD_POSITION_INVALID=4, |
| } NaviStatus_t; |
| |
| #define GPS_POSINTEGRAL_LIMIT 32000 |
| 51,7 → 56,7 |
|---|
| FlightMode_t flightMode = NAVI_FLIGHT_MODE_UNDEF; |
| int16_t naviSticks[2] = {0,0}; |
| |
| uint8_t naviStatus; |
| int8_t naviStatus; |
| |
| // --------------------------------------------------------------------------------- |
| void navi_updateFlightMode(void) { |
| 266,30 → 271,35 |
|---|
| void navigation_periodicTaskAndPRTY(int16_t* PRTY) { |
| static uint8_t GPS_P_Delay = 0; |
| static uint16_t beep_rythm = 0; |
| |
| static uint16_t navi_testOscPrescaler = 0; |
| static uint8_t navi_testOscTimer = 0; |
| |
| if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED)) { |
| navi_setNeutral(); |
| naviStatus = NAVI_STATUS_NO_COMPASS; |
| return; |
| } |
| |
| navi_updateFlightMode(); |
| |
| navi_testOscPrescaler++; |
| if (navi_testOscPrescaler == 488) { |
| navi_testOscPrescaler = 0; |
| navi_testOscTimer++; |
| if (navi_testOscTimer == staticParams.naviTestOscPeriod) { |
| navi_testOscTimer = 0; |
| if (staticParams.naviTestOscAmplitude) { |
| holdPosition.status = NEWDATA; |
| holdPosition.latitude += staticParams.naviTestOscAmplitude * 90L; |
| } |
| } else if (navi_testOscTimer == staticParams.naviTestOscPeriod/2) { |
| if (staticParams.naviTestOscAmplitude) { |
| holdPosition.status = NEWDATA; |
| holdPosition.latitude -= staticParams.naviTestOscAmplitude * 90L; |
| } |
| } |
| navi_testOscPrescaler = 0; |
| navi_testOscTimer++; |
| if (navi_testOscTimer == staticParams.naviTestOscPeriod) { |
| navi_testOscTimer = 0; |
| if (staticParams.naviTestOscAmplitude) { |
| holdPosition.status = NEWDATA; |
| holdPosition.latitude += staticParams.naviTestOscAmplitude * 90L; // there are about 90 units per meter. |
| } |
| } else if (navi_testOscTimer == staticParams.naviTestOscPeriod/2) { |
| if (staticParams.naviTestOscAmplitude) { |
| holdPosition.status = NEWDATA; |
| holdPosition.latitude -= staticParams.naviTestOscAmplitude * 90L; |
| } |
| } |
| } |
| |
| navi_updateFlightMode(); |
| |
| // store home position if start of flight flag is set |
| if (MKFlags & MKFLAG_CALIBRATE) { |
| MKFlags &= ~(MKFLAG_CALIBRATE); |