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

WP: Per-waypoint speed setting

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-21 15:03:08 +10:00
parent dad6c57463
commit d26c8b0e01
3 changed files with 27 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1831,6 +1831,22 @@ bool isApproachingLastWaypoint(void)
}
}
float getActiveWaypointSpeed(void)
{
uint16_t waypointSpeed = posControl.navConfig->max_speed;
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) {
waypointSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
if (waypointSpeed < 50 || waypointSpeed > posControl.navConfig->max_speed) {
waypointSpeed = posControl.navConfig->max_speed;
}
}
}
return waypointSpeed;
}
/*-----------------------------------------------------------
* A function to reset navigation PIDs and states

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

@ -259,10 +259,10 @@ static float getVelocityHeadingAttenuationFactor(void)
}
}
static float getVelocityExpoAttenuationFactor(float velTotal)
static float getVelocityExpoAttenuationFactor(float velTotal, float velMax)
{
// Calculate factor of how velocity with applied expo is different from unchanged velocity
float velScale = constrainf(velTotal / posControl.navConfig->max_speed, 0.01f, 1.0f);
float velScale = constrainf(velTotal / velMax, 0.01f, 1.0f);
// posControl.navConfig->max_speed * ((velScale * velScale * velScale) * posControl.posResponseExpo + velScale * (1 - posControl.posResponseExpo)) / velTotal;
// ((velScale * velScale * velScale) * posControl.posResponseExpo + velScale * (1 - posControl.posResponseExpo)) / velScale
@ -279,17 +279,20 @@ static void updatePositionVelocityController_MC(void)
float newVelX = posErrorX * posControl.pids.pos[X].param.kP;
float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
// Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s
float maxSpeed = getActiveWaypointSpeed();
// 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);
newVelTotal = posControl.navConfig->max_speed;
if (newVelTotal > maxSpeed) {
newVelX = maxSpeed * (newVelX / newVelTotal);
newVelY = maxSpeed * (newVelY / newVelTotal);
newVelTotal = maxSpeed;
}
// Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
float velHeadFactor = getVelocityHeadingAttenuationFactor();
float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal);
float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed);
posControl.desiredState.vel.V.X = newVelX * velHeadFactor * velExpoFactor;
posControl.desiredState.vel.V.Y = newVelY * velHeadFactor * velExpoFactor;

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

@ -286,6 +286,7 @@ void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWayp
bool isWaypointReached(navWaypointPosition_t * waypoint);
bool isWaypointMissed(navWaypointPosition_t * waypoint);
bool isApproachingLastWaypoint(void);
float getActiveWaypointSpeed(void);
int16_t rcCommandToLeanAngle(int16_t rcCommand);
int16_t leanAngleToRcCommand(int16_t leanAngle);