Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Heading controller

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-04-13 19:21:05 +02:00
parent eff264f515
commit c58e28c709
3 changed files with 62 additions and 40 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -289,6 +289,34 @@ bool adjustFixedWingPositionFromRCInput(void)
return (rcRollAdjustment);
}
float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing) {
static float limit = 0.0f;
if (limit == 0.0f) {
limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
}
const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
const float yawAdjustment = navPidApply2(
&posControl.pids.fw_heading,
0,
applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100),
US2S(deltaMicros),
-limit,
limit,
yawPidFlags
) / 100.0f;
DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
return yawAdjustment;
}
static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
{
static timeUs_t previousTimeMonitoringUpdate;
@ -349,32 +377,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
* It is working in relative mode and we aim to keep error at zero
*/
if (STATE(FW_HEADING_USE_YAW)) {
static float limit = 0.0f;
if (limit == 0.0f) {
limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
}
const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
float yawAdjustment = navPidApply2(
&posControl.pids.fw_heading,
0,
applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100),
US2S(deltaMicros),
-limit,
limit,
yawPidFlags
) / 100.0f;
DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
posControl.rcAdjustment[YAW] = yawAdjustment;
posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
} else {
posControl.rcAdjustment[YAW] = 0;
}

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

@ -445,7 +445,7 @@ bool adjustFixedWingHeadingFromRCInput(void);
bool adjustFixedWingPositionFromRCInput(void);
void applyFixedWingPositionController(timeUs_t currentTimeUs);
float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing);
void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);

45
src/main/navigation/navigation_rover_boat.c
View file

@ -28,6 +28,7 @@ FILE_COMPILE_FOR_SIZE
#ifdef USE_NAV
#include "build/debug.h"
#include "common/utils.h"
#include "fc/rc_controls.h"
#include "flight/mixer.h"
@ -36,6 +37,34 @@ FILE_COMPILE_FOR_SIZE
#include "navigation/navigation_private.h"
static bool isYawAdjustmentValid = false;
static int32_t navHeadingError;
static void update2DPositionHeadingController(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
{
static timeUs_t previousTimeMonitoringUpdate;
static int32_t previousHeadingError;
static bool errorIsDecreasing;
int32_t targetBearing = calculateBearingToDestination(&posControl.desiredState.pos);
/*
* Calculate NAV heading error
* Units are centidegrees
*/
navHeadingError = wrap_18000(targetBearing - posControl.actualState.yaw);
// 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(navHeadingError));
// Save values for next iteration
previousHeadingError = navHeadingError;
previousTimeMonitoringUpdate = currentTimeUs;
}
posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
}
void applyRoverBoatPositionController(timeUs_t currentTimeUs)
{
@ -61,17 +90,8 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs)
previousTimePositionUpdate = currentTimeUs;
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
// Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ)
// Account for pilot's roll input (move position target left/right at max of max_manual_speed)
// POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
// FIXME: verify the above
// calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2);
// updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate);
//FIXME build a simple 2D position controller
}
else {
update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate);
} else {
resetFixedWingPositionController();
}
@ -98,7 +118,6 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[YAW] = posControl.rcAdjustment[YAW];
}
// const uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
}
}
@ -111,7 +130,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
rcCommand[YAW] = 0;
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
} else if (navStateFlags & NAV_CTL_POS) {
applyFixedWingPositionController(currentTimeUs);
applyRoverBoatPositionController(currentTimeUs);
applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
}
}