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

Add navCalculatePathToDestination() function

Browse source 
Calculates distance and bearing to the given location in local
coordinate space.

Added navDestinationPath_t, which encapsulates distance and
bearing.

Refactor calculateDistanceToDestination() and
calculateBearingToDestination() a bit to move the calculations to
static functions that can be reused by navCalculatePathTo()
This commit is contained in:
Alberto García Hierro 2018-12-26 11:19:36 +00:00
parent 38c0caa12b
commit 4df6e9aca9
2 changed files with 45 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

41
src/main/navigation/navigation.c
View file

@ -1899,20 +1899,49 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void)
/*-----------------------------------------------------------
* Calculates distance and bearing to destination point
*-----------------------------------------------------------*/
static uint32_t calculateDistanceFromDelta(float deltaX, float deltaY)
{
return sqrtf(sq(deltaX) + sq(deltaY));
}
static int32_t calculateBearingFromDelta(float deltaX, float deltaY)
{
return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
}
uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos)
{
const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x;
const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y;
const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
const float deltaX = destinationPos->x - posvel->pos.x;
const float deltaY = destinationPos->y - posvel->pos.y;
return sqrtf(sq(deltaX) + sq(deltaY));
return calculateDistanceFromDelta(deltaX, deltaY);
}
int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
{
const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x;
const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y;
const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
const float deltaX = destinationPos->x - posvel->pos.x;
const float deltaY = destinationPos->y - posvel->pos.y;
return wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(deltaY, deltaX)));
return calculateBearingFromDelta(deltaX, deltaY);
}
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos)
{
if (posControl.flags.estPosStatus == EST_NONE ||
posControl.flags.estHeadingStatus == EST_NONE) {
return false;
}
const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
const float deltaX = destinationPos->x - posvel->pos.x;
const float deltaY = destinationPos->y - posvel->pos.y;
result->distance = calculateDistanceFromDelta(deltaX, deltaY);
result->bearing = calculateBearingFromDelta(deltaX, deltaY);
return true;
}
/*-----------------------------------------------------------