Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +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 * 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) uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos)
{ {
const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x; const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y; 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) int32_t calculateBearingToDestination(const fpVector3_t * destinationPos)
{ {
const float deltaX = destinationPos->x - navGetCurrentActualPositionAndVelocity()->pos.x; const navEstimatedPosVel_t *posvel = navGetCurrentActualPositionAndVelocity();
const float deltaY = destinationPos->y - navGetCurrentActualPositionAndVelocity()->pos.y; 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;
} }
/*----------------------------------------------------------- /*-----------------------------------------------------------

10
src/main/navigation/navigation.h
View file

@ -17,6 +17,8 @@
#pragma once #pragma once
#include <stdbool.h>
#include "common/axis.h" #include "common/axis.h"
#include "common/filter.h" #include "common/filter.h"
#include "common/maths.h" #include "common/maths.h"
@ -222,6 +224,11 @@ typedef struct {
int32_t yaw; // deg * 100 int32_t yaw; // deg * 100
} navWaypointPosition_t; } navWaypointPosition_t;
typedef struct navDestinationPath_s {
uint32_t distance; // meters * 100
int32_t bearing; // deg * 100
} navDestinationPath_t;
typedef struct { typedef struct {
float kP; float kP;
float kI; float kI;
@ -384,6 +391,9 @@ void geoConvertGeodeticToLocal(gpsOrigin_s * origin, const gpsLocation_t * llh,
void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * pos, gpsLocation_t * llh); void geoConvertLocalToGeodetic(const gpsOrigin_s * origin, const fpVector3_t * pos, gpsLocation_t * llh);
float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
/* Distance/bearing calculation */
bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);
/* Failsafe-forced RTH mode */ /* Failsafe-forced RTH mode */
void activateForcedRTH(void); void activateForcedRTH(void);
void abortForcedRTH(void); void abortForcedRTH(void);