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

Velocity attenuation for WP mode if not facing the next waypoint. Turn first, accelerate later logic. Don't apply rth_tail_first for airplanes - can't go tail first

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-12 16:40:39 +10:00
parent 540eaa7290
commit a63b0a01dc
3 changed files with 40 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

15
src/main/flight/navigation_rewrite.c
View file

@ -602,6 +602,11 @@ static uint32_t navGetCurrentStateTime(void)
return millis() - posControl.navStateActivationTimeMs;
}
navigationFSMStateFlags_t navGetCurrentStateFlags(void)
{
return navGetStateFlags(posControl.navState);
}
/*************************************************************************************************/
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState)
{
@ -742,7 +747,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_2D_HEAD_HOME(naviga
}
else {
// Update XY-position target
if (posControl.navConfig->flags.rth_tail_first) {
if (posControl.navConfig->flags.rth_tail_first && !STATE(FIXED_WING)) {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
}
else {
@ -825,8 +830,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL
if ((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -50.0f) {
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_3D_HEAD_HOME
}
if (posControl.navConfig->flags.rth_tail_first) {
else {
// Climb to safe altitude and turn to correct direction
if (posControl.navConfig->flags.rth_tail_first && !STATE(FIXED_WING)) {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
}
else {
@ -835,6 +841,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_CLIMB_TO_SAFE_AL
return NAV_FSM_EVENT_NONE;
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(navigationFSMState_t previousState)
{
@ -851,7 +858,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_HEAD_HOME(naviga
}
else {
// Update XYZ-position target
if (posControl.navConfig->flags.rth_tail_first) {
if (posControl.navConfig->flags.rth_tail_first && !STATE(FIXED_WING)) {
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
}
else {

23
src/main/flight/navigation_rewrite_multicopter.c
View file

@ -246,22 +246,39 @@ bool adjustMulticopterPositionFromRCInput(void)
}
}
static float getVelocityAttenuationFactor(void)
{
// In WP mode scale velocity if heading is different from bearing
if (navGetCurrentStateFlags() & NAV_AUTO_WP) {
int32_t headingError = constrain(wrap_18000(posControl.desiredState.yaw - posControl.actualState.yaw), -9000, 9000);
float velScaling = cos_approx(CENTIDEGREES_TO_RADIANS(headingError));
return constrainf(velScaling * velScaling, 0.05f, 1.0f);
} else {
return 1.0f;
}
}
static void updatePositionVelocityController_MC(void)
{
float posErrorX = posControl.desiredState.pos.V.X - posControl.actualState.pos.V.X;
float posErrorY = posControl.desiredState.pos.V.Y - posControl.actualState.pos.V.Y;
// Calculate target velocity
float newVelX = posErrorX * posControl.pids.pos[X].param.kP;
float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY));
// Scale velocity to respect max_speed
float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY));
if (newVelTotal > posControl.navConfig->max_speed) {
newVelX = posControl.navConfig->max_speed * (newVelX / newVelTotal);
newVelY = posControl.navConfig->max_speed * (newVelY / newVelTotal);
}
posControl.desiredState.vel.V.X = newVelX;
posControl.desiredState.vel.V.Y = newVelY;
// Apply attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
float velAttFactor = getVelocityAttenuationFactor();
posControl.desiredState.vel.V.X = newVelX * velAttFactor;
posControl.desiredState.vel.V.Y = newVelY * velAttFactor;
#if defined(NAV_BLACKBOX)
navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.V.X), -32678, 32767);

4
src/main/flight/navigation_rewrite_private.h
View file

@ -272,10 +272,14 @@ uint32_t calculateDistanceToDestination(t_fp_vector * destinationPos);
int32_t calculateBearingToDestination(t_fp_vector * destinationPos);
void resetLandingDetector(void);
bool isLandingDetected(void);
navigationFSMStateFlags_t navGetCurrentStateFlags(void);
void setHomePosition(t_fp_vector * pos, int32_t yaw);
void setDesiredPosition(t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t useMask);
void setDesiredSurfaceOffset(float surfaceOffset);
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);
bool isWaypointReached(navWaypointPosition_t * waypoint);
bool isWaypointMissed(navWaypointPosition_t * waypoint);
bool isApproachingLastWaypoint(void);