src/main/flight/navigation.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18
  19 #include <stdbool.h>
  20 #include <stdint.h>
  21 #include <ctype.h>
  22 #include <string.h>
  23 #include <math.h>
  24
  25 #include "platform.h"
  26 #include "debug.h"
  27
  28 #include "common/maths.h"
  29 #include "common/axis.h"
  30
  31 #include "drivers/system.h"
  32 #include "drivers/serial.h"
  33 #include "drivers/serial_uart.h"
  34 #include "drivers/gpio.h"
  35 #include "drivers/light_led.h"
  36
  37 #include "sensors/sensors.h"
  38
  39 #include "io/beeper.h"
  40 #include "io/serial.h"
  41 #include "io/gps.h"
  42 #include "io/rc_controls.h"
  43
  44 #include "flight/pid.h"
  45 #include "flight/navigation.h"
  46 #include "flight/gps_conversion.h"
  47
  48 #include "rx/rx.h"
  49
  50
  51 #include "config/config.h"
  52 #include "config/runtime_config.h"
  53
  54 extern int16_t magHold;
  55
  56 #ifdef GPS
  57
  58 bool areSticksInApModePosition(uint16_t ap_mode);
  59
  60 // **********************
  61 // GPS
  62 // **********************
  63 int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 };    // it's the angles that must be applied for GPS correction
  64 int32_t GPS_home[2];
  65 int32_t GPS_hold[2];
  66
  67 uint16_t GPS_distanceToHome;        // distance to home point in meters
  68 int16_t GPS_directionToHome;        // direction to home or hol point in degrees
  69
  70 static int16_t nav[2];
  71 static int16_t nav_rated[2];               // Adding a rate controller to the navigation to make it smoother
  72 navigationMode_e nav_mode = NAV_MODE_NONE;    // Navigation mode
  73
  74 static gpsProfile_t *gpsProfile;
  75
  76 void gpsUseProfile(gpsProfile_t *gpsProfileToUse)
  77 {
  78     gpsProfile = gpsProfileToUse;
  79 }
  80
  81 // When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
  82 void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile)
  83 {
  84     gpsUseProfile(initialGpsProfile);
  85     gpsUsePIDs(pidProfile);
  86 }
  87
  88
  89
  90 /*-----------------------------------------------------------
  91  *
  92  * Multiwii GPS code - revision: 1097
  93  *
  94  *-----------------------------------------------------------*/
  95 #define POSHOLD_IMAX           20       // degrees
  96 #define POSHOLD_RATE_IMAX      20       // degrees
  97 #define NAV_IMAX               20       // degrees
  98
  99 /* GPS navigation can control the heading */
 100 #define NAV_TAIL_FIRST             0    // true - copter comes in with tail first
 101 #define NAV_SET_TAKEOFF_HEADING    1    // true - when copter arrives to home position it rotates it's head to takeoff direction
 102
 103 #define GPS_FILTERING              1    // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
 104 #define GPS_LOW_SPEED_D_FILTER     1    // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise
 105
 106 static bool check_missed_wp(void);
 107 static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing);
 108 //static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
 109 static void GPS_calc_longitude_scaling(int32_t lat);
 110 static void GPS_calc_velocity(void);
 111 static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng);
 112 static void GPS_calc_poshold(void);
 113 static void GPS_calc_nav_rate(uint16_t max_speed);
 114 static void GPS_update_crosstrack(void);
 115 static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow);
 116
 117 static int32_t wrap_18000(int32_t error);
 118 static int32_t wrap_36000(int32_t angle);
 119
 120 typedef struct {
 121     int16_t last_velocity;
 122 } LeadFilter_PARAM;
 123
 124 typedef struct {
 125     float kP;
 126     float kI;
 127     float kD;
 128     float Imax;
 129 } PID_PARAM;
 130
 131 static PID_PARAM posholdPID_PARAM;
 132 static PID_PARAM poshold_ratePID_PARAM;
 133 static PID_PARAM navPID_PARAM;
 134
 135 typedef struct {
 136     float integrator;          // integrator value
 137     int32_t last_input;        // last input for derivative
 138     float last_derivative;     // last derivative for low-pass filter
 139     float output;
 140     float derivative;
 141 } PID;
 142
 143 static PID posholdPID[2];
 144 static PID poshold_ratePID[2];
 145 static PID navPID[2];
 146
 147 static int32_t get_P(int32_t error, PID_PARAM *pid)
 148 {
 149     return (float)error * pid->kP;
 150 }
 151
 152 static int32_t get_I(int32_t error, float *dt, PID *pid, PID_PARAM *pid_param)
 153 {
 154     pid->integrator += ((float)error * pid_param->kI) * *dt;
 155     pid->integrator = constrain(pid->integrator, -pid_param->Imax, pid_param->Imax);
 156     return pid->integrator;
 157 }
 158
 159 static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
 160 {
 161     pid->derivative = (input - pid->last_input) / *dt;
 162
 163     // Low pass filter cut frequency for derivative calculation
 164     // Set to  "1 / ( 2 * PI * gps_lpf )
 165     float pidFilter = (1.0f / (2.0f * M_PIf * (float)gpsProfile->gps_lpf));
 166     // discrete low pass filter, cuts out the
 167     // high frequency noise that can drive the controller crazy
 168     pid->derivative = pid->last_derivative + (*dt / (pidFilter + *dt)) * (pid->derivative - pid->last_derivative);
 169     // update state
 170     pid->last_input = input;
 171     pid->last_derivative = pid->derivative;
 172     // add in derivative component
 173     return pid_param->kD * pid->derivative;
 174 }
 175
 176 static void reset_PID(PID *pid)
 177 {
 178     pid->integrator = 0;
 179     pid->last_input = 0;
 180     pid->last_derivative = 0;
 181 }
 182
 183 #define GPS_X 1
 184 #define GPS_Y 0
 185
 186 /****************** PI and PID controllers for GPS ********************///32938 -> 33160
 187
 188 #define RADX100                    0.000174532925f
 189 #define CROSSTRACK_GAIN            1
 190 #define NAV_SLOW_NAV               true
 191 #define NAV_BANK_MAX               3000 // 30deg max banking when navigating (just for security and testing)
 192
 193 static float dTnav;             // Delta Time in milliseconds for navigation computations, updated with every good GPS read
 194 static int16_t actual_speed[2] = { 0, 0 };
 195 static float GPS_scaleLonDown = 1.0f;  // this is used to offset the shrinking longitude as we go towards the poles
 196
 197 // The difference between the desired rate of travel and the actual rate of travel
 198 // updated after GPS read - 5-10hz
 199 static int16_t rate_error[2];
 200 static int32_t error[2];
 201
 202 // Currently used WP
 203 static int32_t GPS_WP[2];
 204
 205 ////////////////////////////////////////////////////////////////////////////////
 206 // Location & Navigation
 207 ////////////////////////////////////////////////////////////////////////////////
 208 // This is the angle from the copter to the "next_WP" location in degrees * 100
 209 static int32_t target_bearing;
 210 ////////////////////////////////////////////////////////////////////////////////
 211 // Crosstrack
 212 ////////////////////////////////////////////////////////////////////////////////
 213 // deg * 100, The original angle to the next_WP when the next_WP was set
 214 // Also used to check when we pass a WP
 215 static int32_t original_target_bearing;
 216 // The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
 217 static int16_t crosstrack_error;
 218 ////////////////////////////////////////////////////////////////////////////////
 219 // The location of the copter in relation to home, updated every GPS read (1deg - 100)
 220 //static int32_t home_to_copter_bearing;
 221 // distance between plane and home in cm
 222 //static int32_t home_distance;
 223 // distance between plane and next_WP in cm
 224 static uint32_t wp_distance;
 225
 226 // used for slow speed wind up when start navigation;
 227 static int16_t waypoint_speed_gov;
 228
 229 ////////////////////////////////////////////////////////////////////////////////////
 230 // moving average filter variables
 231 //
 232 #define GPS_FILTER_VECTOR_LENGTH 5
 233
 234 static uint8_t GPS_filter_index = 0;
 235 static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
 236 static int32_t GPS_filter_sum[2];
 237 static int32_t GPS_read[2];
 238 static int32_t GPS_filtered[2];
 239 static int32_t GPS_degree[2];   //the lat lon degree without any decimals (lat/10 000 000)
 240 static uint16_t fraction3[2];
 241
 242 // This is the angle from the copter to the "next_WP" location
 243 // with the addition of Crosstrack error in degrees * 100
 244 static int32_t nav_bearing;
 245 // saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home
 246 static int16_t nav_takeoff_bearing;
 247
 248 void GPS_calculateDistanceAndDirectionToHome(void)
 249 {
 250     if (STATE(GPS_FIX_HOME)) {      // If we don't have home set, do not display anything
 251         uint32_t dist;
 252         int32_t dir;
 253         GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
 254         GPS_distanceToHome = dist / 100;
 255         GPS_directionToHome = dir / 100;
 256     } else {
 257         GPS_distanceToHome = 0;
 258         GPS_directionToHome = 0;
 259     }
 260 }
 261
 262 void onGpsNewData(void)
 263 {
 264     int axis;
 265     static uint32_t nav_loopTimer;
 266     uint16_t speed;
 267
 268
 269     if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) {
 270         return;
 271     }
 272
 273     if (!ARMING_FLAG(ARMED))
 274         DISABLE_STATE(GPS_FIX_HOME);
 275
 276     if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED))
 277         GPS_reset_home_position();
 278
 279     // Apply moving average filter to GPS data
 280 #if defined(GPS_FILTERING)
 281     GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
 282     for (axis = 0; axis < 2; axis++) {
 283         GPS_read[axis] = GPS_coord[axis];               // latest unfiltered data is in GPS_latitude and GPS_longitude
 284         GPS_degree[axis] = GPS_read[axis] / 10000000;   // get the degree to assure the sum fits to the int32_t
 285
 286         // How close we are to a degree line ? its the first three digits from the fractions of degree
 287         // later we use it to Check if we are close to a degree line, if yes, disable averaging,
 288         fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000;
 289
 290         GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
 291         GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000);
 292         GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
 293         GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
 294         if (nav_mode == NAV_MODE_POSHOLD) {             // we use gps averaging only in poshold mode...
 295             if (fraction3[axis] > 1 && fraction3[axis] < 999)
 296                 GPS_coord[axis] = GPS_filtered[axis];
 297         }
 298     }
 299 #endif
 300
 301     //
 302     // Calculate time delta for navigation loop, range 0-1.0f, in seconds
 303     //
 304     // Time for calculating x,y speed and navigation pids
 305     dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
 306     nav_loopTimer = millis();
 307     // prevent runup from bad GPS
 308     dTnav = MIN(dTnav, 1.0f);
 309
 310     GPS_calculateDistanceAndDirectionToHome();
 311
 312     // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
 313     GPS_calc_velocity();
 314
 315     if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
 316         // we are navigating
 317
 318         // gps nav calculations, these are common for nav and poshold
 319         GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
 320         GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
 321
 322         switch (nav_mode) {
 323         case NAV_MODE_POSHOLD:
 324             // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
 325             GPS_calc_poshold();
 326             break;
 327
 328         case NAV_MODE_WP:
 329             speed = GPS_calc_desired_speed(gpsProfile->nav_speed_max, NAV_SLOW_NAV);    // slow navigation
 330             // use error as the desired rate towards the target
 331             // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
 332             GPS_calc_nav_rate(speed);
 333
 334             // Tail control
 335             if (gpsProfile->nav_controls_heading) {
 336                 if (NAV_TAIL_FIRST) {
 337                     magHold = wrap_18000(nav_bearing - 18000) / 100;
 338                 } else {
 339                     magHold = nav_bearing / 100;
 340                 }
 341             }
 342             // Are we there yet ?(within x meters of the destination)
 343             if ((wp_distance <= gpsProfile->gps_wp_radius) || check_missed_wp()) {      // if yes switch to poshold mode
 344                 nav_mode = NAV_MODE_POSHOLD;
 345                 if (NAV_SET_TAKEOFF_HEADING) {
 346                     magHold = nav_takeoff_bearing;
 347                 }
 348             }
 349             break;
 350         default:
 351             break;
 352         }
 353     }                   //end of gps calcs
 354 }
 355
 356 void GPS_reset_home_position(void)
 357 {
 358     if (STATE(GPS_FIX) && GPS_numSat >= 5) {
 359         GPS_home[LAT] = GPS_coord[LAT];
 360         GPS_home[LON] = GPS_coord[LON];
 361         GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
 362         nav_takeoff_bearing = heading;              // save takeoff heading
 363         // Set ground altitude
 364         ENABLE_STATE(GPS_FIX_HOME);
 365     }
 366 }
 367
 368 // reset navigation (stop the navigation processor, and clear nav)
 369 void GPS_reset_nav(void)
 370 {
 371     int i;
 372
 373     for (i = 0; i < 2; i++) {
 374         GPS_angle[i] = 0;
 375         nav_rated[i] = 0;
 376         nav[i] = 0;
 377         reset_PID(&posholdPID[i]);
 378         reset_PID(&poshold_ratePID[i]);
 379         reset_PID(&navPID[i]);
 380     }
 381 }
 382
 383 // Get the relevant P I D values and set the PID controllers
 384 void gpsUsePIDs(pidProfile_t *pidProfile)
 385 {
 386     posholdPID_PARAM.kP = (float)pidProfile->P8[PIDPOS] / 100.0f;
 387     posholdPID_PARAM.kI = (float)pidProfile->I8[PIDPOS] / 100.0f;
 388     posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
 389
 390     poshold_ratePID_PARAM.kP = (float)pidProfile->P8[PIDPOSR] / 10.0f;
 391     poshold_ratePID_PARAM.kI = (float)pidProfile->I8[PIDPOSR] / 100.0f;
 392     poshold_ratePID_PARAM.kD = (float)pidProfile->D8[PIDPOSR] / 1000.0f;
 393     poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
 394
 395     navPID_PARAM.kP = (float)pidProfile->P8[PIDNAVR] / 10.0f;
 396     navPID_PARAM.kI = (float)pidProfile->I8[PIDNAVR] / 100.0f;
 397     navPID_PARAM.kD = (float)pidProfile->D8[PIDNAVR] / 1000.0f;
 398     navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
 399 }
 400
 401 // OK here is the onboard GPS code
 402
 403 ////////////////////////////////////////////////////////////////////////////////////
 404 // PID based GPS navigation functions
 405 // Author : EOSBandi
 406 // Based on code and ideas from the Arducopter team: Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen
 407 // Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni
 408
 409 ////////////////////////////////////////////////////////////////////////////////////
 410 // this is used to offset the shrinking longitude as we go towards the poles
 411 // It's ok to calculate this once per waypoint setting, since it changes a little within the reach of a multicopter
 412 //
 413 static void GPS_calc_longitude_scaling(int32_t lat)
 414 {
 415     float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f;
 416     GPS_scaleLonDown = cos_approx(rads);
 417 }
 418
 419 ////////////////////////////////////////////////////////////////////////////////////
 420 // Sets the waypoint to navigate, reset neccessary variables and calculate initial values
 421 //
 422 void GPS_set_next_wp(int32_t *lat, int32_t *lon)
 423 {
 424     GPS_WP[LAT] = *lat;
 425     GPS_WP[LON] = *lon;
 426
 427     GPS_calc_longitude_scaling(*lat);
 428     GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
 429
 430     nav_bearing = target_bearing;
 431     GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
 432     original_target_bearing = target_bearing;
 433     waypoint_speed_gov = gpsProfile->nav_speed_min;
 434 }
 435
 436 ////////////////////////////////////////////////////////////////////////////////////
 437 // Check if we missed the destination somehow
 438 //
 439 static bool check_missed_wp(void)
 440 {
 441     int32_t temp;
 442     temp = target_bearing - original_target_bearing;
 443     temp = wrap_18000(temp);
 444     return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
 445 }
 446
 447 #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
 448 #define TAN_89_99_DEGREES 5729.57795f
 449
 450 ////////////////////////////////////////////////////////////////////////////////////
 451 // Get distance between two points in cm
 452 // Get bearing from pos1 to pos2, returns an 1deg = 100 precision
 453 static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)
 454 {
 455     float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
 456     float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
 457     *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;
 458
 459     *bearing = 9000.0f + atan2f(-dLat, dLon) * TAN_89_99_DEGREES;      // Convert the output radians to 100xdeg
 460     if (*bearing < 0)
 461         *bearing += 36000;
 462 }
 463
 464 ////////////////////////////////////////////////////////////////////////////////////
 465 // keep old calculation function for compatibility (could be removed later) distance in meters, bearing in degree
 466 //
 467 //static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing) {
 468 //  uint32_t d1;
 469 //  int32_t  d2;
 470 //  GPS_distance_cm_bearing(&lat1,&lon1,&lat2,&lon2,&d1,&d2);
 471 //  *dist = d1 / 100;          //convert to meters
 472 //  *bearing = d2 /  100;      //convert to degrees
 473 //}
 474
 475 ////////////////////////////////////////////////////////////////////////////////////
 476 // Calculate our current speed vector from gps position data
 477 //
 478 static void GPS_calc_velocity(void)
 479 {
 480     static int16_t speed_old[2] = { 0, 0 };
 481     static int32_t last_coord[2] = { 0, 0 };
 482     static uint8_t init = 0;
 483     // y_GPS_speed positive = Up
 484     // x_GPS_speed positive = Right
 485
 486     if (init) {
 487         float tmp = 1.0f / dTnav;
 488         actual_speed[GPS_X] = (float)(GPS_coord[LON] - last_coord[LON]) * GPS_scaleLonDown * tmp;
 489         actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last_coord[LAT]) * tmp;
 490
 491         actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
 492         actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
 493
 494         speed_old[GPS_X] = actual_speed[GPS_X];
 495         speed_old[GPS_Y] = actual_speed[GPS_Y];
 496     }
 497     init = 1;
 498
 499     last_coord[LON] = GPS_coord[LON];
 500     last_coord[LAT] = GPS_coord[LAT];
 501 }
 502
 503 ////////////////////////////////////////////////////////////////////////////////////
 504 // Calculate a location error between two gps coordinates
 505 // Because we are using lat and lon to do our distance errors here's a quick chart:
 506 //      100     = 1m
 507 //      1000    = 11m    = 36 feet
 508 //      1800    = 19.80m = 60 feet
 509 //      3000    = 33m
 510 //      10000   = 111m
 511 //
 512 static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, int32_t *gps_lat, int32_t *gps_lng)
 513 {
 514     error[LON] = (float)(*target_lng - *gps_lng) * GPS_scaleLonDown;   // X Error
 515     error[LAT] = *target_lat - *gps_lat;        // Y Error
 516 }
 517
 518 ////////////////////////////////////////////////////////////////////////////////////
 519 // Calculate nav_lat and nav_lon from the x and y error and the speed
 520 //
 521 static void GPS_calc_poshold(void)
 522 {
 523     int32_t d;
 524     int32_t target_speed;
 525     int axis;
 526
 527     for (axis = 0; axis < 2; axis++) {
 528         target_speed = get_P(error[axis], &posholdPID_PARAM);       // calculate desired speed from lon error
 529         rate_error[axis] = target_speed - actual_speed[axis];       // calc the speed error
 530
 531         nav[axis] = get_P(rate_error[axis], &poshold_ratePID_PARAM) +
 532                     get_I(rate_error[axis] + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
 533         d = get_D(error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
 534         d = constrain(d, -2000, 2000);
 535
 536         // get rid of noise
 537 #if defined(GPS_LOW_SPEED_D_FILTER)
 538         if (ABS(actual_speed[axis]) < 50)
 539             d = 0;
 540 #endif
 541
 542         nav[axis] += d;
 543         nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
 544         navPID[axis].integrator = poshold_ratePID[axis].integrator;
 545     }
 546 }
 547
 548 ////////////////////////////////////////////////////////////////////////////////////
 549 // Calculate the desired nav_lat and nav_lon for distance flying such as RTH
 550 //
 551 static void GPS_calc_nav_rate(uint16_t max_speed)
 552 {
 553     float trig[2];
 554     float temp;
 555     int axis;
 556
 557     // push us towards the original track
 558     GPS_update_crosstrack();
 559
 560     // nav_bearing includes crosstrack
 561     temp = (9000l - nav_bearing) * RADX100;
 562     trig[GPS_X] = cos_approx(temp);
 563     trig[GPS_Y] = sin_approx(temp);
 564
 565     for (axis = 0; axis < 2; axis++) {
 566         rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];
 567         rate_error[axis] = constrain(rate_error[axis], -1000, 1000);
 568         // P + I + D
 569         nav[axis] = get_P(rate_error[axis], &navPID_PARAM) +
 570                     get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM) +
 571                     get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM);
 572
 573         nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
 574         poshold_ratePID[axis].integrator = navPID[axis].integrator;
 575     }
 576 }
 577
 578 ////////////////////////////////////////////////////////////////////////////////////
 579 // Calculating cross track error, this tries to keep the copter on a direct line
 580 // when flying to a waypoint.
 581 //
 582 static void GPS_update_crosstrack(void)
 583 {
 584     if (ABS(wrap_18000(target_bearing - original_target_bearing)) < 4500) {     // If we are too far off or too close we don't do track following
 585         float temp = (target_bearing - original_target_bearing) * RADX100;
 586         crosstrack_error = sin_approx(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
 587         nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
 588         nav_bearing = wrap_36000(nav_bearing);
 589     } else {
 590         nav_bearing = target_bearing;
 591     }
 592 }
 593
 594 ////////////////////////////////////////////////////////////////////////////////////
 595 // Determine desired speed when navigating towards a waypoint, also implement slow
 596 // speed rampup when starting a navigation
 597 //
 598 //      |< WP Radius
 599 //      0  1   2   3   4   5   6   7   8m
 600 //      ...|...|...|...|...|...|...|...|
 601 //                100  |  200     300     400cm/s
 602 //                 |                                        +|+
 603 //                 |< we should slow to 1.5 m/s as we hit the target
 604 //
 605 static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
 606 {
 607     // max_speed is default 400 or 4m/s
 608     if (_slow) {
 609         max_speed = MIN(max_speed, wp_distance / 2);
 610     } else {
 611         max_speed = MIN(max_speed, wp_distance);
 612         max_speed = MAX(max_speed, gpsProfile->nav_speed_min);      // go at least 100cm/s
 613     }
 614
 615     // limit the ramp up of the speed
 616     // waypoint_speed_gov is reset to 0 at each new WP command
 617     if (max_speed > waypoint_speed_gov) {
 618         waypoint_speed_gov += (int)(100.0f * dTnav);    // increase at .5/ms
 619         max_speed = waypoint_speed_gov;
 620     }
 621     return max_speed;
 622 }
 623
 624 ////////////////////////////////////////////////////////////////////////////////////
 625 // Utilities
 626 //
 627 static int32_t wrap_18000(int32_t error)
 628 {
 629     if (error > 18000)
 630         error -= 36000;
 631     if (error < -18000)
 632         error += 36000;
 633     return error;
 634 }
 635
 636 static int32_t wrap_36000(int32_t angle)
 637 {
 638     if (angle > 36000)
 639         angle -= 36000;
 640     if (angle < 0)
 641         angle += 36000;
 642     return angle;
 643 }
 644
 645 void updateGpsStateForHomeAndHoldMode(void)
 646 {
 647     float sin_yaw_y = sin_approx(heading * 0.0174532925f);
 648     float cos_yaw_x = cos_approx(heading * 0.0174532925f);
 649     if (gpsProfile->nav_slew_rate) {
 650         nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); // TODO check this on uint8
 651         nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate);
 652         GPS_angle[AI_ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
 653         GPS_angle[AI_PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
 654     } else {
 655         GPS_angle[AI_ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
 656         GPS_angle[AI_PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
 657     }
 658 }
 659
 660 void updateGpsWaypointsAndMode(void)
 661 {
 662     bool resetNavNow = false;
 663     static bool gpsReadyBeepDone = false;
 664
 665     if (STATE(GPS_FIX) && GPS_numSat >= 5) {
 666
 667         //
 668         // process HOME mode
 669         //
 670         // HOME mode takes priority over HOLD mode.
 671
 672         if (IS_RC_MODE_ACTIVE(BOXGPSHOME)) {
 673             if (!FLIGHT_MODE(GPS_HOME_MODE)) {
 674
 675                 // Transition to HOME mode
 676                 ENABLE_FLIGHT_MODE(GPS_HOME_MODE);
 677                 DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
 678                 GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
 679                 nav_mode = NAV_MODE_WP;
 680                 resetNavNow = true;
 681             }
 682         } else {
 683             if (FLIGHT_MODE(GPS_HOME_MODE)) {
 684
 685                 // Transition from HOME mode
 686                 DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
 687                 nav_mode = NAV_MODE_NONE;
 688                 resetNavNow = true;
 689             }
 690
 691             //
 692             // process HOLD mode
 693             //
 694
 695             if (IS_RC_MODE_ACTIVE(BOXGPSHOLD) && areSticksInApModePosition(gpsProfile->ap_mode)) {
 696                 if (!FLIGHT_MODE(GPS_HOLD_MODE)) {
 697
 698                     // Transition to HOLD mode
 699                     ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
 700                     GPS_hold[LAT] = GPS_coord[LAT];
 701                     GPS_hold[LON] = GPS_coord[LON];
 702                     GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
 703                     nav_mode = NAV_MODE_POSHOLD;
 704                     resetNavNow = true;
 705                 }
 706             } else {
 707                 if (FLIGHT_MODE(GPS_HOLD_MODE)) {
 708
 709                     // Transition from HOLD mode
 710                     DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
 711                     nav_mode = NAV_MODE_NONE;
 712                     resetNavNow = true;
 713                 }
 714             }
 715         }
 716         if (!gpsReadyBeepDone) {            //if 'ready' beep not yet done
 717             beeper(BEEPER_READY_BEEP);      //do ready beep now
 718             gpsReadyBeepDone = true;        //only beep once
 719         }
 720     } else {
 721         if (FLIGHT_MODE(GPS_HOLD_MODE | GPS_HOME_MODE)) {
 722
 723             // Transition from HOME or HOLD mode
 724             DISABLE_FLIGHT_MODE(GPS_HOME_MODE);
 725             DISABLE_FLIGHT_MODE(GPS_HOLD_MODE);
 726             nav_mode = NAV_MODE_NONE;
 727             resetNavNow = true;
 728         }
 729     }
 730
 731     if (resetNavNow) {
 732         GPS_reset_nav();
 733     }
 734 }
 735
 736 #endif