Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 06:45:14 +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;
}
/*-----------------------------------------------------------

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

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