/* GPS navigation can control the heading */
#define NAV_TAIL_FIRST 0 // true - copter comes in with tail first
#define NAV_SET_TAKEOFF_HEADING 1 // true - when copter arrives to home position it rotates it's head to takeoff direction
#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
#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
/// Low pass filter cut frequency for derivative calculation.
// static const float ac_pid_filter = 1.0f / (2.0f * M_PI * (float)cfg.gps_lpf); // Set to "1 / ( 2 * PI * f_cut )"
#define AC_PID_FILTER (1.0f / (2.0f * M_PI * (float)cfg.gps_lpf))
/// Iterate the PID, return the new control value
///
/// Positive error produces positive output.
///
/// @param error The measured error value
/// @param dt The time delta in milliseconds (note
/// that update interval cannot be more
/// than 65.535 seconds due to limited range
/// of the data type).
/// @param scaler An arbitrary scale factor
///
/// @returns The updated control output.
///
static int32_t AC_PID_get_p(AC_PID * ac_pid, int32_t error, PID_PARAM * pid)
{
return (float) error *pid->kP;
}
/****************** PI and PID controllers for GPS ********************///32938 -> 33160
static AC_PID pi_poshold[2];
static AC_PID pid_poshold_rate[2];
static AC_PID pid_nav[2];
#define RADX100 0.000174532925f
#define CROSSTRACK_GAIN 1
#define NAV_SLOW_NAV true
#define NAV_BANK_MAX 3000 // 30deg max banking when navigating (just for security and testing)
static float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
static int16_t actual_speed[2] = { 0, 0 };
static float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles
// The difference between the desired rate of travel and the actual rate of travel
// updated after GPS read - 5-10hz
static int16_t rate_error[2];
static int32_t error[2];
//Currently used WP
static int32_t GPS_WP[2];
////////////////////////////////////////////////////////////////////////////////
// Location & Navigation
////////////////////////////////////////////////////////////////////////////////
// This is the angle from the copter to the "next_WP" location in degrees * 100
static int32_t target_bearing;
////////////////////////////////////////////////////////////////////////////////
// Crosstrack
////////////////////////////////////////////////////////////////////////////////
// deg * 100, The original angle to the next_WP when the next_WP was set
// Also used to check when we pass a WP
static int32_t original_target_bearing;
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
////////////////////////////////////////////////////////////////////////////////
// The location of the copter in relation to home, updated every GPS read (1deg - 100)
//static int32_t home_to_copter_bearing;
// distance between plane and home in cm
//static int32_t home_distance;
// distance between plane and next_WP in cm
static uint32_t wp_distance;
// used for slow speed wind up when start navigation;
static int16_t waypoint_speed_gov;
////////////////////////////////////////////////////////////////////////////////////
// moving average filter variables
//
#define GPS_FILTER_VECTOR_LENGTH 5
// This is the angle from the copter to the "next_WP" location
// with the addition of Crosstrack error in degrees * 100
static int32_t nav_bearing;
// saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home
static int16_t nav_takeoff_bearing;
void GPS_NewData(uint16_t c)
{
int 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 (f.GPS_FIX && GPS_numSat >= 5) {
if (!f.ARMED) {
f.GPS_FIX_HOME = 0;
}
if (!f.GPS_FIX_HOME && f.ARMED) {
f.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+1) % 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 (f.GPS_HOLD_MODE || f.GPS_HOME_MODE) { // 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
}
}
}
//reset navigation (stop the navigation processor, and clear nav)
void GPS_reset_nav(void)
{
int i;
for (i = 0; i < 2; i++) {
GPS_angle[i] = 0;
nav[i] = 0;
AC_PID_reset(&pi_poshold[i]);
AC_PID_reset(&pid_poshold_rate[i]);
AC_PID_reset(&pid_nav[i]);
}
}
//Get the relevant P I D values and set the PID controllers
static void GPS_set_pids(void)
{
posholdPID.kP = (float) cfg.P8[PIDPOS] / 100.0f;
posholdPID.kI = (float) cfg.I8[PIDPOS] / 100.0f;
posholdPID.Imax = POSHOLD_RATE_IMAX * 100;
////////////////////////////////////////////////////////////////////////////////////
// PID based GPS navigation functions
// Author : EOSBandi
// Based on code and ideas from the Arducopter team: Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen
// Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni
////////////////////////////////////////////////////////////////////////////////////
// this is used to offset the shrinking longitude as we go towards the poles
// It's ok to calculate this once per waypoint setting, since it changes a little within the reach of a multicopter
//
static void GPS_calc_longitude_scaling(int32_t lat)
{
float rads = (abs((float)lat)) * (0.0174532925f / 10000000.0f);
GPS_scaleLonDown = cosf(rads);
}
////////////////////////////////////////////////////////////////////////////////////
// Sets the waypoint to navigate, reset neccessary variables and calculate initial values
//
void GPS_set_next_wp(int32_t * lat, int32_t * lon)
{
GPS_WP[LAT] = *lat;
GPS_WP[LON] = *lon;
////////////////////////////////////////////////////////////////////////////////////
// Calculate a location error between two gps coordinates
// Because we are using lat and lon to do our distance errors here's a quick chart:
// 100 = 1m
// 1000 = 11m = 36 feet
// 1800 = 19.80m = 60 feet
// 3000 = 33m
// 10000 = 111m
//
static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng)
{
error[LON] = (float) (*target_lng - *gps_lng) * GPS_scaleLonDown; // X Error
error[LAT] = *target_lat - *gps_lat; // Y Error
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
static void GPS_calc_poshold(void)
{
int32_t p, i, d;
int32_t output;
int32_t target_speed;
int axis;
p = AC_PID_get_p(&pid_poshold_rate[axis], rate_error[axis], &poshold_ratePID);
i = AC_PID_get_i(&pid_poshold_rate[axis], rate_error[axis] + error[axis], &dTnav, &poshold_ratePID);
d = AC_PID_get_d(&pid_poshold_rate[axis], error[axis], &dTnav, &poshold_ratePID);
d = constrain(d, -2000, 2000);
// get rid of noise
#if defined(GPS_LOW_SPEED_D_FILTER)
if (abs(actual_speed[axis]) < 50)
d = 0;
#endif
output = p + i + d;
////////////////////////////////////////////////////////////////////////////////////
// Calculate the desired nav_lat and nav_lon for distance flying such as RTH
//
static void GPS_calc_nav_rate(int max_speed)
{
float trig[2];
float temp;
int axis;
// push us towards the original track
GPS_update_crosstrack();
////////////////////////////////////////////////////////////////////////////////////
// Calculating cross track error, this tries to keep the copter on a direct line
// when flying to a waypoint.
//
static void GPS_update_crosstrack(void)
{
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
float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sinf(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
nav_bearing = wrap_36000(nav_bearing);
} else {
nav_bearing = target_bearing;
}
}
////////////////////////////////////////////////////////////////////////////////////
// Determine desired speed when navigating towards a waypoint, also implement slow
// speed rampup when starting a navigation
//
// |< WP Radius
// 0 1 2 3 4 5 6 7 8m
// ...|...|...|...|...|...|...|...|
// 100 | 200 300 400cm/s
// | +|+
// |< we should slow to 1.5 m/s as we hit the target
//
static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)
{
// max_speed is default 400 or 4m/s
if (_slow) {
max_speed = min(max_speed, wp_distance / 2);
max_speed = max(max_speed, 0);
} else {
max_speed = min(max_speed, wp_distance);
max_speed = max(max_speed, cfg.nav_speed_min); // go at least 100cm/s
}
// limit the ramp up of the speed
// waypoint_speed_gov is reset to 0 at each new WP command
if (max_speed > waypoint_speed_gov) {
waypoint_speed_gov += (int) (100.0f * dTnav); // increase at .5/ms
max_speed = waypoint_speed_gov;
}
return max_speed;
}
/* Alex optimization
The latitude or longitude is coded this way in NMEA frames
dm.f coded as degrees + minutes + minute decimal
Where:
- d can be 1 or more char long. generally: 2 char long for latitude, 3 char long for longitude
- m is always 2 char long
- f can be 1 or more char long
This function converts this format in a unique unsigned long where 1 degree = 10 000 000
EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution
with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased
resolution also increased precision of nav calculations
static uint32_t GPS_coord_to_degrees(char *s)
{
char *p = s, *d = s;
uint8_t min, deg = 0;
uint16_t frac = 0, mult = 10000;
while (*p) { // parse the string until its end
if (d != s) {
frac += (*p - '0') * mult; // calculate only fractional part on up to 5 digits (d != s condition is true when the . is located)
mult /= 10;
}
if (*p == '.')
d = p; // locate '.' char in the string
p++;
}
if (p == s)
return 0;
while (s < d - 2) {
deg *= 10; // convert degrees : all chars before minutes ; for the first iteration, deg = 0
deg += *(s++) - '0';
}
min = *(d - 1) - '0' + (*(d - 2) - '0') * 10; // convert minutes : 2 previous char before '.'
return deg * 10000000UL + (min * 100000UL + frac) * 10UL / 6;
}
*/
#define DIGIT_TO_VAL(_x) (_x - '0')
uint32_t GPS_coord_to_degrees(char* s)
{
char *p, *q;
uint8_t deg = 0, min = 0;
unsigned int frac_min = 0;
int i;
// scan for decimal point or end of field
for (p = s; isdigit(*p); p++)
;
q = s;
// convert degrees
while ((p - q) > 2) {
if (deg)
deg *= 10;
deg += DIGIT_TO_VAL(*q++);
}
// convert minutes
while (p > q) {
if (min)
min *= 10;
min += DIGIT_TO_VAL(*q++);
}
// convert fractional minutes
// expect up to four digits, result is in
// ten-thousandths of a minute
if (*p == '.') {
q = p + 1;
for (i = 0; i < 4; i++) {
frac_min *= 10;
if (isdigit(*q))
frac_min += *q++ - '0';
}
}
return deg * 10000000UL + (min * 1000000UL + frac_min * 100UL) / 6;
}
static uint8_t hex_c(uint8_t n)
{ // convert '0'..'9','A'..'F' to 0..15
n -= '0';
if (n > 9)
n -= 7;
n &= 0x0F;
return n;
}
/* This is a light implementation of a GPS frame decoding
This should work with most of modern GPS devices configured to output NMEA frames.
It assumes there are some NMEA GGA frames to decode on the serial bus
Here we use only the following data :
- latitude
- longitude
- GPS fix is/is not ok
- GPS num sat (4 is enough to be +/- reliable)
// added by Mis
- GPS altitude (for OSD displaying)
- GPS speed (for OSD displaying)
*/
#define FRAME_GGA 1
#define FRAME_RMC 2