fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 08:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

GHST: Add support for GPS Telemetry (#19953)

Browse Source 
Add support for the basic GPS telemetry values when using the GHST protocol.

* Fix formatting in GHST GPS telemetry changes

* GHST GPS Telemetry formatting cleanup

* GHST GPS Telemetry, Last formatting change
This commit is contained in:
Tony Cake
2022-08-03 10:44:21 +02:00
committed by GitHub
parent 270c456121
commit f321117568
4 changed files with 119 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/rc_input/ghst_telemetry.cpp
+44
View File
@@ -61,6 +61,14 @@ bool GHSTTelemetry::update(const hrt_abstime &now)
success = send_battery_status();
break;
case 1U:
success = send_gps1_status();
break;
case 2U:
success = send_gps2_status();
break;
default:
success = false;
break;
@@ -93,3 +101,39 @@ bool GHSTTelemetry::send_battery_status()
return success;
}
bool GHSTTelemetry::send_gps1_status()
{
sensor_gps_s vehicle_gps_position;
if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
return false;
}
int32_t latitude = vehicle_gps_position.lat; // 1e-7 degrees
int32_t longitude = vehicle_gps_position.lon; // 1e-7 degrees
uint16_t altitude = vehicle_gps_position.alt / 1000; // mm -> m
return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude);
}
bool GHSTTelemetry::send_gps2_status()
{
sensor_gps_s vehicle_gps_position;
if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
return false;
}
uint16_t ground_speed = (uint16_t)(vehicle_gps_position.vel_d_m_s / 3.6f * 10.f);
uint16_t ground_course = (uint16_t)(math::degrees(vehicle_gps_position.cog_rad) * 100.f);
uint8_t num_sats = vehicle_gps_position.satellites_used;
// TBD: Can these be computed in a RC telemetry driver?
uint16_t home_dist = 0;
uint16_t home_dir = 0;
uint8_t flags = 0;
return ghst_send_telemetry_gps2_status(_uart_fd, ground_speed, ground_course, num_sats, home_dist, home_dir, flags);
}
src/drivers/rc_input/ghst_telemetry.hpp
+5 -1
View File
@@ -44,6 +44,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_gps.h>
#include <drivers/drv_hrt.h>
/**
@@ -69,14 +70,17 @@ public:
private:
bool send_battery_status();
bool send_gps1_status();
bool send_gps2_status();
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
int _uart_fd;
hrt_abstime _last_update {0U};
uint32_t _next_type {0U};
static constexpr uint32_t NUM_DATA_TYPES {1U}; // number of different telemetry data types
static constexpr uint32_t NUM_DATA_TYPES {3U}; // number of different telemetry data types
static constexpr uint32_t UPDATE_RATE_HZ {10U}; // update rate [Hz]
// Factors that should be applied to get correct values