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

WIP: 2D position controller

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-04-13 16:39:28 +02:00
parent f933381d18
commit eff264f515
2 changed files with 57 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -203,6 +203,9 @@ bool adjustFixedWingHeadingFromRCInput(void)
static fpVector3_t virtualDesiredPosition;
static pt1Filter_t fwPosControllerCorrectionFilterState;
/*
* TODO Currently this function resets both FixedWing and Rover & Boat position controller
*/
void resetFixedWingPositionController(void)
{
virtualDesiredPosition.x = 0;

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

@ -35,6 +35,57 @@ FILE_COMPILE_FOR_SIZE
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
static bool isYawAdjustmentValid = false;
void applyRoverBoatPositionController(timeUs_t currentTimeUs)
{
static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
previousTimeUpdate = currentTimeUs;
// If last position update was too long in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTimeUs;
previousTimePositionUpdate = currentTimeUs;
resetFixedWingPositionController();
return;
}
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
// If we have new position - update velocity and acceleration controllers
if (posControl.flags.horizontalPositionDataNew) {
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
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 {
resetFixedWingPositionController();
}
// Indicate that information is no longer usable
posControl.flags.horizontalPositionDataConsumed = 1;
}
isYawAdjustmentValid = true;
}
else {
isYawAdjustmentValid = false;
}
}
void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
@ -43,9 +94,9 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
if (navStateFlags & NAV_CTL_POS) {
// if (isYawAdjustmentValid) {
if (isYawAdjustmentValid) {
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);