1
0
Fork 0
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:
Michel Pastor 2018-06-13 17:18:56 +02:00
parent a5b6d49851
commit c9a69a6232
7 changed files with 94 additions and 11 deletions

View file

@ -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),

View file

@ -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;

View file

@ -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;

View file

@ -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

View file

@ -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.

View file

@ -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

View file

@ -294,6 +294,8 @@ typedef struct {
typedef struct {
fpVector3_t targetPos;
int32_t yaw;
int32_t previousYaw;
timeMs_t lastYawAdjustmentTime;
} navCruise_t;
typedef struct {