UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-24 15:27:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(crsf): add local altitude/vertical-speed telemetry and fix GPS frame (#27948)

Browse Source 
* fix(crsf): correct GPS telemetry ground speed, course, and altitude offset

Ground speed was sent from the NED down-velocity component with the km/h conversion inverted, course over ground wraps negative values through the uint16 field, and the rc_input frame applied a +1 m offset where the protocol expects +1000 m.

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

* feat(crsf): send local altitude and vertical speed telemetry

Add the CRSF barometric altitude frame (0x09), fed from vehicle_local_position, to both the crsf_rc driver and the legacy rc_input telemetry. Shows up as Alt and VSpd sensors on EdgeTX/OpenTX radios, complementing the MSL altitude already sent in the GPS frame.

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

* docs(crsf): clarify local-altitude telemetry semantics and units

Note that the Alt/VSpd sensors reuse the CRSF baro-altitude frame to carry PX4's fused local altitude (above the estimator origin), not a raw barometer reading, and document the EdgeTX version requirements. Add terse unit comments on the GPS-frame fields.

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

---------

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>
This commit is contained in:
Jacob Dahl
2026-07-19 10:16:23 -06:00
committed by GitHub
parent d11b996d89
commit 5ea7337a31
7 changed files with 148 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
docs/en/telemetry/crsf_telemetry.md
View File

@@ -209,7 +209,12 @@ The supported telemetry messages and their source are listed below (this table i
| Roll | FC Roll angle | FC |
| Yaw | FC Yaw angle | FC |
| FM | Flight mode | FC |
| VSPD | Barometer | FC |
| Alt | Altitude above the local origin (takeoff point) | FC |
| VSpd | Vertical speed | FC |
::: info
`GAlt` (in the GPS sensor) is GPS altitude above mean sea level, while `Alt`/`VSpd` are PX4's fused local altitude and climb rate relative to the estimator origin (roughly the takeoff point). PX4 sends these in the CRSF barometric-altitude frame, so despite the frame name the value is not a raw barometer reading. `VSpd` uses the linear vertical-speed form, which requires a recent EdgeTX (2.9+); the altitude displays on EdgeTX 2.7.1 and later.
:::
## See Also

60
src/drivers/rc/crsf_rc/CrsfRc.cpp
View File

@@ -114,6 +114,31 @@ int CrsfRc::task_spawn(int argc, char *argv[])
return PX4_OK;
}
/**
* pack an altitude for the barometric altitude frame: decimeters + 10000 offset,
* or meters with the MSB set when above the decimeter range
*/
static uint16_t pack_baro_altitude(const float altitude_m)
{
int32_t altitude_dm = lroundf(altitude_m * 10.f) + 10000;
if (altitude_dm < 0) {
return 0;
}
if (altitude_dm < 0x8000) {
return (uint16_t)altitude_dm;
}
int32_t altitude_full_m = lroundf(altitude_m);
if (altitude_full_m > 0x7FFF) {
altitude_full_m = 0x7FFF;
}
return (uint16_t)altitude_full_m | 0x8000;
}
void CrsfRc::Run()
{
if (should_exit()) {
@@ -303,9 +328,9 @@ void CrsfRc::Run()
if (_vehicle_gps_position_sub.update(&sensor_gps)) {
int32_t latitude = static_cast<int32_t>(round(sensor_gps.latitude_deg * 1e7));
int32_t longitude = static_cast<int32_t>(round(sensor_gps.longitude_deg * 1e7));
uint16_t groundspeed = sensor_gps.vel_d_m_s / 3.6f * 10.f;
uint16_t gps_heading = math::degrees(sensor_gps.cog_rad) * 100.f;
uint16_t altitude = static_cast<int16_t>(sensor_gps.altitude_msl_m) + 1000;
uint16_t groundspeed = sensor_gps.vel_m_s * 3.6f * 10.f; // 0.1 km/h
uint16_t gps_heading = math::degrees(matrix::wrap_2pi(sensor_gps.cog_rad)) * 100.f;
uint16_t altitude = static_cast<int16_t>(sensor_gps.altitude_msl_m) + 1000; // meters + 1000 offset
uint8_t num_satellites = sensor_gps.satellites_used;
this->SendTelemetryGps(latitude, longitude, groundspeed, gps_heading, altitude, num_satellites);
}
@@ -392,6 +417,24 @@ void CrsfRc::Run()
this->SendTelemetryFlightMode(flight_mode);
}
break;
case 4:
// Reuse the CRSF baro-altitude frame to carry the fused local altitude
// (above the EKF origin), not a raw barometer reading. EdgeTX shows it as "Alt".
vehicle_local_position_s local_position;
if (_vehicle_local_position_sub.update(&local_position) && local_position.z_valid) {
uint16_t altitude = pack_baro_altitude(-local_position.z);
int16_t vertical_speed = 0;
if (local_position.v_z_valid) {
vertical_speed = lroundf(math::constrain(-local_position.vz * 100.f, (float)INT16_MIN, (float)INT16_MAX));
}
this->SendTelemetryBaroAltitude(altitude, vertical_speed);
}
break;
}
@@ -531,6 +574,17 @@ bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, cons
return _uart->write((void *) buf, (size_t) offset);
}
bool CrsfRc::SendTelemetryBaroAltitude(const uint16_t altitude, const int16_t vertical_speed)
{
uint8_t buf[(uint8_t)crsf_payload_size_t::baro_altitude + 4];
int offset = 0;
WriteFrameHeader(buf, offset, crsf_frame_type_t::baro_altitude, (uint8_t)crsf_payload_size_t::baro_altitude);
write_uint16_t(buf, offset, altitude);
write_uint16_t(buf, offset, (uint16_t)vertical_speed);
WriteFrameCrc(buf, offset, sizeof(buf));
return _uart->write((void *) buf, (size_t) offset);
}
bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode)
{
const int max_length = 16;

8
src/drivers/rc/crsf_rc/CrsfRc.hpp
View File

@@ -52,6 +52,7 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
@@ -92,6 +93,8 @@ private:
bool SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw);
bool SendTelemetryBaroAltitude(const uint16_t altitude, const int16_t vertical_speed);
bool SendTelemetryFlightMode(const char *flight_mode);
bool BindCRSF();
@@ -113,17 +116,19 @@ private:
// telemetry
hrt_abstime _telemetry_update_last{0};
static constexpr int num_data_types{4}; ///< number of different telemetry data types
static constexpr int num_data_types{5}; ///< number of different telemetry data types
int _next_type{0};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)};
enum class crsf_frame_type_t : uint8_t {
gps = 0x02,
battery_sensor = 0x08,
baro_altitude = 0x09,
link_statistics = 0x14,
rc_channels_packed = 0x16,
attitude = 0x1E,
@@ -141,6 +146,7 @@ private:
enum class crsf_payload_size_t : uint8_t {
gps = 15,
battery_sensor = 8,
baro_altitude = 4, ///< altitude (uint16) + vertical speed (int16)
link_statistics = 10,
rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
attitude = 6,

55
src/drivers/rc_input/crsf_telemetry.cpp
View File

@@ -65,6 +65,10 @@ bool CRSFTelemetry::update(const hrt_abstime &now)
case 3:
sent = send_flight_mode();
break;
case 4:
sent = send_baro_altitude();
break;
}
_last_update = now;
@@ -98,15 +102,60 @@ bool CRSFTelemetry::send_gps()
int32_t latitude = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7));
int32_t longitude = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7));
uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f;
uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f;
uint16_t altitude = static_cast<int16_t>(round(vehicle_gps_position.altitude_msl_m + 1.0));
uint16_t groundspeed = vehicle_gps_position.vel_m_s * 3.6f * 10.f; // 0.1 km/h
uint16_t gps_heading = math::degrees(matrix::wrap_2pi(vehicle_gps_position.cog_rad)) * 100.f;
uint16_t altitude = static_cast<uint16_t>(round(vehicle_gps_position.altitude_msl_m) + 1000); // meters + 1000 offset
uint8_t num_satellites = vehicle_gps_position.satellites_used;
return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed,
gps_heading, altitude, num_satellites);
}
/**
* pack an altitude for the barometric altitude frame: decimeters + 10000 offset,
* or meters with the MSB set when above the decimeter range
*/
static uint16_t pack_baro_altitude(const float altitude_m)
{
int32_t altitude_dm = lroundf(altitude_m * 10.f) + 10000;
if (altitude_dm < 0) {
return 0;
}
if (altitude_dm < 0x8000) {
return (uint16_t)altitude_dm;
}
int32_t altitude_full_m = lroundf(altitude_m);
if (altitude_full_m > 0x7FFF) {
altitude_full_m = 0x7FFF;
}
return (uint16_t)altitude_full_m | 0x8000;
}
bool CRSFTelemetry::send_baro_altitude()
{
// Reuse the CRSF baro-altitude frame to carry the fused local altitude
// (above the EKF origin), not a raw barometer reading. EdgeTX shows it as "Alt".
vehicle_local_position_s local_position;
if (!_vehicle_local_position_sub.update(&local_position) || !local_position.z_valid) {
return false;
}
uint16_t altitude = pack_baro_altitude(-local_position.z);
int16_t vertical_speed = 0;
if (local_position.v_z_valid) {
vertical_speed = lroundf(math::constrain(-local_position.vz * 100.f, (float)INT16_MIN, (float)INT16_MAX));
}
return crsf_send_telemetry_baro_altitude(_uart_fd, altitude, vertical_speed);
}
bool CRSFTelemetry::send_attitude()
{
vehicle_attitude_s vehicle_attitude;

5
src/drivers/rc_input/crsf_telemetry.h
View File

@@ -43,6 +43,7 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h>
@@ -77,15 +78,17 @@ private:
bool send_gps();
bool send_attitude();
bool send_flight_mode();
bool send_baro_altitude();
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
hrt_abstime _last_update{0};
static constexpr int num_data_types{4}; ///< number of different telemetry data types
static constexpr int num_data_types{5}; ///< number of different telemetry data types
int _next_type{0};
int _uart_fd;

13
src/lib/rc/crsf.cpp
View File

@@ -62,6 +62,7 @@
enum class crsf_frame_type_t : uint8_t {
gps = 0x02,
battery_sensor = 0x08,
baro_altitude = 0x09,
link_statistics = 0x14,
rc_channels_packed = 0x16,
attitude = 0x1E,
@@ -79,6 +80,7 @@ enum class crsf_frame_type_t : uint8_t {
enum class crsf_payload_size_t : uint8_t {
gps = 15,
battery_sensor = 8,
baro_altitude = 4, ///< altitude (uint16) + vertical speed (int16)
link_statistics = 10,
rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
attitude = 6,
@@ -446,6 +448,17 @@ bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, u
return write(uart_fd, buf, offset) == offset;
}
bool crsf_send_telemetry_baro_altitude(int uart_fd, uint16_t altitude, int16_t vertical_speed)
{
uint8_t buf[(uint8_t)crsf_payload_size_t::baro_altitude + 4];
int offset = 0;
write_frame_header(buf, offset, crsf_frame_type_t::baro_altitude, (uint8_t)crsf_payload_size_t::baro_altitude);
write_uint16_t(buf, offset, altitude);
write_uint16_t(buf, offset, (uint16_t)vertical_speed);
write_frame_crc(buf, offset, sizeof(buf));
return write(uart_fd, buf, offset) == offset;
}
bool crsf_send_telemetry_attitude(int uart_fd, int16_t pitch, int16_t roll, int16_t yaw)
{
uint8_t buf[(uint8_t)crsf_payload_size_t::attitude + 4];

9
src/lib/rc/crsf.h
View File

@@ -112,6 +112,15 @@ __EXPORT bool crsf_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_
__EXPORT bool crsf_send_telemetry_gps(int uart_fd, int32_t latitude, int32_t longitude, uint16_t groundspeed,
uint16_t gps_heading, uint16_t altitude, uint8_t num_satellites);
/**
* Send telemetry barometric altitude information
* @param uart_fd UART file descriptor
* @param altitude Altitude [decimeters + 10000 offset], or [meters] with the MSB set
* @param vertical_speed Vertical speed [cm/s]
* @return true on success
*/
__EXPORT bool crsf_send_telemetry_baro_altitude(int uart_fd, uint16_t altitude, int16_t vertical_speed);
/**
* Send telemetry Attitude information