rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3385 from martinbudden/bf_use_nav 

Added USE_NAV build flag
This commit is contained in:
J Blackman 2017-06-30 14:48:42 +10:00 committed by GitHub
parent b9f533abe3 c0ce62d601
commit e36ad629e4
2 changed files with 31 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/main/flight/navigation.c
View File

@ -107,19 +107,22 @@ void navigationInit(void)
#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
static bool check_missed_wp(void);
static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist, int32_t * bearing);
//static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing);
static void GPS_calc_longitude_scaling(int32_t lat);
static void GPS_calc_velocity(void);
static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng);
#ifdef USE_NAV
static bool check_missed_wp(void);
static void GPS_calc_poshold(void);
static void GPS_calc_nav_rate(uint16_t max_speed);
static void GPS_update_crosstrack(void);
static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow);
static int32_t wrap_36000(int32_t angle);
#endif
static int32_t wrap_18000(int32_t error);
static int32_t wrap_36000(int32_t angle);
typedef struct {
int16_t last_velocity;
@ -134,7 +137,6 @@ typedef struct {
static PID_PARAM posholdPID_PARAM;
static PID_PARAM poshold_ratePID_PARAM;
static PID_PARAM navPID_PARAM;
typedef struct {
float integrator; // integrator value
@ -146,6 +148,9 @@ typedef struct {
static PID posholdPID[2];
static PID poshold_ratePID[2];
#ifdef USE_NAV
static PID_PARAM navPID_PARAM;
static PID navPID[2];
static int32_t get_P(int32_t error, PID_PARAM *pid)
@ -176,6 +181,7 @@ static int32_t get_D(int32_t input, float *dt, PID *pid, PID_PARAM *pid_param)
// add in derivative component
return pid_param->kD * pid->derivative;
}
#endif
static void reset_PID(PID *pid)
{
@ -197,11 +203,15 @@ static void reset_PID(PID *pid)
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 = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
static int32_t error[2];
#ifdef USE_NAV
// 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];
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path
static int16_t crosstrack_error;
#endif
// Currently used WP
static int32_t GPS_WP[2];
@ -217,8 +227,6 @@ static int32_t target_bearing;
// 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;
@ -265,10 +273,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
void onGpsNewData(void)
{
int axis;
static uint32_t nav_loopTimer;
uint16_t speed;
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return;
@ -283,7 +288,7 @@ void onGpsNewData(void)
// 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++) {
for (int axis = 0; axis < 2; axis++) {
GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // 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
@ -321,6 +326,7 @@ void onGpsNewData(void)
// calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
#ifdef USE_NAV
if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
// we are navigating
@ -328,6 +334,7 @@ void onGpsNewData(void)
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
uint16_t speed;
switch (nav_mode) {
case NAV_MODE_POSHOLD:
// Desired output is in nav_lat and nav_lon where 1deg inclination is 100
@ -360,6 +367,7 @@ void onGpsNewData(void)
break;
}
} //end of gps calcs
#endif
}
void GPS_reset_home_position(void)
@ -385,7 +393,9 @@ void GPS_reset_nav(void)
nav[i] = 0;
reset_PID(&posholdPID[i]);
reset_PID(&poshold_ratePID[i]);
#ifdef USE_NAV
reset_PID(&navPID[i]);
#endif
}
}
@ -401,10 +411,12 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
poshold_ratePID_PARAM.kD = (float)pidProfile->pid[PID_POSR].D / 1000.0f;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#ifdef USE_NAV
navPID_PARAM.kP = (float)pidProfile->pid[PID_NAVR].P / 10.0f;
navPID_PARAM.kI = (float)pidProfile->pid[PID_NAVR].I / 100.0f;
navPID_PARAM.kD = (float)pidProfile->pid[PID_NAVR].D / 1000.0f;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
#endif
}
// OK here is the onboard GPS code
@ -442,6 +454,7 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
waypoint_speed_gov = navigationConfig()->nav_speed_min;
}
#ifdef USE_NAV
////////////////////////////////////////////////////////////////////////////////////
// Check if we missed the destination somehow
//
@ -452,6 +465,7 @@ static bool check_missed_wp(void)
temp = wrap_18000(temp);
return (ABS(temp) > 10000); // we passed the waypoint by 100 degrees
}
#endif
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
#define TAN_89_99_DEGREES 5729.57795f
@ -522,6 +536,7 @@ static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, in
error[LAT] = *target_lat - *gps_lat; // Y Error
}
#ifdef USE_NAV
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
@ -627,6 +642,7 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
}
return max_speed;
}
#endif
////////////////////////////////////////////////////////////////////////////////////
// Utilities
@ -640,6 +656,7 @@ static int32_t wrap_18000(int32_t error)
return error;
}
#ifdef USE_NAV
static int32_t wrap_36000(int32_t angle)
{
if (angle > 36000)
@ -648,6 +665,7 @@ static int32_t wrap_36000(int32_t angle)
angle += 36000;
return angle;
}
#endif
void updateGpsStateForHomeAndHoldMode(void)
{

7
src/main/target/common_fc_pre.h
View File

@ -115,6 +115,7 @@
#define TELEMETRY_SRXL
#define USE_DASHBOARD
#define USE_MSP_DISPLAYPORT
#define USE_RCSPLIT
#define USE_RX_MSP
#define USE_SERIALRX_JETIEXBUS
#define USE_SENSOR_NAMES
@ -131,10 +132,8 @@
#endif
#if (FLASH_SIZE > 256)
// Temporarily moved this here because of overflowing flash size on F3
// Temporarily moved GPS here because of overflowing flash size on F3
#define GPS
#define USE_NAV
#define USE_UNCOMMON_MIXERS
#endif
#define USE_RCSPLIT