Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 06:15:14 +03:00
Code Issues Wiki Activity

add osd items

Browse source
This commit is contained in:
breadoven 2022-12-04 18:18:14 +00:00
parent b796042318
commit 24917556c2
8 changed files with 48 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/drivers/osd_symbols.h
View file

@ -173,6 +173,8 @@
#define SYM_AH_V_M_1 0xD9 // 217 mAh/v-m right
#define SYM_FLIGHT_MINS_REMAINING 0xDA // 216 Flight time (mins) remaining
#define SYM_FLIGHT_HOURS_REMAINING 0xDB // 217 Flight time (hours) remaining
#define SYM_GROUND_COURSE 0xDC // 220 Ground course
#define SYM_TRACK_ERROR 0xDD // 221 Cross track error
#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo
#define SYM_LOGO_WIDTH 6

27
src/main/io/osd.c
View file

@ -1779,6 +1779,19 @@ static bool osdDrawSingleElement(uint8_t item)
break;
}
case OSD_GROUND_COURSE:
{
buff[0] = SYM_GROUND_COURSE;
if (osdIsHeadingValid()) {
tfp_sprintf(&buff[1], "%3d", (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog));
} else {
buff[1] = buff[2] = buff[3] = '-';
}
buff[4] = SYM_DEGREES;
buff[5] = '\0';
break;
}
case OSD_COURSE_HOLD_ERROR:
{
if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
@ -1825,6 +1838,18 @@ static bool osdDrawSingleElement(uint8_t item)
break;
}
case OSD_CROSS_TRACK_ERROR:
{
if (isWaypointNavTrackingActive()) {
buff[0] = SYM_TRACK_ERROR;
osdFormatDistanceSymbol(buff + 1, navigationGetCrossTrackError(), 0);
} else {
displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
return true;
}
break;
}
case OSD_GPS_HDOP:
{
buff[0] = SYM_HDP_L;
@ -3436,8 +3461,10 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
osdLayoutsConfig->item_pos[0][OSD_GROUND_COURSE] = OSD_POS(12, 3);
osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ERROR] = OSD_POS(12, 2);
osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ADJUSTMENT] = OSD_POS(12, 2);
osdLayoutsConfig->item_pos[0][OSD_CROSS_TRACK_ERROR] = OSD_POS(12, 3);
osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG;
osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;

2
src/main/io/osd.h
View file

@ -266,6 +266,8 @@ typedef enum {
OSD_GLIDE_RANGE,
OSD_CLIMB_EFFICIENCY,
OSD_NAV_WP_MULTI_MISSION_INDEX,
OSD_GROUND_COURSE, // 140
OSD_CROSS_TRACK_ERROR,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;

4
src/main/navigation/navigation.c
View file

@ -2119,7 +2119,7 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
/*-----------------------------------------------------------
* Processes an update to estimated heading
*-----------------------------------------------------------*/
void updateActualHeading(bool headingValid, int32_t newHeading)
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse)
{
/* Update heading. Check if we're acquiring a valid heading for the
* first time and update home heading accordingly.
@ -2150,7 +2150,9 @@ void updateActualHeading(bool headingValid, int32_t newHeading)
}
posControl.rthState.homeFlags |= NAV_HOME_VALID_HEADING;
}
/* Use course over ground for fixed wing nav "heading" when valid - FIXME use heading and cog as specifically required for FW and MR */
posControl.actualState.yaw = newHeading;
posControl.actualState.cog = newGroundCourse; // currently only used for OSD display
posControl.flags.estHeadingStatus = newEstHeading;
/* Precompute sin/cos of yaw angle */

1
src/main/navigation/navigation.h
View file

@ -609,6 +609,7 @@ void updateLandingStatus(void);
const navigationPIDControllers_t* getNavigationPIDControllers(void);
int32_t navigationGetHeadingError(void);
float navigationGetCrossTrackError(void);
int32_t getCruiseHeadingAdjustment(void);
bool isAdjustingPosition(void);
bool isAdjustingHeading(void);

14
src/main/navigation/navigation_fixedwing.c
View file

@ -72,6 +72,7 @@ static bool isYawAdjustmentValid = false;
static float throttleSpeedAdjustment = 0;
static bool isAutoThrottleManuallyIncreased = false;
static int32_t navHeadingError;
static float navCrossTrackError;
static int8_t loiterDirYaw = 1;
bool needToCalculateCircularLoiter;
@ -398,19 +399,19 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
// courseVirtualCorrection initially used to determine current position relative to course line for later use
int32_t courseVirtualCorrection = wrap_18000(posControl.activeWaypoint.yaw - virtualTargetBearing);
float distToCourseLine = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
float navCrossTrackError = ABS(posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(courseVirtualCorrection)));
// tracking only active when certain distance and heading conditions are met
if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) && distToCourseLine > 200) {
if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.yaw)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > 200) {
int32_t courseHeadingError = wrap_18000(posControl.activeWaypoint.yaw - posControl.actualState.yaw);
// captureFactor adjusts distance/heading sensitivity balance when closing in on course line.
// Closing distance threashold based on speed and an assumed 1 second response time.
float captureFactor = distToCourseLine < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f;
float captureFactor = navCrossTrackError < posControl.actualState.velXY ? constrainf(2.0f - ABS(courseHeadingError) / 500.0f, 0.0f, 2.0f) : 1.0f;
// bias between reducing distance to course line and aligning with course heading adjusted by waypoint_tracking_accuracy
// initial courseCorrectionFactor based on distance to course line
float courseCorrectionFactor = constrainf(captureFactor * distToCourseLine / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
float courseCorrectionFactor = constrainf(captureFactor * navCrossTrackError / (1000.0f * navConfig()->fw.wp_tracking_accuracy), 0.0f, 1.0f);
courseCorrectionFactor = courseVirtualCorrection < 0 ? -courseCorrectionFactor : courseCorrectionFactor;
// course heading alignment factor
@ -825,3 +826,8 @@ int32_t navigationGetHeadingError(void)
{
return navHeadingError;
}
float navigationGetCrossTrackError(void)
{
return navCrossTrackError;
}

2
src/main/navigation/navigation_pos_estimator.c
View file

@ -794,7 +794,7 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
/* 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));
updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(yawValue), DECIDEGREES_TO_CENTIDEGREES(posEstimator.est.cog));
/* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {

3
src/main/navigation/navigation_private.h
View file

@ -124,6 +124,7 @@ typedef struct {
navEstimatedPosVel_t abs;
navEstimatedPosVel_t agl;
int32_t yaw;
int32_t cog;
// Service values
float sinYaw;
@ -455,7 +456,7 @@ bool isLastMissionWaypoint(void);
float getActiveWaypointSpeed(void);
bool isWaypointNavTrackingActive(void);
void updateActualHeading(bool headingValid, int32_t newHeading);
void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus);