void GPS_NewData(uint16_t c) { gps.bytes_rx ++; if (!GPS_newFrame(c)) return; gps.frames_rx ++; sensorsSet(SENSOR_GPS); xTimerReset(gpsLostTimer, 10); gps.update = (gps.update == 1 ? 0 : 1); if (flag(FLAG_GPS_FIX) && gps.numSat > 4) xSemaphoreGive(gpsSemaphore); }
static void GPS_NewData(uint16_t c) // Called by uart2Init interrupt { static int32_t LatSpikeTab[5], LonSpikeTab[5]; static bool FilterCleared = false; int32_t diff; uint8_t i; if (GPS_newFrame(c)) { if(!GPS_FIX) // Don't fill spikefilter with pure shit { if(!FilterCleared) { for (i = 0; i < 5; i++) { LatSpikeTab[i] = 0; LonSpikeTab[i] = 0; } FilterCleared = true; } } else // We have a fix. { FilterCleared = false; FiveElementSpikeFilterINT32(IRQGPS_coord[LAT], LatSpikeTab); // Always feed filter FiveElementSpikeFilterINT32(IRQGPS_coord[LON], LonSpikeTab); if (LatSpikeTab[2] && LonSpikeTab[2]) // Is the Spikefilter valid and not in runup phase? { if(!GPS_Conf600[LON]) // No scaling? { IRQGPS_coord[LAT] = LatSpikeTab[2]; IRQGPS_coord[LON] = LonSpikeTab[2]; } else // We have Scaling. Is new GPS val more than 6m away? { // If so take the spikefiltervalue if(LatSpikeTab[2] > IRQGPS_coord[LAT]) diff = LatSpikeTab[2] - IRQGPS_coord[LAT]; else diff = IRQGPS_coord[LAT] - LatSpikeTab[2]; if(abs(diff) > GPS_Conf600[LAT]) IRQGPS_coord[LAT] = LatSpikeTab[2]; if(LonSpikeTab[2] > IRQGPS_coord[LON]) diff = LonSpikeTab[2] - IRQGPS_coord[LON]; else diff = IRQGPS_coord[LON] - LonSpikeTab[2]; if(abs(diff) > GPS_Conf600[LON]) IRQGPS_coord[LON] = LonSpikeTab[2]; } } } TimestampNewGPSdata = millis(); // Set timestamp of Data arrival in MS } }
static void GPS_NewData(uint16_t c) { if (GPS_newFrame(c)) { if (GPS_update == 1) GPS_update = 0; else GPS_update = 1; if (GPS_fix == 1 && GPS_numSat > 3) { if (GPS_fix_home == 0) { GPS_fix_home = 1; GPS_latitude_home = GPS_latitude; GPS_longitude_home = GPS_longitude; } if (GPSModeHold == 1) GPS_distance(GPS_latitude_hold, GPS_longitude_hold, GPS_latitude, GPS_longitude, &GPS_distanceToHold, &GPS_directionToHold); else GPS_distance(GPS_latitude_home, GPS_longitude_home, GPS_latitude, GPS_longitude, &GPS_distanceToHome, &GPS_directionToHome); } } }
void GPS_NewData(void) { static uint32_t nav_loopTimer; uint32_t dist; int32_t dir; uint8_t axis; int16_t speed; uint16_t c = GPSBufferAvailable(); while (c--) { if (GPS_newFrame(GPSBufferRead())) { if (GPS_Info.GPS_update == 1) GPS_Info.GPS_update = 0; else GPS_Info.GPS_update = 1; if (fgps.GPS_FIX && GPS_Info.GPS_numSat >= 5) { if (!checkArm()) { fgps.GPS_FIX_HOME = 0; } if (!fgps.GPS_FIX_HOME && checkArm()) { GPS_reset_home_position(); } //Apply moving average filter to GPS data #if defined(GPS_FILTERING) GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH; for (axis = 0; axis< 2; axis++) { GPS_data[axis] = GPS_Info.GPS_coord[axis]; //latest unfiltered data is in GPS_latitude and GPS_longitude GPS_degree[axis] = GPS_data[axis] / 10000000; // get the degree to assure the sum fits to the int32_t // How close we are to a degree line ? its the first three digits from the fractions of degree // later we use it to Check if we are close to a degree line, if yes, disable averaging, fraction3[axis] = (GPS_data[axis]- GPS_degree[axis]*10000000) / 10000; GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index]; GPS_filter[axis][GPS_filter_index] = GPS_data[axis] - (GPS_degree[axis]*10000000); GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index]; GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000); if ( nav_mode == NAV_MODE_POSHOLD) { //we use gps averaging only in poshold mode... if ( fraction3[axis]>1 && fraction3[axis]<999 ) GPS_Info.GPS_coord[axis] = GPS_filtered[axis]; } } #endif //Time for calculating x,y speed and navigation pids dTnav = (float)(getTickCount() - nav_loopTimer)/ 10000.0f; nav_loopTimer = getTickCount(); // prevent runup from bad GPS dTnav = min(dTnav, 1.0f); //calculate distance and bearings for gui and other stuff continously - From home to copter GPS_distance_cm_bearing(&GPS_Info.GPS_coord[LAT],&GPS_Info.GPS_coord[LON],&GPS_Info.GPS_home[LAT],&GPS_Info.GPS_home[LON],&dist,&dir); GPS_Info.GPS_distanceToHome = dist/100; GPS_Info.GPS_directionToHome = dir/100; if (!fgps.GPS_FIX_HOME) { //If we don't have home set, do not display anything GPS_Info.GPS_distanceToHome = 0; GPS_Info.GPS_directionToHome = 0; } //calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating GPS_calc_velocity(); if (fgps.GPS_HOLD_MODE || fgps.GPS_HOME_MODE){ //ok we are navigating //do gps nav calculations here, these are common for nav and poshold #if defined(GPS_LEAD_FILTER) GPS_distance_cm_bearing(&GPS_coord_lead[LAT],&GPS_coord_lead[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing); GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord_lead[LAT],&GPS_coord_lead[LON]); #else GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing); GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]); #endif switch (nav_mode) { case NAV_MODE_POSHOLD: //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_poshold(); break; case NAV_MODE_WP: speed = GPS_calc_desired_speed(NAV_SPEED_MAX, NAV_SLOW_NAV); //slow navigation // use error as the desired rate towards the target //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_nav_rate(speed); //Tail control if (NAV_CONTROLS_HEADING) { if (NAV_TAIL_FIRST) { magHold = wrap_18000(nav_bearing-18000)/100; } else { magHold = nav_bearing/100; } } // Are we there yet ?(within 2 meters of the destination) if ((wp_distance <= GPS_wp_radius) || check_missed_wp()){ //if yes switch to poshold mode nav_mode = NAV_MODE_POSHOLD; if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; } } break; } } //end of gps calcs } } } }
void serialCom() { uint8_t c,cc,port,state,bytesTXBuff; static uint8_t offset[UART_NUMBER]; static uint8_t dataSize[UART_NUMBER]; static uint8_t c_state[UART_NUMBER]; uint32_t timeMax; // limit max time in this function in case of GPS timeMax = micros(); for(port=0;port<UART_NUMBER;port++) { CURRENTPORT=port; #define RX_COND #if defined(SERIAL_RX) && (UART_NUMBER > 1) #define RX_COND && (RX_SERIAL_PORT != port) #endif cc = SerialAvailable(port); while (cc-- RX_COND) { bytesTXBuff = SerialUsedTXBuff(port); // indicates the number of occupied bytes in TX buffer if (bytesTXBuff > TX_BUFFER_SIZE - 50 ) return; // ensure there is enough free TX buffer to go further (50 bytes margin) c = SerialRead(port); #ifdef SUPPRESS_ALL_SERIAL_MSP evaluateOtherData(c); // no MSP handling, so go directly #else //SUPPRESS_ALL_SERIAL_MSP state = c_state[port]; // regular data handling to detect and handle MSP and other data if (state == IDLE) { if (c=='$') state = HEADER_START; else evaluateOtherData(c); // evaluate all other incoming serial data } else if (state == HEADER_START) { state = (c=='M') ? HEADER_M : IDLE; } else if (state == HEADER_M) { state = (c=='<') ? HEADER_ARROW : IDLE; } else if (state == HEADER_ARROW) { if (c > INBUF_SIZE) { // now we are expecting the payload size state = IDLE; continue; } dataSize[port] = c; checksum[port] = c; offset[port] = 0; indRX[port] = 0; state = HEADER_SIZE; // the command is to follow } else if (state == HEADER_SIZE) { cmdMSP[port] = c; checksum[port] ^= c; state = HEADER_CMD; } else if (state == HEADER_CMD) { if (offset[port] < dataSize[port]) { checksum[port] ^= c; inBuf[offset[port]++][port] = c; } else { if (checksum[port] == c) // compare calculated and transferred checksum evaluateCommand(cmdMSP[port]); // we got a valid packet, evaluate it state = IDLE; cc = 0; // no more than one MSP per port and per cycle } } c_state[port] = state; // SERIAL: try to detect a new nav frame based on the current received buffer #if defined(GPS_SERIAL) if (GPS_SERIAL == port) { static uint32_t GPS_last_frame_seen; //Last gps frame seen at this time, used to detect stalled gps communication if (GPS_newFrame(c)) { //We had a valid GPS data frame, so signal task scheduler to switch to compute if (GPS_update == 1) GPS_update = 0; else GPS_update = 1; //Blink GPS update GPS_last_frame_seen = timeMax; GPS_Frame = 1; } // Check for stalled GPS, if no frames seen for 1.2sec then consider it LOST if ((timeMax - GPS_last_frame_seen) > 1200000) { //No update since 1200ms clear fix... f.GPS_FIX = 0; GPS_numSat = 0; } } if (micros()-timeMax>250) return; // Limit the maximum execution time of serial decoding to avoid time spike #endif #endif // SUPPRESS_ALL_SERIAL_MSP } // while } // for }
uint8_t GPS_NewData(void) { uint8_t axis; #if defined(I2C_GPS) static uint8_t _i2c_gps_status; //Do not use i2c_writereg, since writing a register does not work if an i2c_stop command is issued at the end //Still investigating, however with separated i2c_repstart and i2c_write commands works... and did not caused i2c errors on a long term test. GPS_numSat = (_i2c_gps_status & 0xf0) >> 4; _i2c_gps_status = i2c_readReg(I2C_GPS_ADDRESS,I2C_GPS_STATUS_00); //Get status register uint8_t *varptr; #if defined(I2C_GPS_SONAR) i2c_rep_start(I2C_GPS_ADDRESS<<1); i2c_write(I2C_GPS_SONAR_ALT); i2c_rep_start((I2C_GPS_ADDRESS<<1)|1); varptr = (uint8_t *)&sonarAlt; // altitude (in cm? maybe) *varptr++ = i2c_readAck(); *varptr = i2c_readNak(); #endif if (_i2c_gps_status & I2C_GPS_STATUS_3DFIX) { //Check is we have a good 3d fix (numsats>5) f.GPS_FIX = 1; if (_i2c_gps_status & I2C_GPS_STATUS_NEW_DATA) { //Check about new data GPS_Frame = 1; i2c_rep_start(I2C_GPS_ADDRESS<<1); i2c_write(I2C_GPS_LOCATION); //Start read from here 2x2 bytes distance and direction i2c_rep_start((I2C_GPS_ADDRESS<<1)|1); varptr = (uint8_t *)&GPS_coord[LAT]; // for latitude displaying *varptr++ = i2c_readAck(); *varptr++ = i2c_readAck(); *varptr++ = i2c_readAck(); *varptr = i2c_readAck(); varptr = (uint8_t *)&GPS_coord[LON]; // for longitude displaying *varptr++ = i2c_readAck(); *varptr++ = i2c_readAck(); *varptr++ = i2c_readAck(); *varptr = i2c_readNak(); i2c_rep_start(I2C_GPS_ADDRESS<<1); i2c_write(I2C_GPS_GROUND_SPEED); i2c_rep_start((I2C_GPS_ADDRESS<<1)|1); varptr = (uint8_t *)&GPS_speed; // speed in cm/s for OSD *varptr++ = i2c_readAck(); *varptr = i2c_readAck(); varptr = (uint8_t *)&GPS_altitude; // altitude in meters for OSD *varptr++ = i2c_readAck(); *varptr = i2c_readAck(); varptr = (uint8_t *)&GPS_ground_course; *varptr++ = i2c_readAck(); *varptr = i2c_readNak(); } else { return 0; } } else { f.GPS_FIX = 0; return 0; } #endif #if defined(GPS_SERIAL) || defined(GPS_FROM_OSD) #if defined(GPS_SERIAL) uint8_t c = SerialAvailable(GPS_SERIAL); if (c==0) return 0; while (c--) { if (GPS_newFrame(SerialRead(GPS_SERIAL))) { #elif defined(GPS_FROM_OSD) { if(GPS_update & 2) { // Once second bit of GPS_update is set, indicate new GPS datas is readed from OSD - all in right format. GPS_update &= 1; // We have: GPS_fix(0-2), GPS_numSat(0-15), GPS_coord[LAT & LON](signed, in 1/10 000 000 degres), GPS_altitude(signed, in meters) and GPS_speed(in cm/s) #endif GPS_Frame = 1; } } #endif return 1; } uint8_t GPS_Compute(void) { if (GPS_Frame == 0) return 0; else GPS_Frame = 0; if (GPS_update == 1) GPS_update = 0; else GPS_update = 1; if (f.GPS_FIX && GPS_numSat >= 5) { #if !defined(DONT_RESET_HOME_AT_ARM) if (!f.ARMED) {f.GPS_FIX_HOME = 0;} #endif if (!f.GPS_FIX_HOME && f.ARMED) { GPS_reset_home_position(); } //Apply moving average filter to GPS data #if defined(GPS_FILTERING) GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH; for (uint8_t axis = 0; axis< 2; axis++) { GPS_read[axis] = GPS_coord[axis]; //latest unfiltered data is in GPS_latitude and GPS_longitude GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t // How close we are to a degree line ? its the first three digits from the fractions of degree // later we use it to Check if we are close to a degree line, if yes, disable averaging, fraction3[axis] = (GPS_read[axis]- GPS_degree[axis]*10000000) / 10000; GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index]; GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis]*10000000); GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index]; GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000); if ( nav_mode == NAV_MODE_POSHOLD) { //we use gps averaging only in poshold mode... if ( fraction3[axis]>1 && fraction3[axis]<999 ) GPS_coord[axis] = GPS_filtered[axis]; } } #endif //dTnav calculation //Time for calculating x,y speed and navigation pids static uint32_t nav_loopTimer; dTnav = (float)(millis() - nav_loopTimer)/ 1000.0; nav_loopTimer = millis(); // prevent runup from bad GPS dTnav = min(dTnav, 1.0); //calculate distance and bearings for gui and other stuff continously - From home to copter uint32_t dist; int32_t dir; GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_home[LAT],&GPS_home[LON],&dist,&dir); GPS_distanceToHome = dist/100; GPS_directionToHome = dir/100; if (!f.GPS_FIX_HOME) { //If we don't have home set, do not display anything GPS_distanceToHome = 0; GPS_directionToHome = 0; } //calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating GPS_calc_velocity(); if (f.GPS_HOLD_MODE || f.GPS_HOME_MODE){ //ok we are navigating //do gps nav calculations here, these are common for nav and poshold #if defined(GPS_LEAD_FILTER) GPS_distance_cm_bearing(&GPS_coord_lead[LAT],&GPS_coord_lead[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing); GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord_lead[LAT],&GPS_coord_lead[LON]); #else GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing); GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]); #endif switch (nav_mode) { case NAV_MODE_POSHOLD: //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_poshold(); break; case NAV_MODE_WP: int16_t speed = GPS_calc_desired_speed(NAV_SPEED_MAX, NAV_SLOW_NAV); //slow navigation // use error as the desired rate towards the target //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_nav_rate(speed); //Tail control if (NAV_CONTROLS_HEADING) { if (NAV_TAIL_FIRST) { magHold = wrap_18000(nav_bearing-18000)/100; } else { magHold = nav_bearing/100; } } // Are we there yet ?(within 2 meters of the destination) if ((wp_distance <= GPS_wp_radius) || check_missed_wp()){ //if yes switch to poshold mode nav_mode = NAV_MODE_POSHOLD; if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; } } break; } } //end of gps calcs } } void GPS_reset_home_position(void) { if (f.GPS_FIX && GPS_numSat >= 5) { GPS_home[LAT] = GPS_coord[LAT]; GPS_home[LON] = GPS_coord[LON]; GPS_calc_longitude_scaling(GPS_coord[LAT]); //need an initial value for distance and bearing calc nav_takeoff_bearing = att.heading; //save takeoff heading //Set ground altitude f.GPS_FIX_HOME = 1; } } //reset navigation (stop the navigation processor, and clear nav) void GPS_reset_nav(void) { uint8_t i; for(i=0;i<2;i++) { nav_rated[i] = 0; nav[i] = 0; reset_PID(&posholdPID[i]); reset_PID(&poshold_ratePID[i]); reset_PID(&navPID[i]); nav_mode = NAV_MODE_NONE; } } //Get the relevant P I D values and set the PID controllers void GPS_set_pids(void) { posholdPID_PARAM.kP = (float)conf.pid[PIDPOS].P8/100.0; posholdPID_PARAM.kI = (float)conf.pid[PIDPOS].I8/100.0; posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; poshold_ratePID_PARAM.kP = (float)conf.pid[PIDPOSR].P8/10.0; poshold_ratePID_PARAM.kI = (float)conf.pid[PIDPOSR].I8/100.0; poshold_ratePID_PARAM.kD = (float)conf.pid[PIDPOSR].D8/1000.0; poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; navPID_PARAM.kP = (float)conf.pid[PIDNAVR].P8/10.0; navPID_PARAM.kI = (float)conf.pid[PIDNAVR].I8/100.0; navPID_PARAM.kD = (float)conf.pid[PIDNAVR].D8/1000.0; navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; }
void GPS_NewData(uint16_t c) { uint8_t axis; static uint32_t nav_loopTimer; uint32_t dist; int32_t dir; int16_t speed; if (GPS_newFrame(c)) { if (GPS_update == 1) GPS_update = 0; else GPS_update = 1; if (GPS_fix == 1 && GPS_numSat >= 5) { if (armed == 0) { GPS_fix_home = 0; } if (GPS_fix_home == 0 && armed) { GPS_fix_home = 1; GPS_home[LAT] = GPS_coord[LAT]; GPS_home[LON] = GPS_coord[LON]; GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc nav_takeoff_bearing = heading; // save takeoff heading } // Apply moving average filter to GPS data #if defined(GPS_FILTERING) GPS_filter_index = ++GPS_filter_index % GPS_FILTER_VECTOR_LENGTH; for (axis = 0; axis < 2; axis++) { GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t // How close we are to a degree line ? its the first three digits from the fractions of degree // later we use it to Check if we are close to a degree line, if yes, disable averaging, fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000; GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index]; GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000); GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index]; GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000); if (nav_mode == NAV_MODE_POSHOLD) { //we use gps averaging only in poshold mode... if (fraction3[axis] > 1 && fraction3[axis] < 999) GPS_coord[axis] = GPS_filtered[axis]; } } #endif // dTnav calculation // Time for calculating x,y speed and navigation pids dTnav = (float) (millis() - nav_loopTimer) / 1000.0f; nav_loopTimer = millis(); // prevent runup from bad GPS dTnav = min(dTnav, 1.0f); // calculate distance and bearings for gui and other stuff continously - From home to copter GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir); GPS_distanceToHome = dist / 100; GPS_directionToHome = dir / 100; //calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating GPS_calc_velocity(); if (GPSModeHold == 1 || GPSModeHome == 1) { // ok we are navigating // do gps nav calculations here, these are common for nav and poshold GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); switch (nav_mode) { case NAV_MODE_POSHOLD: // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_poshold(); break; case NAV_MODE_WP: speed = GPS_calc_desired_speed(cfg.nav_speed_max, NAV_SLOW_NAV); //slow navigation // use error as the desired rate towards the target // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 GPS_calc_nav_rate(speed); // Tail control if (cfg.nav_controls_heading) { if (NAV_TAIL_FIRST) { magHold = wrap_18000(nav_bearing - 18000) / 100; } else { magHold = nav_bearing / 100; } } // Are we there yet ?(within x meters of the destination) if ((wp_distance <= cfg.gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode nav_mode = NAV_MODE_POSHOLD; if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; } } break; } } //end of gps calcs } } }