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

Merge pull request #4116 from iNavFlight/agh_calculate_path_to_destination

Browse source 
Add navCalculatePathToDestination() function
This commit is contained in:
Alberto García Hierro 2019-01-06 22:08:38 +00:00 • committed by GitHub
commit 3cf574f043
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
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

@ -1900,20 +1900,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;
}
/*-----------------------------------------------------------