Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Use course over ground as the default for fixed wing navigation "heading" #8529

Merged
merged 6 commits into from
Nov 22, 2022
Merged

Use course over ground as the default for fixed wing navigation "heading" #8529

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
87b47af
Update navigation.c
breadoven Nov 5, 2022
d91e2fc
changes
breadoven Nov 5, 2022
cfe53aa
Update navigation_pos_estimator.c
breadoven Nov 7, 2022
3d11e10
add posEstimator specific course variable
breadoven Nov 13, 2022
9c0abbd
use velocity
breadoven Nov 20, 2022
7acf4b0
update method
breadoven Nov 21, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 23 additions & 4 deletions src/main/navigation/navigation_pos_estimator.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down Expand Up @@ -686,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
Expand Down Expand Up @@ -758,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;
Expand All @@ -774,8 +791,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 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 */
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
Expand Down
4 changes: 4 additions & 0 deletions src/main/navigation/navigation_pos_estimator_private.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@

#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
Expand Down Expand Up @@ -125,6 +126,9 @@ typedef struct {

// FLOW
float flowCoordinates[2];

// COURSE
int16_t cog; // course over ground (decidegrees)
} navPositionEstimatorESTIMATE_t;

typedef struct {
Expand Down