rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

LP-291 msp_send_comp_gps(): calculate distance and direction to home using PositionState and TakeOffLocation, same way it is done in GCS.

Browse Source
This commit is contained in:
Vladimir Zidar 2016-04-24 01:49:09 +02:00
parent fec3924ee0
commit 69a51956e8
1 changed files with 29 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
flight/modules/UAVOMSPBridge/UAVOMSPBridge.c
View File

@ -50,6 +50,7 @@
#include "flightstatus.h"
#include "systemstats.h"
#include "systemalarms.h"
#include "takeofflocation.h"
#include "homelocation.h"
#include "positionstate.h"
#include "velocitystate.h"
@ -65,9 +66,6 @@
#include "pios_sensors.h"
#define WGS84_RADIUS_EARTH_KM 6371.008f // Earth's radius in km
#define PI 3.14159265358979323846f // [-]
#define PIOS_INCLUDE_MSP_BRIDGE
#if defined(PIOS_INCLUDE_MSP_BRIDGE)
@ -520,6 +518,12 @@ static void msp_send_raw_gps(struct msp_bridge *m)
}
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
static inline float Sq(float x)
{
return x * x;
}
static void msp_send_comp_gps(struct msp_bridge *m)
{
union {
@ -531,44 +535,33 @@ static void msp_send_comp_gps(struct msp_bridge *m)
} __attribute__((packed)) comp_gps;
} data;
GPSPositionSensorData gps_data;
HomeLocationData home_data;
if ((GPSPositionSensorHandle() == NULL) || (HomeLocationHandle() == NULL)) {
data.comp_gps.distance_to_home = 0;
data.comp_gps.direction_to_home = 0;
data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD
} else {
GPSPositionSensorGet(&gps_data);
HomeLocationGet(&home_data);
if ((gps_data.Status < GPSPOSITIONSENSOR_STATUS_FIX2D) || (home_data.Set == HOMELOCATION_SET_FALSE)) {
data.comp_gps.distance_to_home = 0;
data.comp_gps.direction_to_home = 0;
data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD
} else {
data.comp_gps.home_position_valid = 1; // Home distance and direction will display on OSD
int32_t delta_lon = (home_data.Longitude - gps_data.Longitude); // degrees * 1e7
int32_t delta_lat = (home_data.Latitude - gps_data.Latitude); // degrees * 1e7
float delta_y = DEG2RAD((float)delta_lon * WGS84_RADIUS_EARTH_KM); // KM * 1e7
float delta_x = DEG2RAD((float)delta_lat * WGS84_RADIUS_EARTH_KM); // KM * 1e7
delta_y *= cosf(DEG2RAD((float)home_data.Latitude * 1e-7f)); // Latitude compression correction
data.comp_gps.distance_to_home = (uint16_t)(sqrtf(delta_x * delta_x + delta_y * delta_y) * 1e-4f); // meters
if ((delta_lon == 0) && (delta_lat == 0)) {
data.comp_gps.distance_to_home = 0;
data.comp_gps.direction_to_home = 0;
} else {
data.comp_gps.direction_to_home = RAD2DEG((int16_t)(atan2f(delta_y, delta_x))); // degrees;
}
data.comp_gps.home_position_valid = 0; // Home distance and direction will not display on OSD
if( PositionStateHandle() && TakeOffLocationHandle() )
{
TakeOffLocationData takeOffLocation;
TakeOffLocationGet(&takeOffLocation);
if(takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID)
{
PositionStateData positionState;
PositionStateGet(&positionState);
data.comp_gps.distance_to_home = (uint16_t)sqrtf( Sq(takeOffLocation.East - positionState.East) + Sq(takeOffLocation.North - positionState.North) );
data.comp_gps.direction_to_home = RAD2DEG( atan2f(takeOffLocation.East - positionState.East, takeOffLocation.North - positionState.North) );
data.comp_gps.home_position_valid = 1;
}
}
// here, CC3D implementation could use raw gps data (GPSPositionSensorData) and locally cached GPSPositionSensorData at arming time as TakeOffLocation.
msp_send(m, MSP_COMP_GPS, data.buf, sizeof(data));
}
}
static void msp_send_altitude(struct msp_bridge *m)
{