From 0caa4dc1712771faec3ca57756998dedfd29bea7 Mon Sep 17 00:00:00 2001 From: acfloria Date: Wed, 11 Apr 2018 13:07:22 +0200 Subject: [PATCH] Mavlink: Don't use a packed struct as input for the convert_limit_safe function - Addional small cleanup of the iridium driver and the include guards of mavlink module header files --- src/drivers/telemetry/iridiumsbd/IridiumSBD.h | 7 ++--- src/modules/mavlink/mavlink_high_latency2.cpp | 28 ++++++++++++++----- src/modules/mavlink/mavlink_high_latency2.h | 7 ++--- src/modules/mavlink/mavlink_simple_analyzer.h | 13 ++++----- 4 files changed, 31 insertions(+), 24 deletions(-) diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index 6d9ee82f42..613704a074 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -88,7 +88,6 @@ extern "C" __EXPORT int iridiumsbd_main(int argc, char *argv[]); #define SATCOM_RX_MSG_BUF_LEN 270 // RX buffer size for MT messages #define SATCOM_RX_COMMAND_BUF_LEN 50 // RX buffer size for other commands #define SATCOM_SIGNAL_REFRESH_DELAY 20000000 // update signal quality every 20s -#define MAVLINK_PACKAGE_START 254 // The value of the first byte of the mavlink header /** * The driver for the Rockblock 9602 and 9603 RockBlock module for satellite communication over the Iridium satellite system. @@ -308,14 +307,14 @@ private: char _test_command[32]; hrt_abstime _test_timer = 0; - uint8_t _rx_command_buf[SATCOM_RX_COMMAND_BUF_LEN] = {0}; + uint8_t _rx_command_buf[SATCOM_RX_COMMAND_BUF_LEN] = {}; int _rx_command_len = 0; - uint8_t _rx_msg_buf[SATCOM_RX_MSG_BUF_LEN] = {0}; + uint8_t _rx_msg_buf[SATCOM_RX_MSG_BUF_LEN] = {}; int _rx_msg_end_idx = 0; int _rx_msg_read_idx = 0; - uint8_t _tx_buf[SATCOM_TX_BUF_LEN] = {0}; + uint8_t _tx_buf[SATCOM_TX_BUF_LEN] = {}; int _tx_buf_write_idx = 0; bool _tx_buf_write_pending = false; diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp index 3d9824617e..d9cac1fc54 100644 --- a/src/modules/mavlink/mavlink_high_latency2.cpp +++ b/src/modules/mavlink/mavlink_high_latency2.cpp @@ -134,7 +134,10 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) updated |= write_wind_estimate(&msg); if (updated) { - convert_limit_safe(t / 1000, &msg.timestamp); + uint32_t timestamp; + convert_limit_safe(t / 1000, timestamp); + msg.timestamp = timestamp; + msg.type = _mavlink->get_system_type(); msg.autopilot = MAV_AUTOPILOT_PX4; @@ -280,7 +283,9 @@ bool MavlinkStreamHighLatency2::write_fw_ctrl_status(mavlink_high_latency2_t *ms const bool updated = _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_time, &fw_pos_ctrl_status); if (_fw_pos_ctrl_status_time > 0) { - convert_limit_safe(fw_pos_ctrl_status.wp_dist * 0.1f, &msg->target_distance); + uint16_t target_distance; + convert_limit_safe(fw_pos_ctrl_status.wp_dist * 0.1f, target_distance); + msg->target_distance = target_distance; } return updated; @@ -308,16 +313,23 @@ bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *m const bool updated = _global_pos_sub->update(&_global_pos_time, &global_pos); if (_global_pos_time > 0) { - convert_limit_safe(global_pos.lat * 1e7, &msg->latitude); - convert_limit_safe(global_pos.lon * 1e7, &msg->longitude); + int32_t latitude, longitude; + convert_limit_safe(global_pos.lat * 1e7, latitude); + convert_limit_safe(global_pos.lon * 1e7, longitude); + msg->latitude = latitude; + msg->longitude = longitude; + + int16_t altitude; if (global_pos.alt > 0) { - convert_limit_safe(global_pos.alt + 0.5f, &msg->altitude); + convert_limit_safe(global_pos.alt + 0.5f, altitude); } else { - convert_limit_safe(global_pos.alt - 0.5f, &msg->altitude); + convert_limit_safe(global_pos.alt - 0.5f, altitude); } + msg->altitude = altitude; + msg->heading = static_cast(math::degrees(_wrap_2pi(global_pos.yaw)) * 0.5f); } @@ -344,7 +356,9 @@ bool MavlinkStreamHighLatency2::write_tecs_status(mavlink_high_latency2_t *msg) const bool updated = _tecs_status_sub->update(&_tecs_time, &tecs_status); if (_tecs_time > 0) { - convert_limit_safe(tecs_status.altitudeSp, &msg->target_altitude); + int16_t target_altitude; + convert_limit_safe(tecs_status.altitudeSp, target_altitude); + msg->target_altitude = target_altitude; } return updated; diff --git a/src/modules/mavlink/mavlink_high_latency2.h b/src/modules/mavlink/mavlink_high_latency2.h index 616064e07b..a5ec6dbc73 100644 --- a/src/modules/mavlink/mavlink_high_latency2.h +++ b/src/modules/mavlink/mavlink_high_latency2.h @@ -37,14 +37,13 @@ * @author Achermann Florian */ +#pragma once + #include "mavlink_main.h" #include "mavlink_messages.h" #include "mavlink_simple_analyzer.h" #include "mavlink_stream.h" -#ifndef MAVLINK_HIGH_LATENCY2_H_ -#define MAVLINK_HIGH_LATENCY2_H_ - class MavlinkStreamHighLatency2 : public MavlinkStream { public: @@ -197,5 +196,3 @@ protected: void set_default_values(mavlink_high_latency2_t &msg) const; }; - -#endif /* MAVLINK_HIGH_LATENCY2_H_ */ diff --git a/src/modules/mavlink/mavlink_simple_analyzer.h b/src/modules/mavlink/mavlink_simple_analyzer.h index 6600ab38bc..57d43436d1 100644 --- a/src/modules/mavlink/mavlink_simple_analyzer.h +++ b/src/modules/mavlink/mavlink_simple_analyzer.h @@ -37,8 +37,7 @@ * @author Achermann Florian */ -#ifndef MAVLINK_SIMPLE_ANALYZER_H_ -#define MAVLINK_SIMPLE_ANALYZER_H_ +#pragma once #include @@ -137,17 +136,15 @@ private: }; template -void convert_limit_safe(Tin in, Tout *out) +void convert_limit_safe(Tin in, Tout &out) { if (in > std::numeric_limits::max()) { - *out = std::numeric_limits::max(); + out = std::numeric_limits::max(); } else if (in < std::numeric_limits::min()) { - *out = std::numeric_limits::min(); + out = std::numeric_limits::min(); } else { - *out = static_cast(in); + out = static_cast(in); } } - -#endif /* MAVLINK_SIMPLE_ANALYZER_H_ */