From 87b47af57367b832dde642e1787c0b7f04018f3a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 5 Nov 2022 12:07:58 +0000 Subject: [PATCH 1/6] Update navigation.c --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 562f2621041..bdc31c9c7ac 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2148,7 +2148,7 @@ void updateActualHeading(bool headingValid, int32_t newHeading) } posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING; } - posControl.actualState.yaw = newHeading; + posControl.actualState.yaw = isGPSHeadingValid() && STATE(AIRPLANE) ? gpsSol.groundCourse * 10 : newHeading; posControl.flags.estHeadingStatus = newEstHeading; /* Precompute sin/cos of yaw angle */ From d91e2fca622077105ccd0fd2671aadd8830c0122 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 5 Nov 2022 13:11:01 +0000 Subject: [PATCH 2/6] changes --- src/main/navigation/navigation.c | 2 +- src/main/navigation/navigation_pos_estimator.c | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index bdc31c9c7ac..562f2621041 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2148,7 +2148,7 @@ void updateActualHeading(bool headingValid, int32_t newHeading) } posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING; } - posControl.actualState.yaw = isGPSHeadingValid() && STATE(AIRPLANE) ? gpsSol.groundCourse * 10 : newHeading; + posControl.actualState.yaw = newHeading; posControl.flags.estHeadingStatus = newEstHeading; /* Precompute sin/cos of yaw angle */ diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d991ec6ee97..c60448ec451 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -356,11 +356,11 @@ static void restartGravityCalibration(void) } static bool gravityCalibrationComplete(void) -{ +{ if (!gyroConfig()->init_gyro_cal_enabled) { return true; } - + return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } @@ -774,8 +774,10 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) { static navigationTimer_t posPublishTimer; - /* IMU operates in decidegrees while INAV operates in deg*100 */ - updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw)); + /* IMU operates in decidegrees while INAV operates in deg*100 + * Use GPS course over ground for fixed wing nav "heading" when valid */ + int16_t yawValue = isGPSHeadingValid() && STATE(AIRPLANE) ? gpsSol.groundCourse : attitude.values.yaw; + updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(yawValue)); /* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */ if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) { From cfe53aa624caa79f1baca5b4d79e1e9fcf900eb5 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 7 Nov 2022 15:21:39 +0000 Subject: [PATCH 3/6] Update navigation_pos_estimator.c --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index c60448ec451..c825a2bfc4a 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -775,7 +775,7 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) static navigationTimer_t posPublishTimer; /* IMU operates in decidegrees while INAV operates in deg*100 - * Use GPS course over ground for fixed wing nav "heading" when valid */ + * Use GPS course over ground for fixed wing navigation yaw/heading when possible */ int16_t yawValue = isGPSHeadingValid() && STATE(AIRPLANE) ? gpsSol.groundCourse : attitude.values.yaw; updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(yawValue)); From 3d11e105ad91e243fd21347680d460afe9278c96 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 13 Nov 2022 14:18:56 +0000 Subject: [PATCH 4/6] add posEstimator specific course variable --- src/main/fc/fc_tasks.c | 1 + src/main/navigation/navigation.h | 1 + .../navigation/navigation_pos_estimator.c | 47 +++++++++++++------ .../navigation_pos_estimator_private.h | 4 ++ 4 files changed, 39 insertions(+), 14 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 0e0b8c5432d..418f4cbebf3 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -163,6 +163,7 @@ void taskProcessGPS(timeUs_t currentTimeUs) if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTimeUs); + updatePositionEstimator_gpsGroundCourseTopic(); } } #endif diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 339da15392f..b8a34ba910b 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -479,6 +479,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs); void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs); void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt); void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs); +void updatePositionEstimator_gpsGroundCourseTopic(void); /* Navigation system updates */ void updateWaypointsAndNavigationMode(void); diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index c825a2bfc4a..1bb1c4bce12 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -121,6 +121,18 @@ static bool shouldResetReferenceAltitude(void) return false; } +static bool navIsHeadingUsable(void) +{ + if (sensors(SENSOR_GPS)) { + // If we have GPS - we need true IMU north (valid heading) + return isImuHeadingValid(); + } + else { + // If we don't have GPS - we may use whatever we have, other sensors are operating in body frame + return isImuHeadingValid() || positionEstimationConfig()->allow_dead_reckoning; + } +} + #if defined(USE_GPS) /* Why is this here: Because GPS will be sending at quiet a nailed rate (if not overloaded by junk tasks at the brink of its specs) * but we might read out with timejitter because Irq might be off by a few us so we do a +-10% margin around the time between GPS @@ -295,6 +307,25 @@ void onNewGPSData(void) posEstimator.gps.lastUpdateTime = 0; } } + +void updatePositionEstimator_gpsGroundCourseTopic(void) +{ + if (STATE(GPS_FIX) && navIsHeadingUsable()) { + static float lastPositionX = 0; + static float lastPositionY = 0; + + if(gpsStats.lastMessageDt <= INAV_GPS_COG_MAX_UPDATE_TIME_MS) { // use GPS ground course directly if GPS update frequency at least 5Hz + posEstimator.est.cog = gpsSol.groundCourse; + } else { + float deltaPosX = posEstimator.est.pos.x - lastPositionX; + float deltaPosY = posEstimator.est.pos.y - lastPositionY; + uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaPosY, deltaPosX))); + posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse); + } + lastPositionX = posEstimator.est.pos.x; + lastPositionY = posEstimator.est.pos.y; + } +} #endif #if defined(USE_BARO) @@ -474,18 +505,6 @@ static bool navIsAccelerationUsable(void) return true; } -static bool navIsHeadingUsable(void) -{ - if (sensors(SENSOR_GPS)) { - // If we have GPS - we need true IMU north (valid heading) - return isImuHeadingValid(); - } - else { - // If we don't have GPS - we may use whatever we have, other sensors are operating in body frame - return isImuHeadingValid() || positionEstimationConfig()->allow_dead_reckoning; - } -} - static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) { /* Figure out if we have valid position data from our data sources */ @@ -775,8 +794,8 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs) static navigationTimer_t posPublishTimer; /* IMU operates in decidegrees while INAV operates in deg*100 - * Use GPS course over ground for fixed wing navigation yaw/heading when possible */ - int16_t yawValue = isGPSHeadingValid() && STATE(AIRPLANE) ? gpsSol.groundCourse : attitude.values.yaw; + * Use course over ground for fixed wing navigation yaw/"heading" */ + int16_t yawValue = isGPSHeadingValid() && STATE(AIRPLANE) ? posEstimator.est.cog : attitude.values.yaw; updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(yawValue)); /* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */ diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 80c66d2fc01..74ee7f95a47 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -44,6 +44,7 @@ #define INAV_BARO_TIMEOUT_MS 200 // Baro timeout #define INAV_SURFACE_TIMEOUT_MS 400 // Surface timeout (missed 3 readings in a row) #define INAV_FLOW_TIMEOUT_MS 200 +#define INAV_GPS_COG_MAX_UPDATE_TIME_MS 200 // max allowed GPS update period when using GPS ground course directly for FW Nav #define CALIBRATING_GRAVITY_TIME_MS 2000 @@ -125,6 +126,9 @@ typedef struct { // FLOW float flowCoordinates[2]; + + // COURSE + int16_t cog; // course over ground (decidegrees) } navPositionEstimatorESTIMATE_t; typedef struct { From 9c0abbd447c0427e9f918592d55ff089755e0765 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 20 Nov 2022 21:15:57 +0000 Subject: [PATCH 5/6] use velocity --- src/main/fc/fc_tasks.c | 2 +- src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_pos_estimator.c | 15 ++++++--------- 3 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 418f4cbebf3..7d141a8e263 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -163,7 +163,7 @@ void taskProcessGPS(timeUs_t currentTimeUs) if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTimeUs); - updatePositionEstimator_gpsGroundCourseTopic(); + updatePositionEstimator_gpsGroundCourseTopic(currentTimeUs); } } #endif diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b8a34ba910b..d9a6222ca48 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -479,7 +479,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs); void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs); void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt); void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs); -void updatePositionEstimator_gpsGroundCourseTopic(void); +void updatePositionEstimator_gpsGroundCourseTopic(timeUs_t currentTimeUs); /* Navigation system updates */ void updateWaypointsAndNavigationMode(void); diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 1bb1c4bce12..58e420bd555 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -308,22 +308,19 @@ void onNewGPSData(void) } } -void updatePositionEstimator_gpsGroundCourseTopic(void) +void updatePositionEstimator_gpsGroundCourseTopic(timeUs_t currentTimeUs) { if (STATE(GPS_FIX) && navIsHeadingUsable()) { - static float lastPositionX = 0; - static float lastPositionY = 0; + static timeUs_t lastUpdateTimeUs = 0; - if(gpsStats.lastMessageDt <= INAV_GPS_COG_MAX_UPDATE_TIME_MS) { // use GPS ground course directly if GPS update frequency at least 5Hz + if(gpsStats.lastMessageDt <= INAV_GPS_COG_MAX_UPDATE_TIME_MS) { // use GPS ground course directly if GPS update rate at least 5Hz posEstimator.est.cog = gpsSol.groundCourse; } else { - float deltaPosX = posEstimator.est.pos.x - lastPositionX; - float deltaPosY = posEstimator.est.pos.y - lastPositionY; - uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaPosY, deltaPosX))); + const float dt = US2S(currentTimeUs - lastUpdateTimeUs); + uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y * dt, posEstimator.est.vel.x * dt))); posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse); } - lastPositionX = posEstimator.est.pos.x; - lastPositionY = posEstimator.est.pos.y; + lastUpdateTimeUs = currentTimeUs; } } #endif From 7acf4b07da914721623a1dcca86dfff5b39bf845 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 21 Nov 2022 19:21:25 +0000 Subject: [PATCH 6/6] update method --- src/main/fc/fc_tasks.c | 1 - src/main/navigation/navigation.h | 1 - .../navigation/navigation_pos_estimator.c | 57 ++++++++++--------- .../navigation_pos_estimator_private.h | 2 +- 4 files changed, 30 insertions(+), 31 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 7d141a8e263..0e0b8c5432d 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -163,7 +163,6 @@ void taskProcessGPS(timeUs_t currentTimeUs) if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTimeUs); - updatePositionEstimator_gpsGroundCourseTopic(currentTimeUs); } } #endif diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d9a6222ca48..339da15392f 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -479,7 +479,6 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs); void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs); void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt); void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs); -void updatePositionEstimator_gpsGroundCourseTopic(timeUs_t currentTimeUs); /* Navigation system updates */ void updateWaypointsAndNavigationMode(void); diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 58e420bd555..ab979446237 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -121,18 +121,6 @@ static bool shouldResetReferenceAltitude(void) return false; } -static bool navIsHeadingUsable(void) -{ - if (sensors(SENSOR_GPS)) { - // If we have GPS - we need true IMU north (valid heading) - return isImuHeadingValid(); - } - else { - // If we don't have GPS - we may use whatever we have, other sensors are operating in body frame - return isImuHeadingValid() || positionEstimationConfig()->allow_dead_reckoning; - } -} - #if defined(USE_GPS) /* Why is this here: Because GPS will be sending at quiet a nailed rate (if not overloaded by junk tasks at the brink of its specs) * but we might read out with timejitter because Irq might be off by a few us so we do a +-10% margin around the time between GPS @@ -307,22 +295,6 @@ void onNewGPSData(void) posEstimator.gps.lastUpdateTime = 0; } } - -void updatePositionEstimator_gpsGroundCourseTopic(timeUs_t currentTimeUs) -{ - if (STATE(GPS_FIX) && navIsHeadingUsable()) { - static timeUs_t lastUpdateTimeUs = 0; - - if(gpsStats.lastMessageDt <= INAV_GPS_COG_MAX_UPDATE_TIME_MS) { // use GPS ground course directly if GPS update rate at least 5Hz - posEstimator.est.cog = gpsSol.groundCourse; - } else { - const float dt = US2S(currentTimeUs - lastUpdateTimeUs); - uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y * dt, posEstimator.est.vel.x * dt))); - posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse); - } - lastUpdateTimeUs = currentTimeUs; - } -} #endif #if defined(USE_BARO) @@ -502,6 +474,18 @@ static bool navIsAccelerationUsable(void) return true; } +static bool navIsHeadingUsable(void) +{ + if (sensors(SENSOR_GPS)) { + // If we have GPS - we need true IMU north (valid heading) + return isImuHeadingValid(); + } + else { + // If we don't have GPS - we may use whatever we have, other sensors are operating in body frame + return isImuHeadingValid() || positionEstimationConfig()->allow_dead_reckoning; + } +} + static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) { /* Figure out if we have valid position data from our data sources */ @@ -702,6 +686,20 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) return false; } +static void estimationCalculateGroundCourse(timeUs_t currentTimeUs) +{ + if (STATE(GPS_FIX) && navIsHeadingUsable()) { + static timeUs_t lastUpdateTimeUs = 0; + + if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate + const float dt = US2S(currentTimeUs - lastUpdateTimeUs); + uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y * dt, posEstimator.est.vel.x * dt))); + posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse); + lastUpdateTimeUs = currentTimeUs; + } + } +} + /** * Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc) * Function is called at main loop rate @@ -774,6 +772,9 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) } } + /* Update ground course */ + estimationCalculateGroundCourse(currentTimeUs); + /* Update uncertainty */ posEstimator.est.eph = ctx.newEPH; posEstimator.est.epv = ctx.newEPV; diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 74ee7f95a47..dd171fa8e62 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -39,12 +39,12 @@ #define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate #define INAV_PITOT_UPDATE_RATE 10 +#define INAV_COG_UPDATE_RATE_HZ 20 // ground course update rate #define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout #define INAV_BARO_TIMEOUT_MS 200 // Baro timeout #define INAV_SURFACE_TIMEOUT_MS 400 // Surface timeout (missed 3 readings in a row) #define INAV_FLOW_TIMEOUT_MS 200 -#define INAV_GPS_COG_MAX_UPDATE_TIME_MS 200 // max allowed GPS update period when using GPS ground course directly for FW Nav #define CALIBRATING_GRAVITY_TIME_MS 2000