fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-10 08:01:37 +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);
}