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

Merge pull request #4607 from martinbudden/bfa_gps_nav_separation 

Better separation between GPS and NAV
This commit is contained in:
Martin Budden 2017-11-19 04:42:48 +00:00 committed by GitHub
parent a9105e88c4 cfc87d1891
commit 5d9e6485c4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
17 changed files with 243 additions and 218 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/blackbox/blackbox.c
View File

@ -54,7 +54,6 @@
#include "flight/failsafe.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"

2
src/main/fc/config.c
View File

@ -301,7 +301,7 @@ void activateConfig(void)
useRcControlsConfig(currentPidProfile);
useAdjustmentConfig(currentPidProfile);
#ifdef USE_GPS
#ifdef USE_NAV
gpsUsePIDs(currentPidProfile);
#endif

4
src/main/fc/fc_core.c
View File

@ -553,7 +553,7 @@ void processRx(timeUs_t currentTimeUs)
}
#endif
#ifdef USE_GPS
#ifdef USE_NAV
if (sensors(SENSOR_GPS)) {
updateGpsWaypointsAndMode();
}
@ -649,7 +649,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
processRcCommand();
#ifdef USE_GPS
#ifdef USE_NAV
if (sensors(SENSOR_GPS)) {
if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
updateGpsStateForHomeAndHoldMode();

2
src/main/fc/fc_init.c
View File

@ -607,7 +607,9 @@ void init(void)
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {
gpsInit();
#ifdef USE_NAV
navigationInit();
#endif
}
#endif

1
src/main/fc/rc_controls.c
View File

@ -60,7 +60,6 @@
#include "scheduler/scheduler.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "flight/failsafe.h"
static pidProfile_t *pidProfile;

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

@ -22,7 +22,7 @@
#include "platform.h"
#ifdef USE_GPS
#ifdef USE_NAV
#include "build/debug.h"
@ -72,16 +72,11 @@ bool areSticksInApModePosition(uint16_t ap_mode);
// **********************
// GPS
// **********************
int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 }; // it's the angles that must be applied for GPS correction
int32_t GPS_home[2];
int32_t GPS_hold[2];
uint16_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home or hol point in degrees
static int16_t nav[2];
static int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother
navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode
// When using PWM input GPS usage reduces number of available channels by 2 - see pwm_common.c/pwmInit()
void navigationInit(void)
@ -104,23 +99,17 @@ void navigationInit(void)
#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
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);
@ -149,7 +138,6 @@ typedef struct {
static PID posholdPID[2];
static PID poshold_ratePID[2];
#ifdef USE_NAV
static PID_PARAM navPID_PARAM;
static PID navPID[2];
@ -181,7 +169,6 @@ 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)
{
@ -190,8 +177,6 @@ static void reset_PID(PID *pid)
pid->last_derivative = 0;
}
#define GPS_X 1
#define GPS_Y 0
/****************** PI and PID controllers for GPS ********************///32938 -> 33160
@ -200,18 +185,13 @@ static void reset_PID(PID *pid)
#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 = 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];
// 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];
@ -238,95 +218,14 @@ 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
static uint8_t GPS_filter_index = 0;
static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
static int32_t GPS_filter_sum[2];
static int32_t GPS_read[2];
static int32_t GPS_filtered[2];
static int32_t GPS_degree[2]; //the lat lon degree without any decimals (lat/10 000 000)
static uint16_t fraction3[2];
// 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_calculateDistanceAndDirectionToHome(void)
void navNewGpsData(void)
{
if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
} else {
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
}
}
void onGpsNewData(void)
{
static uint32_t nav_loopTimer;
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return;
}
if (!ARMING_FLAG(ARMED))
DISABLE_STATE(GPS_FIX_HOME);
if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(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 (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
// 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) {
if (axis == LAT) {
gpsSol.llh.lat = GPS_filtered[LAT];
} else {
gpsSol.llh.lon = GPS_filtered[LON];
}
}
}
}
#endif
//
// Calculate time delta for navigation loop, range 0-1.0f, in seconds
//
// 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);
GPS_calculateDistanceAndDirectionToHome();
// 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
@ -367,19 +266,6 @@ void onGpsNewData(void)
break;
}
} //end of gps calcs
#endif
}
void GPS_reset_home_position(void)
{
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading
// Set ground altitude
ENABLE_STATE(GPS_FIX_HOME);
}
}
// reset navigation (stop the navigation processor, and clear nav)
@ -393,9 +279,7 @@ 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
}
}
@ -411,12 +295,10 @@ 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
@ -431,12 +313,6 @@ void gpsUsePIDs(pidProfile_t *pidProfile)
// 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) / 10000000.0f) * 0.0174532925f;
GPS_scaleLonDown = cos_approx(rads);
}
////////////////////////////////////////////////////////////////////////////////////
// Sets the waypoint to navigate, reset neccessary variables and calculate initial values
//
@ -454,7 +330,6 @@ 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
//
@ -465,24 +340,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
////////////////////////////////////////////////////////////////////////////////////
// Get distance between two points in cm
// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)
{
float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
*dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;
*bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg
if (*bearing < 0)
*bearing += 36000;
}
////////////////////////////////////////////////////////////////////////////////////
// keep old calculation function for compatibility (could be removed later) distance in meters, bearing in degree
@ -495,32 +353,6 @@ static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1,
// *bearing = d2 / 100; //convert to degrees
//}
////////////////////////////////////////////////////////////////////////////////////
// Calculate our current speed vector from gps position data
//
static void GPS_calc_velocity(void)
{
static int16_t speed_old[2] = { 0, 0 };
static int32_t last_coord[2] = { 0, 0 };
static uint8_t init = 0;
if (init) {
float tmp = 1.0f / dTnav;
actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
speed_old[GPS_X] = actual_speed[GPS_X];
speed_old[GPS_Y] = actual_speed[GPS_Y];
}
init = 1;
last_coord[LON] = gpsSol.llh.lon;
last_coord[LAT] = gpsSol.llh.lat;
}
////////////////////////////////////////////////////////////////////////////////////
// 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:
@ -536,7 +368,6 @@ 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
//
@ -642,7 +473,6 @@ static uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow)
}
return max_speed;
}
#endif
////////////////////////////////////////////////////////////////////////////////////
// Utilities
@ -656,7 +486,6 @@ static int32_t wrap_18000(int32_t error)
return error;
}
#ifdef USE_NAV
static int32_t wrap_36000(int32_t angle)
{
if (angle > 36000)
@ -665,7 +494,6 @@ static int32_t wrap_36000(int32_t angle)
angle += 36000;
return angle;
}
#endif
void updateGpsStateForHomeAndHoldMode(void)
{

15
src/main/flight/navigation.h
View File

@ -18,13 +18,8 @@
#pragma once
#include "common/axis.h"
#include "io/gps.h"
// navigation mode
typedef enum {
NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} navigationMode_e;
// FIXME ap_mode is badly named, it's a value that is compared to rcCommand, not a flag at it's name implies.
@ -40,22 +35,14 @@ typedef struct navigationConfig_s {
PG_DECLARE(navigationConfig_t, navigationConfig);
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
extern int32_t GPS_home[2];
extern int32_t GPS_hold[2];
extern uint16_t GPS_distanceToHome; // distance to home point in meters
extern int16_t GPS_directionToHome; // direction to home or hol point in degrees
extern navigationMode_e nav_mode; // Navigation mode
void navigationInit(void);
void GPS_reset_home_position(void);
void GPS_reset_nav(void);
void GPS_set_next_wp(int32_t* lat, int32_t* lon);
void gpsUsePIDs(struct pidProfile_s *pidProfile);
void updateGpsStateForHomeAndHoldMode(void);
void updateGpsWaypointsAndMode(void);
void onGpsNewData(void);

3
src/main/flight/pid.c
View File

@ -45,7 +45,8 @@
#include "flight/pid.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "io/gps.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"

11
src/main/interface/msp.c
View File

@ -1251,7 +1251,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sb
}
#endif // USE_OSD_SLAVE
#ifdef USE_GPS
#ifdef USE_NAV
static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src)
{
uint8_t wp_no;
@ -1312,7 +1312,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
uint32_t i;
uint8_t value;
const unsigned int dataSize = sbufBytesRemaining(src);
#ifdef USE_GPS
#ifdef USE_NAV
uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0;
#endif
@ -1743,7 +1743,8 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
gpsSol.groundSpeed = sbufReadU16(src);
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
break;
#endif // USE_GPS
#ifdef USE_NAV
case MSP_SET_WP:
wp_no = sbufReadU8(src); //get the wp number
lat = sbufReadU32(src);
@ -1768,7 +1769,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
}
break;
#endif
#endif // USE_NAV
case MSP_SET_FEATURE_CONFIG:
featureClearAll();
@ -2177,7 +2178,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
mspFc4waySerialCommand(dst, src, mspPostProcessFn);
ret = MSP_RESULT_ACK;
#endif
#ifdef USE_GPS
#ifdef USE_NAV
} else if (cmdMSP == MSP_WP) {
mspFcWpCommand(dst, src);
ret = MSP_RESULT_ACK;

188
src/main/io/gps.c
View File

@ -48,7 +48,7 @@
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "flight/navigation.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "sensors/sensors.h"
@ -71,6 +71,28 @@ static char *gpsPacketLogChar = gpsPacketLog;
// **********************
// GPS
// **********************
int32_t GPS_home[2];
uint16_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home or hol point in degrees
int16_t GPS_angle[ANGLE_INDEX_COUNT] = { 0, 0 }; // it's the angles that must be applied for GPS correction
float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
int16_t actual_speed[2] = { 0, 0 };
int16_t nav_takeoff_bearing;
navigationMode_e nav_mode = NAV_MODE_NONE; // Navigation mode
// moving average filter variables
#define GPS_FILTERING 1 // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency
#ifdef GPS_FILTERING
#define GPS_FILTER_VECTOR_LENGTH 5
static uint8_t GPS_filter_index = 0;
static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
static int32_t GPS_filter_sum[2];
static int32_t GPS_read[2];
static int32_t GPS_filtered[2];
static int32_t GPS_degree[2]; //the lat lon degree without any decimals (lat/10 000 000)
static uint16_t fraction3[2];
#endif
gpsSolutionData_t gpsSol;
uint32_t GPS_packetCount = 0;
uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
@ -111,6 +133,7 @@ static const gpsInitData_t gpsInitData[] = {
#define DEFAULT_BAUD_RATE_INDEX 0
#ifdef USE_GPS_UBLOX
static const uint8_t ubloxInit[] = {
//Preprocessor Pedestrian Dynamic Platform Model Option
#if defined(GPS_UBLOX_MODE_PEDESTRIAN)
@ -179,7 +202,7 @@ static const ubloxSbas_t ubloxSbas[] = {
{ SBAS_MSAS, { 0x00, 0x02, 0x02, 0x00, 0x35, 0xEF}},
{ SBAS_GAGAN, { 0x80, 0x01, 0x00, 0x00, 0xB2, 0xE8}}
};
#endif // USE_GPS_UBLOX
typedef enum {
GPS_UNKNOWN,
@ -212,8 +235,12 @@ static void shiftPacketLog(void)
}
static void gpsNewData(uint16_t c);
#ifdef USE_GPS_NMEA
static bool gpsNewFrameNMEA(char c);
#endif
#ifdef USE_GPS_UBLOX
static bool gpsNewFrameUBLOX(uint8_t data);
#endif
static void gpsSetState(gpsState_e state)
{
@ -268,6 +295,7 @@ void gpsInit(void)
gpsSetState(GPS_INITIALIZING);
}
#ifdef USE_GPS_NMEA
void gpsInitNmea(void)
{
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
@ -316,7 +344,9 @@ void gpsInitNmea(void)
break;
}
}
#endif // USE_GPS_NMEA
#ifdef USE_GPS_UBLOX
void gpsInitUblox(void)
{
uint32_t now;
@ -400,16 +430,21 @@ void gpsInitUblox(void)
break;
}
}
#endif // USE_GPS_UBLOX
void gpsInitHardware(void)
{
switch (gpsConfig()->provider) {
case GPS_NMEA:
#ifdef USE_GPS_NMEA
gpsInitNmea();
#endif
break;
case GPS_UBLOX:
#ifdef USE_GPS_UBLOX
gpsInitUblox();
#endif
break;
}
}
@ -495,11 +530,16 @@ bool gpsNewFrame(uint8_t c)
{
switch (gpsConfig()->provider) {
case GPS_NMEA: // NMEA
#ifdef USE_GPS_NMEA
return gpsNewFrameNMEA(c);
#endif
break;
case GPS_UBLOX: // UBX binary
#ifdef USE_GPS_UBLOX
return gpsNewFrameUBLOX(c);
#endif
break;
}
return false;
}
@ -566,6 +606,7 @@ static uint32_t GPS_coord_to_degrees(char *coordinateString)
*/
// helper functions
#ifdef USE_GPS_NMEA
static uint32_t grab_fields(char *src, uint8_t mult)
{ // convert string to uint32
uint32_t i;
@ -764,7 +805,9 @@ static bool gpsNewFrameNMEA(char c)
}
return frameOK;
}
#endif // USE_GPS_NMEA
#ifdef USE_GPS_UBLOX
// UBX support
typedef struct {
uint8_t preamble1;
@ -1086,6 +1129,7 @@ static bool gpsNewFrameUBLOX(uint8_t data)
}
return parsed;
}
#endif // USE_GPS_UBLOX
static void gpsHandlePassthrough(uint8_t data)
{
@ -1114,4 +1158,142 @@ void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort)
serialPassthrough(gpsPort, gpsPassthroughPort, &gpsHandlePassthrough, NULL);
}
float GPS_scaleLonDown = 1.0f; // this is used to offset the shrinking longitude as we go towards the poles
void GPS_calc_longitude_scaling(int32_t lat)
{
float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f;
GPS_scaleLonDown = cos_approx(rads);
}
void GPS_reset_home_position(void)
{
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
#ifdef USE_NAV
nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading
#endif
// Set ground altitude
ENABLE_STATE(GPS_FIX_HOME);
}
}
////////////////////////////////////////////////////////////////////////////////////
#define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS 1.113195f
#define TAN_89_99_DEGREES 5729.57795f
// Get distance between two points in cm
// Get bearing from pos1 to pos2, returns an 1deg = 100 precision
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing)
{
float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees
float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
*dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;
*bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg
if (*bearing < 0)
*bearing += 36000;
}
void GPS_calculateDistanceAndDirectionToHome(void)
{
if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
} else {
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
}
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate our current speed vector from gps position data
//
static void GPS_calc_velocity(void)
{
static int16_t speed_old[2] = { 0, 0 };
static int32_t last_coord[2] = { 0, 0 };
static uint8_t init = 0;
if (init) {
float tmp = 1.0f / dTnav;
actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
speed_old[GPS_X] = actual_speed[GPS_X];
speed_old[GPS_Y] = actual_speed[GPS_Y];
}
init = 1;
last_coord[LON] = gpsSol.llh.lon;
last_coord[LAT] = gpsSol.llh.lat;
}
void onGpsNewData(void)
{
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return;
}
if (!ARMING_FLAG(ARMED))
DISABLE_STATE(GPS_FIX_HOME);
if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(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 (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
// 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) {
if (axis == LAT) {
gpsSol.llh.lat = GPS_filtered[LAT];
} else {
gpsSol.llh.lon = GPS_filtered[LON];
}
}
}
}
#endif
//
// Calculate time delta for navigation loop, range 0-1.0f, in seconds
//
// Time for calculating x,y speed and navigation pids
static uint32_t nav_loopTimer;
dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = MIN(dTnav, 1.0f);
GPS_calculateDistanceAndDirectionToHome();
// 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
navNewGpsData();
#endif
}
#endif

24
src/main/io/gps.h
View File

@ -17,6 +17,7 @@
#pragma once
#include "common/axis.h"
#include "common/time.h"
#include "config/parameter_group.h"
@ -25,6 +26,8 @@
#define LON 1
#define GPS_DEGREES_DIVIDER 10000000L
#define GPS_X 1
#define GPS_Y 0
typedef enum {
GPS_NMEA = 0,
@ -114,6 +117,22 @@ typedef struct gpsData_s {
#define GPS_PACKET_LOG_ENTRY_COUNT 21 // To make this useful we should log as many packets as we can fit characters a single line of a OLED display.
extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
extern int32_t GPS_home[2];
extern uint16_t GPS_distanceToHome; // distance to home point in meters
extern int16_t GPS_directionToHome; // direction to home or hol point in degrees
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
extern float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read
extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles
extern int16_t actual_speed[2];
extern int16_t nav_takeoff_bearing;
// navigation mode
typedef enum {
NAV_MODE_NONE = 0,
NAV_MODE_POSHOLD,
NAV_MODE_WP
} navigationMode_e;
extern navigationMode_e nav_mode; // Navigation mode
extern gpsData_t gpsData;
extern gpsSolutionData_t gpsSol;
@ -134,4 +153,9 @@ void gpsUpdate(timeUs_t currentTimeUs);
bool gpsNewFrame(uint8_t c);
struct serialPort_s;
void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);
void onGpsNewData(void);
void GPS_reset_home_position(void);
void GPS_calc_longitude_scaling(int32_t lat);
void navNewGpsData(void);
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);

3
src/main/target/RCEXPLORERF3/target.h
View File

@ -104,6 +104,9 @@
#define SERIALRX_PROVIDER SERIALRX_SBUS
#define SERIALRX_UART SERIAL_PORT_USART1
#define USE_GPS
#define USE_GPS_UBLOX
#define USE_GPS_NMEA
#define USE_NAV
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

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

@ -145,6 +145,8 @@
#if (FLASH_SIZE > 256)
// Temporarily moved GPS here because of overflowing flash size on F3
#define USE_GPS
#define USE_GPS_UBLOX
#define USE_GPS_NMEA
#define USE_NAV
#define USE_UNCOMMON_MIXERS
#define USE_OSD_ADJUSTMENTS

1
src/main/telemetry/hott.c
View File

@ -74,7 +74,6 @@
#include "flight/altitude.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "io/gps.h"

1
src/main/telemetry/ltm.c
View File

@ -70,7 +70,6 @@
#include "flight/imu.h"
#include "flight/failsafe.h"
#include "flight/altitude.h"
#include "flight/navigation.h"
#include "telemetry/telemetry.h"
#include "telemetry/ltm.h"

1
src/main/telemetry/smartport.c
View File

@ -36,7 +36,6 @@
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/navigation.h"
#include "interface/msp.h"

2
src/test/unit/osd_unittest.cc
View File

@ -57,7 +57,7 @@ extern "C" {
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint8_t GPS_numSat;
uint16_t GPS_distanceToHome;
uint16_t GPS_directionToHome;
int16_t GPS_directionToHome;
int32_t GPS_coord[2];
gpsSolutionData_t gpsSol;