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); |