mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 00:05:28 +03:00
Add cruise mode heading change and heading error OSD indicators
This commit is contained in:
parent
a5b6d49851
commit
c9a69a6232
7 changed files with 94 additions and 11 deletions
|
@ -190,6 +190,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
|
|||
#endif // GPS
|
||||
OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING),
|
||||
OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH),
|
||||
OSD_ELEMENT_ENTRY("CRS HEAD. ERR", OSD_CRUISE_HEADING_ERROR),
|
||||
OSD_ELEMENT_ENTRY("CRS HEAD. ADJ", OSD_CRUISE_HEADING_ADJUSTMENT),
|
||||
#if defined(USE_BARO) || defined(USE_GPS)
|
||||
OSD_ELEMENT_ENTRY("VARIO", OSD_VARIO),
|
||||
OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM),
|
||||
|
|
|
@ -1257,6 +1257,53 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
buff[5] = '\0';
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_CRUISE_HEADING_ERROR:
|
||||
{
|
||||
if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_CRUISE_MODE)) {
|
||||
displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
|
||||
return true;
|
||||
}
|
||||
|
||||
buff[0] = SYM_HEADING;
|
||||
|
||||
if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_CRUISE_MODE) && isAdjustingPosition())) {
|
||||
buff[1] = buff[2] = buff[3] = '-';
|
||||
} else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
|
||||
int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError()));
|
||||
if (ABS(herr) > 99)
|
||||
strcpy(buff + 1, ">99");
|
||||
else
|
||||
tfp_sprintf(buff + 1, "%3d", herr);
|
||||
}
|
||||
|
||||
buff[4] = SYM_DEGREES;
|
||||
buff[5] = '\0';
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_CRUISE_HEADING_ADJUSTMENT:
|
||||
{
|
||||
int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment()));
|
||||
|
||||
if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_CRUISE_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) {
|
||||
displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
|
||||
return true;
|
||||
}
|
||||
|
||||
buff[0] = SYM_HEADING;
|
||||
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
buff[1] = buff[2] = buff[3] = buff[4] = '-';
|
||||
} else if (FLIGHT_MODE(NAV_CRUISE_MODE)) {
|
||||
tfp_sprintf(buff + 1, "%4d", heading_adjust);
|
||||
}
|
||||
|
||||
buff[5] = SYM_DEGREES;
|
||||
buff[6] = '\0';
|
||||
break;
|
||||
}
|
||||
|
||||
case OSD_GPS_HDOP:
|
||||
{
|
||||
buff[0] = SYM_HDP_L;
|
||||
|
@ -2194,6 +2241,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
|
|||
osdConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
|
||||
osdConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
|
||||
osdConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
|
||||
osdConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2);
|
||||
osdConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2);
|
||||
osdConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
|
||||
osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(1, 3) | OSD_VISIBLE_FLAG;
|
||||
osdConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
|
||||
|
|
|
@ -113,6 +113,8 @@ typedef enum {
|
|||
OSD_REMAINING_DISTANCE_BEFORE_RTH,
|
||||
OSD_POWER_SUPPLY_IMPEDANCE,
|
||||
OSD_HOME_HEADING_ERROR,
|
||||
OSD_CRUISE_HEADING_ERROR,
|
||||
OSD_CRUISE_HEADING_ADJUSTMENT,
|
||||
OSD_ITEM_COUNT // MUST BE LAST
|
||||
} osd_items_e;
|
||||
|
||||
|
|
|
@ -916,6 +916,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na
|
|||
}
|
||||
|
||||
posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
|
||||
posControl.cruise.previousYaw = posControl.cruise.yaw;
|
||||
posControl.cruise.lastYawAdjustmentTime = 0;
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
|
||||
}
|
||||
|
@ -923,7 +925,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na
|
|||
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState)
|
||||
{
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
static timeMs_t lastYawChangeTime = 0;
|
||||
|
||||
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
|
||||
|
||||
|
@ -936,15 +937,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n
|
|||
|
||||
// User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
|
||||
if (posControl.flags.isAdjustingHeading) {
|
||||
timeMs_t timeDifference = currentTimeMs - lastYawChangeTime;
|
||||
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
|
||||
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
|
||||
float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
|
||||
float centidegsPerIteration = rateTarget * timeDifference / 1000.0f;
|
||||
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
|
||||
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
|
||||
lastYawChangeTime = currentTimeMs;
|
||||
posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
|
||||
}
|
||||
|
||||
if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000)
|
||||
posControl.cruise.previousYaw = posControl.cruise.yaw;
|
||||
|
||||
uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm]
|
||||
|
||||
if ((previousState == NAV_STATE_CRUISE_2D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_2D_ADJUSTING)
|
||||
|
@ -969,10 +973,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(nav
|
|||
|
||||
// User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
|
||||
if (posControl.flags.isAdjustingPosition) {
|
||||
posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
|
||||
posControl.cruise.lastYawAdjustmentTime = millis();
|
||||
return NAV_FSM_EVENT_NONE; // reprocess the state
|
||||
}
|
||||
|
||||
posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
|
||||
resetPositionController();
|
||||
|
||||
return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
|
||||
|
@ -3073,6 +3078,18 @@ const navigationPIDControllers_t* getNavigationPIDControllers(void) {
|
|||
return &posControl.pids;
|
||||
}
|
||||
|
||||
bool isAdjustingPosition(void) {
|
||||
return posControl.flags.isAdjustingPosition;
|
||||
}
|
||||
|
||||
bool isAdjustingHeading(void) {
|
||||
return posControl.flags.isAdjustingHeading;
|
||||
}
|
||||
|
||||
int32_t getCruiseHeadingAdjustment(void) {
|
||||
return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
|
||||
}
|
||||
|
||||
#else // NAV
|
||||
|
||||
#ifdef USE_GPS
|
||||
|
|
|
@ -394,6 +394,11 @@ float calculateAverageSpeed();
|
|||
|
||||
const navigationPIDControllers_t* getNavigationPIDControllers(void);
|
||||
|
||||
int32_t navigationGetHeadingError(void);
|
||||
int32_t getCruiseHeadingAdjustment(void);
|
||||
bool isAdjustingPosition(void);
|
||||
bool isAdjustingHeading(void);
|
||||
|
||||
/* Returns the heading recorded when home position was acquired.
|
||||
* Note that the navigation system uses deg*100 as unit and angles
|
||||
* are in the [0, 360 * 100) interval.
|
||||
|
|
|
@ -62,6 +62,7 @@ static bool isPitchAdjustmentValid = false;
|
|||
static bool isRollAdjustmentValid = false;
|
||||
static float throttleSpeedAdjustment = 0;
|
||||
static bool isAutoThrottleManuallyIncreased = false;
|
||||
static int32_t navHeadingError;
|
||||
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
@ -280,29 +281,29 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
|
||||
|
||||
// Calculate NAV heading error
|
||||
int32_t headingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
|
||||
navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
|
||||
|
||||
// Forced turn direction
|
||||
// If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
|
||||
if (ABS(headingError) > 17000) {
|
||||
if (ABS(navHeadingError) > 17000) {
|
||||
forceTurnDirection = true;
|
||||
}
|
||||
else if (ABS(headingError) < 9000 && forceTurnDirection) {
|
||||
else if (ABS(navHeadingError) < 9000 && forceTurnDirection) {
|
||||
forceTurnDirection = false;
|
||||
}
|
||||
|
||||
// If forced turn direction flag is enabled we fix the sign of the direction
|
||||
if (forceTurnDirection) {
|
||||
headingError = ABS(headingError);
|
||||
navHeadingError = ABS(navHeadingError);
|
||||
}
|
||||
|
||||
// Slow error monitoring (2Hz rate)
|
||||
if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
|
||||
// Check if error is decreasing over time
|
||||
errorIsDecreasing = (ABS(previousHeadingError) > ABS(headingError));
|
||||
errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError));
|
||||
|
||||
// Save values for next iteration
|
||||
previousHeadingError = headingError;
|
||||
previousHeadingError = navHeadingError;
|
||||
previousTimeMonitoringUpdate = currentTimeUs;
|
||||
}
|
||||
|
||||
|
@ -310,7 +311,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0);
|
||||
|
||||
// Input error in (deg*100), output pitch angle (deg*100)
|
||||
float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + headingError, posControl.actualState.yaw, US2S(deltaMicros),
|
||||
float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + navHeadingError, posControl.actualState.yaw, US2S(deltaMicros),
|
||||
-DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
|
||||
DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
|
||||
pidFlags);
|
||||
|
@ -582,4 +583,9 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
|
|||
}
|
||||
}
|
||||
|
||||
int32_t navigationGetHeadingError(void)
|
||||
{
|
||||
return navHeadingError;
|
||||
}
|
||||
|
||||
#endif // NAV
|
||||
|
|
|
@ -294,6 +294,8 @@ typedef struct {
|
|||
typedef struct {
|
||||
fpVector3_t targetPos;
|
||||
int32_t yaw;
|
||||
int32_t previousYaw;
|
||||
timeMs_t lastYawAdjustmentTime;
|
||||
} navCruise_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue