Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: use location from AHRS not GPS 

The AHRS may be able to supply information even in the absense of an
actual GPS unit.
This commit is contained in:
Peter Barker 2021-02-25 10:59:55 +11:00 committed by Peter Barker
parent 2671a2f71f
commit c059338386
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp
View File

@ -2,7 +2,6 @@
#include <AP_Baro/AP_Baro.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_RPM/AP_RPM.h>
extern const AP_HAL::HAL& hal;
@ -106,8 +105,11 @@ float AP_Frsky_Backend::format_gps(float dec)
*/
void AP_Frsky_Backend::calc_gps_position(void)
{
if (AP::gps().status() >= 3) {
const Location &loc = AP::gps().location(); //get gps instance 0
AP_AHRS &_ahrs = AP::ahrs();
Location loc;
if (_ahrs.get_position(loc)) {
float lat = format_gps(fabsf(loc.lat/10000000.0f));
_SPort_data.latdddmm = lat;
_SPort_data.latmmmm = (lat - _SPort_data.latdddmm) * 10000;
@ -122,7 +124,7 @@ void AP_Frsky_Backend::calc_gps_position(void)
_SPort_data.alt_gps_meters = (int16_t)alt;
_SPort_data.alt_gps_cm = (alt - _SPort_data.alt_gps_meters) * 100;
float speed = AP::gps().ground_speed();
const float speed = AP::ahrs().groundspeed();
_SPort_data.speed_in_meter = speed;
_SPort_data.speed_in_centimeter = (speed - _SPort_data.speed_in_meter) * 100;
} else {
@ -137,7 +139,6 @@ void AP_Frsky_Backend::calc_gps_position(void)
_SPort_data.speed_in_centimeter = 0;
}
AP_AHRS &_ahrs = AP::ahrs();
_SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS
}