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

Remove POSHOLD_2D mode (POSHOLD now implies ALTHOLD); Initial cut on surface tracking modifier

Browse source 
Direct altitude control
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2018-05-12 19:09:34 +10:00
parent 5227273833
commit 73f773f23e
11 changed files with 231 additions and 263 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -107,7 +107,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
const float demSPE = (posControl.desiredState.pos.z / 100.0f) * GRAVITY_MSS;
const float demSKE = 0.0f;
const float estSPE = (posControl.actualState.pos.z / 100.0f) * GRAVITY_MSS;
const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z / 100.0f) * GRAVITY_MSS;
const float estSKE = 0.0f;
// speedWeight controls balance between potential and kinetic energy used for pitch controller
@ -206,8 +206,8 @@ void resetFixedWingPositionController(void)
static void calculateVirtualPositionTarget_FW(float trackingPeriod)
{
float posErrorX = posControl.desiredState.pos.x - posControl.actualState.pos.x;
float posErrorY = posControl.desiredState.pos.y - posControl.actualState.pos.y;
float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
@ -229,14 +229,14 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
// We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - posControl.actualState.pos.x;
posErrorY = loiterTargetY - posControl.actualState.pos.y;
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y;
distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
}
// Calculate virtual waypoint
virtualDesiredPosition.x = posControl.actualState.pos.x + posErrorX * (trackingDistance / distanceToActualTarget);
virtualDesiredPosition.y = posControl.actualState.pos.y + posErrorY * (trackingDistance / distanceToActualTarget);
virtualDesiredPosition.x = navGetCurrentActualPositionAndVelocity()->pos.x + posErrorX * (trackingDistance / distanceToActualTarget);
virtualDesiredPosition.y = navGetCurrentActualPositionAndVelocity()->pos.y + posErrorY * (trackingDistance / distanceToActualTarget);
// Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
if (posControl.flags.isAdjustingPosition) {
@ -330,7 +330,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
}
// 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.estPosStatue >= EST_USABLE)) {
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;
@ -378,7 +378,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
}
// Apply controller only if position source is valid
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
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;
@ -473,8 +473,8 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
* TODO refactor conditions in this metod if logic is proven to be correct
*/
if (navStateFlags & NAV_CTL_LAND) {
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (posControl.actualState.pos.z <= navConfig()->general.land_slowdown_minalt)) ||
((posControl.flags.estSurfaceStatus == EST_TRUSTED) && (posControl.actualState.surface <= navConfig()->general.land_slowdown_minalt)) ) {
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (navGetCurrentActualPositionAndVelocity()->pos.z <= navConfig()->general.land_slowdown_minalt)) ||
((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) {
/*
* Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
*/
@ -534,7 +534,7 @@ void applyFixedWingEmergencyLandingController(void)
void calculateFixedWingInitialHoldPosition(fpVector3_t * pos)
{
// TODO: stub, this should account for velocity and target loiter radius
*pos = posControl.actualState.pos;
*pos = navGetCurrentActualPositionAndVelocity()->pos;
}
void resetFixedWingHeadingController(void)