fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 12:23:33 +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/lib/rc/ghst.cpp
+44
View File
@@ -350,6 +350,19 @@ static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value)
offset += 2;
}
/**
* write an uint32_t value to a buffer at a given offset and increment the offset
*/
static inline void write_uint32_t(uint8_t *buf, int &offset, uint32_t value)
{
// Little Endian
buf[offset] = value & 0xFFU;
buf[offset + 1] = (value & 0xFF00) >> 8U;
buf[offset + 2] = (value & 0xFF0000) >> 16U;
buf[offset + 3] = (value & 0xFF000000) >> 24U;
offset += 4;
}
/**
* write frame header
*/
@@ -385,3 +398,34 @@ bool ghst_send_telemetry_battery_status(int uart_fd, uint16_t voltage_in_10mV,
return write(uart_fd, buf, offset) == offset;
}
bool ghst_send_telemetry_gps1_status(int uart_fd, uint32_t latitude, uint32_t longitude, uint16_t altitude)
{
uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4U]; // address, frame length, type, crc
int offset = 0;
write_frame_header(buf, offset, ghstTelemetryType::gpsPrimary, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY);
write_uint32_t(buf, offset, latitude);
write_uint32_t(buf, offset, longitude);
write_uint16_t(buf, offset, altitude);
write_frame_crc(buf, offset, sizeof(buf));
return write(uart_fd, buf, offset) == offset;
}
bool ghst_send_telemetry_gps2_status(int uart_fd, uint16_t ground_speed, uint16_t ground_course, uint8_t numSats,
uint16_t home_dist, uint16_t home_dir, uint8_t flags)
{
uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4U]; // address, frame length, type, crc
int offset = 0;
write_frame_header(buf, offset, ghstTelemetryType::gpsSecondary, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY);
write_uint16_t(buf, offset, ground_speed);
write_uint16_t(buf, offset, ground_course);
write_uint8_t(buf, offset, numSats);
write_uint16_t(buf, offset, home_dist);
write_uint16_t(buf, offset, home_dir);
write_uint8_t(buf, offset, flags);
write_frame_crc(buf, offset, sizeof(buf));
return write(uart_fd, buf, offset) == offset;
}
src/lib/rc/ghst.hpp
+26 -1
View File
@@ -64,7 +64,9 @@ enum class ghstFrameType {
};
enum class ghstTelemetryType {
batteryPack = 0x23 // battery pack status frame type
batteryPack = 0x23, // battery pack status frame type
gpsPrimary = 0x25, // GPS primary data (lat/long/alt)
gpsSecondary = 0x26 // GPS secondary data (course, dist, flags)
};
struct ghst_frame_header_t {
@@ -139,4 +141,27 @@ __EXPORT bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len,
__EXPORT bool ghst_send_telemetry_battery_status(int uart_fd, uint16_t voltage_in_10mV,
uint16_t current_in_10mA, uint16_t fuel_in_10mAh);
/**
* Send primary GPS information
* @param uart_fd UART file descriptor
* @param latitude GPS latitude [1e-7 degrees]
* @param longitude GPS longitude [1e-7 degrees]
* @param altitude GPS altitude [1m]
* @return true on success
*/
__EXPORT bool ghst_send_telemetry_gps1_status(int uart_fd, uint32_t latitude, uint32_t longitude, uint16_t altitude);
/**
* Send secondary GPS information
* @param uart_fd UART file descriptor
* @param ground_speed Ground Speed [1 km/h]
* @param ground_course Ground Course [1e-7 degrees]
* @param num_sats GPS Satellite count
* @param home_dist Distance to Home [10 m]
* @param home_dir Direction to Home [1e-7 degrees]
* @param flags GPS Flags
* @return true on success
*/
__EXPORT bool ghst_send_telemetry_gps2_status(int uart_fd, uint16_t ground_speed, uint16_t ground_course,
uint8_t num_sats, uint16_t home_dist, uint16_t home_dir, uint8_t flags);
__END_DECLS