Page 357 - 2

//Generate a packed byte of all four GPS boxes.
uint8_t gps_modes_check = (rcOptions[BOXLAND]<< 3) + (rcOptions[BOXGPSHOME]<< 2)
+ (rcOptions[BOXGPSHOLD]<<1) + (rcOptions[BOXGPSNAV]);

if (f.ARMED ) { //Check GPS status and armed
//TODO: implement f.GPS_Trusted flag, idea from Dramida - Check for degraded HDOP an
d sudden speed jumps
if (f.GPS_FIX) {
if (GPS_numSat >5 ) {
if (prv_gps_modes != gps_modes_check) { //Check for chang
e since last loop
NAV_error = NAV_ERROR_NONE;
if (rcOptions[BOXGPSHOME]) { // RTH has the p
riotity over everything else
init_RTH();
} else if (rcOptions[BOXGPSHOLD]) { //Position hold ha
s priority over mission execution //But has less priority than RTH
if (f.GPS_mode == GPS_MODE_NAV)
NAV_paused_at = mission_step.number;
f.GPS_mode = GPS_MODE_HOLD;
f.GPS_BARO_MODE = false;
GPS_set_next_wp(&GPS_coord[LAT], &GPS_coord[LON],&GPS_coord[LAT], & GPS
_coord[LON]); //hold at the current position
set_new_altitude(alt.EstAlt); //and current altitude
NAV_state = NAV_STATE_HOLD_INFINIT;
} else if (rcOptions[BOXLAND]) { //Land now (It has
priority over Navigation)
f.GPS_mode = GPS_MODE_HOLD;
f.GPS_BARO_MODE = true;
GPS_set_next_wp(&GPS_coord[LAT], &GPS_coord[LON],&GPS_coord[LAT], & GPS
_coord[LON]);
set_new_altitude(alt.EstAlt);
NAV_state = NAV_STATE_LAND_START;
} else if (rcOptions[BOXGPSNAV]) { //Start navigation
f.GPS_mode = GPS_MODE_NAV; //Nav mode start
f.GPS_BARO_MODE = true;
GPS_prev[LAT] = GPS_coord[LAT];
GPS_prev[LON] = GPS_coord[LON];
if (NAV_paused_at != 0) {
next_step = NAV_paused_at;
NAV_paused_at = 0; //Clear paused step
} else {
next_step = 1;
jump_times = -10; //Reset jump counter

- 357 -