diff --git a/docs/en/telemetry/crsf_telemetry.md b/docs/en/telemetry/crsf_telemetry.md index d588d835da8..a60a8109f4c 100644 --- a/docs/en/telemetry/crsf_telemetry.md +++ b/docs/en/telemetry/crsf_telemetry.md @@ -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 diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index e62fcff81f9..19ccb61b9d4 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -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(round(sensor_gps.latitude_deg * 1e7)); int32_t longitude = static_cast(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(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(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; diff --git a/src/drivers/rc/crsf_rc/CrsfRc.hpp b/src/drivers/rc/crsf_rc/CrsfRc.hpp index a648048bd07..bfce4900061 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.hpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -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, diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index fb557f84976..c95deac9157 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -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(round(vehicle_gps_position.latitude_deg * 1e7)); int32_t longitude = static_cast(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(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(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; diff --git a/src/drivers/rc_input/crsf_telemetry.h b/src/drivers/rc_input/crsf_telemetry.h index 910a0ec8714..2b6fb368aa6 100644 --- a/src/drivers/rc_input/crsf_telemetry.h +++ b/src/drivers/rc_input/crsf_telemetry.h @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -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; diff --git a/src/lib/rc/crsf.cpp b/src/lib/rc/crsf.cpp index 383a0fbe719..d39064b7e90 100644 --- a/src/lib/rc/crsf.cpp +++ b/src/lib/rc/crsf.cpp @@ -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]; diff --git a/src/lib/rc/crsf.h b/src/lib/rc/crsf.h index 6840c685ee5..7fb34f2dd12 100644 --- a/src/lib/rc/crsf.h +++ b/src/lib/rc/crsf.h @@ -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