From ff49a1b3b569c411add5ad3aa2c8014373a8a9ba Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Thu, 25 Apr 2019 12:52:51 -0700 Subject: [PATCH 01/25] Implement MavMessage working. String only. --- .../telemetry/telemetry_service_impl.h | 20 ++++- integration_tests/telemetry_async.cpp | 12 +++ .../include/plugins/telemetry/telemetry.h | 16 ++++ plugins/telemetry/mocks/telemetry_mock.h | 1 + plugins/telemetry/telemetry.cpp | 30 +++++++ plugins/telemetry/telemetry_impl.cpp | 85 +++++++++++++++++++ plugins/telemetry/telemetry_impl.h | 11 +++ 7 files changed, 174 insertions(+), 1 deletion(-) diff --git a/backend/src/plugins/telemetry/telemetry_service_impl.h b/backend/src/plugins/telemetry/telemetry_service_impl.h index 87861619aa..02c07ee32b 100644 --- a/backend/src/plugins/telemetry/telemetry_service_impl.h +++ b/backend/src/plugins/telemetry/telemetry_service_impl.h @@ -96,6 +96,24 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet return grpc::Status::OK; } + grpc::Status + SubscribeMavMessage(grpc::ServerContext * /* context */, + const dronecode_sdk::rpc::telemetry::SubscribeMavMessageRequest * /* request */, + grpc::ServerWriter *writer) override + { + _telemetry.mav_message_async([&writer](dronecode_sdk::Telemetry::MavMessage mav_message) { + auto rpc_mav_message = new dronecode_sdk::rpc::telemetry::MavMessage(); + rpc_mav_message->set_message_str(mav_message.message_str); + + dronecode_sdk::rpc::telemetry::MavMessageResponse rpc_mav_message_response; + rpc_mav_message_response.set_allocated_mav_message(rpc_mav_message); + writer->Write(rpc_mav_message_response); + }); + + _stop_future.wait(); + return grpc::Status::OK; + } + grpc::Status SubscribeArmed(grpc::ServerContext * /* context */, const dronecode_sdk::rpc::telemetry::SubscribeArmedRequest * /* request */, @@ -339,7 +357,7 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet }); _stop_future.wait(); - return grpc::Status::OK; + return grpc::Status::OK; } void stop() { _stop_promise.set_value(); } diff --git a/integration_tests/telemetry_async.cpp b/integration_tests/telemetry_async.cpp index 93b7021519..aebbb776b3 100644 --- a/integration_tests/telemetry_async.cpp +++ b/integration_tests/telemetry_async.cpp @@ -23,6 +23,7 @@ static void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned); static void print_gps_info(Telemetry::GPSInfo gps_info); static void print_battery(Telemetry::Battery battery); static void print_rc_status(Telemetry::RCStatus rc_status); +static void print_mav_message(Telemetry::MavMessage mav_message); static void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned); static bool _set_rate_error = false; @@ -40,6 +41,7 @@ static bool _received_ground_speed = false; static bool _received_gps_info = false; static bool _received_battery = false; static bool _received_rc_status = false; +static bool _received_mav_message = false; static bool _received_position_velocity_ned = false; TEST_F(SitlTest, TelemetryAsync) @@ -113,6 +115,8 @@ TEST_F(SitlTest, TelemetryAsync) telemetry->rc_status_async(std::bind(&print_rc_status, _1)); + telemetry->mav_message_async(std::bind(&print_mav_message, _1)); + telemetry->position_velocity_ned_async(std::bind(&print_position_velocity_ned, _1)); std::this_thread::sleep_for(std::chrono::seconds(10)); @@ -132,6 +136,7 @@ TEST_F(SitlTest, TelemetryAsync) EXPECT_TRUE(_received_gps_info); EXPECT_TRUE(_received_battery); EXPECT_TRUE(_received_rc_status); + EXPECT_TRUE(_received_mav_message); EXPECT_TRUE(_received_position_velocity_ned); } @@ -238,6 +243,13 @@ void print_rc_status(Telemetry::RCStatus rc_status) _received_rc_status = true; } +void print_mav_message(Telemetry::MavMessage mav_message) +{ + std::cout << "Mav Message [" << mav_message.message_str << "]" + << std::endl; + _received_mav_message = true; +} + void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned) { std::cout << "Got position north: " << position_velocity_ned.position.north_m << " m, " diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 3e5c75c251..3b1219d70e 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -129,6 +129,10 @@ class Telemetry : public PluginBase { 5: RTK float, 6: RTK fixed). */ }; + struct MavMessage { + std::string message_str; + }; + /** * @brief Battery type. */ @@ -246,6 +250,8 @@ class Telemetry : public PluginBase { */ Result set_rate_in_air(double rate_hz); + Result set_rate_mav_message(double rate_hz); + /** * @brief Set rate of attitude updates (synchronous). * @@ -327,6 +333,8 @@ class Telemetry : public PluginBase { */ void set_rate_in_air_async(double rate_hz, result_callback_t callback); + void set_rate_mav_message_async(double rate_hz, result_callback_t callback); + /** * @brief Set rate of attitude updates (asynchronous). * @@ -403,6 +411,8 @@ class Telemetry : public PluginBase { */ bool in_air() const; + MavMessage mav_message() const; + /** * @brief Get the arming status (synchronous). * @@ -527,6 +537,8 @@ class Telemetry : public PluginBase { */ typedef std::function in_air_callback_t; + typedef std::function mav_message_callback_t; // Anotacao: Remove mav_message from here? + /** * @brief Subscribe to in-air updates (asynchronous). * @@ -534,6 +546,8 @@ class Telemetry : public PluginBase { */ void in_air_async(in_air_callback_t callback); + void mav_message_async(mav_message_callback_t callback); + /** * @brief Callback type for armed updates (asynchronous). * @@ -853,4 +867,6 @@ bool operator==(const Telemetry::RCStatus &lhs, const Telemetry::RCStatus &rhs); */ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status); +std::ostream &operator<<(std::ostream &str, Telemetry::MavMessage const &mav_message); + } // namespace dronecode_sdk diff --git a/plugins/telemetry/mocks/telemetry_mock.h b/plugins/telemetry/mocks/telemetry_mock.h index dd5409ee4b..4ccbc42e14 100644 --- a/plugins/telemetry/mocks/telemetry_mock.h +++ b/plugins/telemetry/mocks/telemetry_mock.h @@ -11,6 +11,7 @@ class MockTelemetry { MOCK_CONST_METHOD1(health_async, void(Telemetry::health_callback_t)){}; MOCK_CONST_METHOD1(home_position_async, void(Telemetry::position_callback_t)){}; MOCK_CONST_METHOD1(in_air_async, void(Telemetry::in_air_callback_t)){}; + MOCK_CONST_METHOD1(mav_message_async, void(Telemetry::mav_message_callback_t)){}; MOCK_CONST_METHOD1(armed_async, void(Telemetry::armed_callback_t)){}; MOCK_CONST_METHOD1(gps_info_async, void(Telemetry::gps_info_callback_t)){}; MOCK_CONST_METHOD1(battery_async, void(Telemetry::battery_callback_t)){}; diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index b4c705238c..1526703545 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -31,6 +31,11 @@ Telemetry::Result Telemetry::set_rate_in_air(double rate_hz) return _impl->set_rate_in_air(rate_hz); } +Telemetry::Result Telemetry::set_rate_mav_message(double rate_hz) +{ + return _impl->set_rate_mav_message(rate_hz); +} + Telemetry::Result Telemetry::set_rate_attitude(double rate_hz) { return _impl->set_rate_attitude(rate_hz); @@ -81,6 +86,11 @@ void Telemetry::set_rate_in_air_async(double rate_hz, result_callback_t callback _impl->set_rate_in_air_async(rate_hz, callback); } +void Telemetry::set_rate_mav_message_async(double rate_hz, result_callback_t callback) +{ + _impl->set_rate_mav_message_async(rate_hz, callback); +} + void Telemetry::set_rate_attitude_async(double rate_hz, result_callback_t callback) { _impl->set_rate_attitude_async(rate_hz, callback); @@ -131,6 +141,11 @@ bool Telemetry::in_air() const return _impl->in_air(); } +Telemetry::MavMessage Telemetry::mav_message() const +{ + return _impl->get_mav_message(); +} + bool Telemetry::armed() const { return _impl->armed(); @@ -211,6 +226,11 @@ void Telemetry::in_air_async(in_air_callback_t callback) return _impl->in_air_async(callback); } +void Telemetry::mav_message_async(mav_message_callback_t callback) +{ + return _impl->mav_message_async(callback); +} + void Telemetry::armed_async(armed_callback_t callback) { return _impl->armed_async(callback); @@ -472,4 +492,14 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status << ", signal_strength_percent: " << rc_status.signal_strength_percent << "]"; } +bool operator==(const Telemetry::MavMessage &lhs, const Telemetry::MavMessage &rhs) +{ + return lhs.message_str == rhs.message_str ; +} + +std::ostream &operator<<(std::ostream &str, Telemetry::MavMessage const &mav_message) +{ + return str << "[message: " << mav_message.message_str << "]"; +} + } // namespace dronecode_sdk diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index 85f7966c5d..c8c23290ec 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -5,6 +5,7 @@ #include "px4_custom_mode.h" #include #include +#include namespace dronecode_sdk { @@ -61,6 +62,9 @@ void TelemetryImpl::init() _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_HEARTBEAT, std::bind(&TelemetryImpl::process_heartbeat, this, _1), this); + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_STATUSTEXT, std::bind(&TelemetryImpl::process_statustext, this, _1), this); + _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_RC_CHANNELS, std::bind(&TelemetryImpl::process_rc_channels, this, _1), this); @@ -152,6 +156,12 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz) _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); } +Telemetry::Result TelemetryImpl::set_rate_mav_message(double rate_hz) +{ + return telemetry_result_from_command_result( + _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); // Anotacao: MAVLINK_MSG_ID_STATUSTEXT? +} + Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz) { return telemetry_result_from_command_result( @@ -228,6 +238,14 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::result_call std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } +void TelemetryImpl::set_rate_mav_message_async(double rate_hz, Telemetry::result_callback_t callback) +{ + _parent->set_msg_rate_async( + MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: Possible error here, since I don't know what to set. + rate_hz, + std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); +} + void TelemetryImpl::set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback) { _parent->set_msg_rate_async( @@ -489,6 +507,56 @@ void TelemetryImpl::process_heartbeat(const mavlink_message_t &message) } } +void TelemetryImpl::process_statustext(const mavlink_message_t &message) +{ + mavlink_statustext_t statustext; + mavlink_msg_statustext_decode(&message, &statustext); + + std::string debug_str = "MAVLink: "; + + switch (statustext.severity) { + case MAV_SEVERITY_EMERGENCY: + debug_str += "emergency"; + break; + case MAV_SEVERITY_ALERT: + debug_str += "alert"; + break; + case MAV_SEVERITY_CRITICAL: + debug_str += "critical"; + break; + case MAV_SEVERITY_ERROR: + debug_str += "error"; + break; + case MAV_SEVERITY_WARNING: + debug_str += "warning"; + break; + case MAV_SEVERITY_NOTICE: + debug_str += "notice"; + break; + case MAV_SEVERITY_INFO: + debug_str += "info"; + break; + case MAV_SEVERITY_DEBUG: + debug_str += "debug"; + break; + default: + break; + } + + // statustext.text is not null terminated, therefore we copy it first to + // an array big enough that is zeroed. + char text_with_null[sizeof(statustext.text) + 1]{}; + memcpy(text_with_null, statustext.text, sizeof(statustext.text)); + + std::string mav_message_str = debug_str + ": " + text_with_null; + + set_mav_message({mav_message_str}); + + if (_mav_message_subscription) { + _mav_message_subscription(get_mav_message()); + } +} + void TelemetryImpl::process_rc_channels(const mavlink_message_t &message) { mavlink_rc_channels_t rc_channels; @@ -659,6 +727,18 @@ void TelemetryImpl::set_in_air(bool in_air_new) _in_air = in_air_new; } +void TelemetryImpl::set_mav_message(Telemetry::MavMessage mav_message) +{ + std::lock_guard lock(_mav_message_mutex); + _mav_message = mav_message; +} + +Telemetry::MavMessage TelemetryImpl::get_mav_message() const +{ + std::lock_guard lock(_mav_message_mutex); + return _mav_message; +} + void TelemetryImpl::set_armed(bool armed_new) { _armed = armed_new; @@ -854,6 +934,11 @@ void TelemetryImpl::in_air_async(Telemetry::in_air_callback_t &callback) _in_air_subscription = callback; } +void TelemetryImpl::mav_message_async(Telemetry::mav_message_callback_t &callback) +{ + _mav_message_subscription = callback; +} + void TelemetryImpl::armed_async(Telemetry::armed_callback_t &callback) { _armed_subscription = callback; diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index f110f8bd19..e9f71f34ec 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -30,6 +30,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Result set_rate_position(double rate_hz); Telemetry::Result set_rate_home_position(double rate_hz); Telemetry::Result set_rate_in_air(double rate_hz); + Telemetry::Result set_rate_mav_message(double rate_hz); Telemetry::Result set_rate_attitude(double rate_hz); Telemetry::Result set_rate_camera_attitude(double rate_hz); Telemetry::Result set_rate_ground_speed_ned(double rate_hz); @@ -42,6 +43,7 @@ class TelemetryImpl : public PluginImplBase { void set_rate_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_home_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_in_air_async(double rate_hz, Telemetry::result_callback_t callback); + void set_rate_mav_message_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_camera_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_ground_speed_ned_async(double rate_hz, Telemetry::result_callback_t callback); @@ -54,6 +56,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Position get_home_position() const; bool in_air() const; bool armed() const; + Telemetry::MavMessage get_mav_message() const; Telemetry::EulerAngle get_attitude_euler_angle() const; Telemetry::Quaternion get_attitude_quaternion() const; Telemetry::EulerAngle get_camera_attitude_euler_angle() const; @@ -70,6 +73,7 @@ class TelemetryImpl : public PluginImplBase { void position_async(Telemetry::position_callback_t &callback); void home_position_async(Telemetry::position_callback_t &callback); void in_air_async(Telemetry::in_air_callback_t &callback); + void mav_message_async(Telemetry::mav_message_callback_t &callback); void armed_async(Telemetry::armed_callback_t &callback); void attitude_quaternion_async(Telemetry::attitude_quaternion_callback_t &callback); void attitude_euler_angle_async(Telemetry::attitude_euler_angle_callback_t &callback); @@ -91,6 +95,7 @@ class TelemetryImpl : public PluginImplBase { void set_position(Telemetry::Position position); void set_home_position(Telemetry::Position home_position); void set_in_air(bool in_air); + void set_mav_message(Telemetry::MavMessage mav_message); // Anotacao: HERE> void set_armed(bool armed); void set_attitude_quaternion(Telemetry::Quaternion quaternion); void set_camera_attitude_euler_angle(Telemetry::EulerAngle euler_angle); @@ -116,6 +121,7 @@ class TelemetryImpl : public PluginImplBase { void process_extended_sys_state(const mavlink_message_t &message); void process_sys_status(const mavlink_message_t &message); void process_heartbeat(const mavlink_message_t &message); + void process_statustext(const mavlink_message_t &message); // Anotacao: Problem here? void process_rc_channels(const mavlink_message_t &message); void receive_param_cal_gyro(MAVLinkParameters::Result result, int value); @@ -154,6 +160,10 @@ class TelemetryImpl : public PluginImplBase { std::atomic_bool _in_air{false}; std::atomic_bool _armed{false}; + // Anotacao + mutable std::mutex _mav_message_mutex{}; + Telemetry::MavMessage _mav_message{""}; + mutable std::mutex _attitude_quaternion_mutex{}; Telemetry::Quaternion _attitude_quaternion{NAN, NAN, NAN, NAN}; @@ -184,6 +194,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::position_callback_t _position_subscription{nullptr}; Telemetry::position_callback_t _home_position_subscription{nullptr}; Telemetry::in_air_callback_t _in_air_subscription{nullptr}; + Telemetry::mav_message_callback_t _mav_message_subscription{nullptr}; Telemetry::armed_callback_t _armed_subscription{nullptr}; Telemetry::attitude_quaternion_callback_t _attitude_quaternion_subscription{nullptr}; Telemetry::attitude_euler_angle_callback_t _attitude_euler_angle_subscription{nullptr}; From 2f9a26f1f87777c12dee5787c01f9bd56ab81b1e Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Thu, 25 Apr 2019 17:45:21 -0700 Subject: [PATCH 02/25] Add enum. --- .../telemetry/telemetry_service_impl.h | 29 +++++++++++++++++- .../include/plugins/telemetry/telemetry.h | 11 +++++++ plugins/telemetry/telemetry.cpp | 3 +- plugins/telemetry/telemetry_impl.cpp | 30 ++++++++++++------- plugins/telemetry/telemetry_impl.h | 2 +- 5 files changed, 62 insertions(+), 13 deletions(-) diff --git a/backend/src/plugins/telemetry/telemetry_service_impl.h b/backend/src/plugins/telemetry/telemetry_service_impl.h index 02c07ee32b..b172317b9d 100644 --- a/backend/src/plugins/telemetry/telemetry_service_impl.h +++ b/backend/src/plugins/telemetry/telemetry_service_impl.h @@ -101,9 +101,10 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet const dronecode_sdk::rpc::telemetry::SubscribeMavMessageRequest * /* request */, grpc::ServerWriter *writer) override { - _telemetry.mav_message_async([&writer](dronecode_sdk::Telemetry::MavMessage mav_message) { + _telemetry.mav_message_async([this, &writer](dronecode_sdk::Telemetry::MavMessage mav_message) { auto rpc_mav_message = new dronecode_sdk::rpc::telemetry::MavMessage(); rpc_mav_message->set_message_str(mav_message.message_str); + rpc_mav_message->set_message_type(translateMavMessageType(mav_message.message_type)); dronecode_sdk::rpc::telemetry::MavMessageResponse rpc_mav_message_response; rpc_mav_message_response.set_allocated_mav_message(rpc_mav_message); @@ -114,6 +115,32 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet return grpc::Status::OK; } + dronecode_sdk::rpc::telemetry::MavMessage::MessageType + translateMavMessageType(const dronecode_sdk::Telemetry::MavMessage::MessageType message_type) const + { + switch (message_type) { + default: + case dronecode_sdk::Telemetry::MavMessage::MessageType::EMERGENCY: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_EMERGENCY; + case dronecode_sdk::Telemetry::MavMessage::MessageType::ALERT: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_ALERT; + case dronecode_sdk::Telemetry::MavMessage::MessageType::CRITICAL: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_CRITICAL; + case dronecode_sdk::Telemetry::MavMessage::MessageType::ERROR: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_ERROR; + case dronecode_sdk::Telemetry::MavMessage::MessageType::WARNING: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_WARNING; + case dronecode_sdk::Telemetry::MavMessage::MessageType::NOTICE: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_NOTICE; + case dronecode_sdk::Telemetry::MavMessage::MessageType::INFO: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_INFO; + case dronecode_sdk::Telemetry::MavMessage::MessageType::DEBUS: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_DEBUS; + case dronecode_sdk::Telemetry::MavMessage::MessageType::UNKNOWN: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_UNKNOWN; + } + } + grpc::Status SubscribeArmed(grpc::ServerContext * /* context */, const dronecode_sdk::rpc::telemetry::SubscribeArmedRequest * /* request */, diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 3b1219d70e..0651a71c0f 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -130,6 +130,17 @@ class Telemetry : public PluginBase { }; struct MavMessage { + enum class MessageType { + EMERGENCY, + ALERT, + CRITICAL, + ERROR, + WARNING, + NOTICE, + INFO, + DEBUS, + UNKNOWN + } message_type; std::string message_str; }; diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index 1526703545..d27210eb5c 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -494,7 +494,8 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status bool operator==(const Telemetry::MavMessage &lhs, const Telemetry::MavMessage &rhs) { - return lhs.message_str == rhs.message_str ; + return lhs.message_str == rhs.message_str && + lhs.message_type == rhs.message_type; } std::ostream &operator<<(std::ostream &str, Telemetry::MavMessage const &mav_message) diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index c8c23290ec..10962f2eea 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -513,33 +513,43 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) mavlink_msg_statustext_decode(&message, &statustext); std::string debug_str = "MAVLink: "; + Telemetry::MavMessage::MessageType mav_message_type; switch (statustext.severity) { case MAV_SEVERITY_EMERGENCY: - debug_str += "emergency"; + mav_message_type = Telemetry::MavMessage::MessageType::EMERGENCY; + // debug_str += "emergency"; break; case MAV_SEVERITY_ALERT: - debug_str += "alert"; + mav_message_type = Telemetry::MavMessage::MessageType::ALERT; + // debug_str += "alert"; break; case MAV_SEVERITY_CRITICAL: - debug_str += "critical"; + mav_message_type = Telemetry::MavMessage::MessageType::CRITICAL; + // debug_str += "critical"; break; case MAV_SEVERITY_ERROR: - debug_str += "error"; + mav_message_type = Telemetry::MavMessage::MessageType::ERROR; + // debug_str += "error"; break; case MAV_SEVERITY_WARNING: - debug_str += "warning"; + mav_message_type = Telemetry::MavMessage::MessageType::WARNING; + // debug_str += "warning"; break; case MAV_SEVERITY_NOTICE: - debug_str += "notice"; + mav_message_type = Telemetry::MavMessage::MessageType::NOTICE; + // debug_str += "notice"; break; case MAV_SEVERITY_INFO: - debug_str += "info"; + mav_message_type = Telemetry::MavMessage::MessageType::INFO; + // debug_str += "info"; break; case MAV_SEVERITY_DEBUG: - debug_str += "debug"; + mav_message_type = Telemetry::MavMessage::MessageType::DEBUS; + // debug_str += "debug"; break; default: + mav_message_type = Telemetry::MavMessage::MessageType::UNKNOWN; break; } @@ -548,9 +558,9 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) char text_with_null[sizeof(statustext.text) + 1]{}; memcpy(text_with_null, statustext.text, sizeof(statustext.text)); - std::string mav_message_str = debug_str + ": " + text_with_null; + std::string mav_message_str = text_with_null; //debug_str + ": " + text_with_null; - set_mav_message({mav_message_str}); + set_mav_message({mav_message_type, mav_message_str}); if (_mav_message_subscription) { _mav_message_subscription(get_mav_message()); diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index e9f71f34ec..827fbcf663 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -162,7 +162,7 @@ class TelemetryImpl : public PluginImplBase { // Anotacao mutable std::mutex _mav_message_mutex{}; - Telemetry::MavMessage _mav_message{""}; + Telemetry::MavMessage _mav_message{Telemetry::MavMessage::MessageType::UNKNOWN, ""}; mutable std::mutex _attitude_quaternion_mutex{}; Telemetry::Quaternion _attitude_quaternion{NAN, NAN, NAN, NAN}; From 2757765bad8516ea8cc077f54805f8dac8285917 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Fri, 26 Apr 2019 15:01:19 -0700 Subject: [PATCH 03/25] Rename it to StatusText. --- .../telemetry/telemetry_service_impl.h | 52 ++++++-------- integration_tests/telemetry_async.cpp | 14 ++-- .../include/plugins/telemetry/telemetry.h | 66 ++++++++++++------ plugins/telemetry/mocks/telemetry_mock.h | 2 +- plugins/telemetry/telemetry.cpp | 26 +++---- plugins/telemetry/telemetry_impl.cpp | 69 ++++++------------- plugins/telemetry/telemetry_impl.h | 19 +++-- 7 files changed, 117 insertions(+), 131 deletions(-) diff --git a/backend/src/plugins/telemetry/telemetry_service_impl.h b/backend/src/plugins/telemetry/telemetry_service_impl.h index b172317b9d..d66228e0be 100644 --- a/backend/src/plugins/telemetry/telemetry_service_impl.h +++ b/backend/src/plugins/telemetry/telemetry_service_impl.h @@ -97,47 +97,35 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet } grpc::Status - SubscribeMavMessage(grpc::ServerContext * /* context */, - const dronecode_sdk::rpc::telemetry::SubscribeMavMessageRequest * /* request */, - grpc::ServerWriter *writer) override + SubscribeStatusText(grpc::ServerContext * /* context */, + const dronecode_sdk::rpc::telemetry::SubscribeStatusTextRequest * /* request */, + grpc::ServerWriter *writer) override { - _telemetry.mav_message_async([this, &writer](dronecode_sdk::Telemetry::MavMessage mav_message) { - auto rpc_mav_message = new dronecode_sdk::rpc::telemetry::MavMessage(); - rpc_mav_message->set_message_str(mav_message.message_str); - rpc_mav_message->set_message_type(translateMavMessageType(mav_message.message_type)); - - dronecode_sdk::rpc::telemetry::MavMessageResponse rpc_mav_message_response; - rpc_mav_message_response.set_allocated_mav_message(rpc_mav_message); - writer->Write(rpc_mav_message_response); + _telemetry.status_text_async([this, &writer](dronecode_sdk::Telemetry::StatusText status_text) { + auto rpc_status_text = new dronecode_sdk::rpc::telemetry::StatusText(); + rpc_status_text->set_text(status_text.text); + rpc_status_text->set_type(translateStatusTextType(status_text.type)); + + dronecode_sdk::rpc::telemetry::StatusTextResponse rpc_status_text_response; + rpc_status_text_response.set_allocated_status_text(rpc_status_text); + writer->Write(rpc_status_text_response); }); _stop_future.wait(); return grpc::Status::OK; } - dronecode_sdk::rpc::telemetry::MavMessage::MessageType - translateMavMessageType(const dronecode_sdk::Telemetry::MavMessage::MessageType message_type) const + dronecode_sdk::rpc::telemetry::StatusText::StatusType + translateStatusTextType(const dronecode_sdk::Telemetry::StatusText::StatusType type) const { - switch (message_type) { + switch (type) { default: - case dronecode_sdk::Telemetry::MavMessage::MessageType::EMERGENCY: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_EMERGENCY; - case dronecode_sdk::Telemetry::MavMessage::MessageType::ALERT: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_ALERT; - case dronecode_sdk::Telemetry::MavMessage::MessageType::CRITICAL: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_CRITICAL; - case dronecode_sdk::Telemetry::MavMessage::MessageType::ERROR: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_ERROR; - case dronecode_sdk::Telemetry::MavMessage::MessageType::WARNING: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_WARNING; - case dronecode_sdk::Telemetry::MavMessage::MessageType::NOTICE: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_NOTICE; - case dronecode_sdk::Telemetry::MavMessage::MessageType::INFO: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_INFO; - case dronecode_sdk::Telemetry::MavMessage::MessageType::DEBUS: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_DEBUS; - case dronecode_sdk::Telemetry::MavMessage::MessageType::UNKNOWN: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_UNKNOWN; + case dronecode_sdk::Telemetry::StatusText::StatusType::INFO: + return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_INFO; + case dronecode_sdk::Telemetry::StatusText::StatusType::WARNING: + return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_WARNING; + case dronecode_sdk::Telemetry::StatusText::StatusType::CRITICAL: + return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_CRITICAL; } } diff --git a/integration_tests/telemetry_async.cpp b/integration_tests/telemetry_async.cpp index aebbb776b3..971b5a5a7c 100644 --- a/integration_tests/telemetry_async.cpp +++ b/integration_tests/telemetry_async.cpp @@ -23,7 +23,7 @@ static void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned); static void print_gps_info(Telemetry::GPSInfo gps_info); static void print_battery(Telemetry::Battery battery); static void print_rc_status(Telemetry::RCStatus rc_status); -static void print_mav_message(Telemetry::MavMessage mav_message); +static void print_status_text(Telemetry::StatusText status_text); static void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned); static bool _set_rate_error = false; @@ -41,7 +41,7 @@ static bool _received_ground_speed = false; static bool _received_gps_info = false; static bool _received_battery = false; static bool _received_rc_status = false; -static bool _received_mav_message = false; +static bool _received_status_text = false; static bool _received_position_velocity_ned = false; TEST_F(SitlTest, TelemetryAsync) @@ -115,7 +115,7 @@ TEST_F(SitlTest, TelemetryAsync) telemetry->rc_status_async(std::bind(&print_rc_status, _1)); - telemetry->mav_message_async(std::bind(&print_mav_message, _1)); + telemetry->status_text_async(std::bind(&print_status_text, _1)); telemetry->position_velocity_ned_async(std::bind(&print_position_velocity_ned, _1)); @@ -136,7 +136,7 @@ TEST_F(SitlTest, TelemetryAsync) EXPECT_TRUE(_received_gps_info); EXPECT_TRUE(_received_battery); EXPECT_TRUE(_received_rc_status); - EXPECT_TRUE(_received_mav_message); + EXPECT_TRUE(_received_status_text); EXPECT_TRUE(_received_position_velocity_ned); } @@ -243,11 +243,11 @@ void print_rc_status(Telemetry::RCStatus rc_status) _received_rc_status = true; } -void print_mav_message(Telemetry::MavMessage mav_message) +void print_status_text(Telemetry::StatusText status_text) { - std::cout << "Mav Message [" << mav_message.message_str << "]" + std::cout << "Status Text [" << status_text.text << "]" << std::endl; - _received_mav_message = true; + _received_status_text = true; } void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned) diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 0651a71c0f..94ae8ffcbe 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -129,19 +129,13 @@ class Telemetry : public PluginBase { 5: RTK float, 6: RTK fixed). */ }; - struct MavMessage { - enum class MessageType { - EMERGENCY, - ALERT, - CRITICAL, - ERROR, - WARNING, - NOTICE, - INFO, - DEBUS, - UNKNOWN - } message_type; - std::string message_str; + struct StatusText { + enum class StatusType { + INFO, + WARNING, + CRITICAL + } type; + std::string text; }; /** @@ -261,7 +255,13 @@ class Telemetry : public PluginBase { */ Result set_rate_in_air(double rate_hz); - Result set_rate_mav_message(double rate_hz); + /** + * @brief Set rate of status text updates (synchronous). + * + * @param rate_hz Rate in Hz. + * @return Result of request. + */ + Result set_rate_status_text(double rate_hz); /** * @brief Set rate of attitude updates (synchronous). @@ -344,7 +344,13 @@ class Telemetry : public PluginBase { */ void set_rate_in_air_async(double rate_hz, result_callback_t callback); - void set_rate_mav_message_async(double rate_hz, result_callback_t callback); + /** + * @brief Set rate of status text updates (asynchronous). + * + * @param rate_hz Rate in Hz. + * @param callback Callback to receive request result. + */ + void set_rate_status_text_async(double rate_hz, result_callback_t callback); /** * @brief Set rate of attitude updates (asynchronous). @@ -415,6 +421,13 @@ class Telemetry : public PluginBase { */ Position home_position() const; + /** + * @brief Get status text (synchronous). + * + * @return Status text. + */ + StatusText status_text() const; + /** * @brief Get the in-air status (synchronous). * @@ -422,8 +435,6 @@ class Telemetry : public PluginBase { */ bool in_air() const; - MavMessage mav_message() const; - /** * @brief Get the arming status (synchronous). * @@ -548,7 +559,12 @@ class Telemetry : public PluginBase { */ typedef std::function in_air_callback_t; - typedef std::function mav_message_callback_t; // Anotacao: Remove mav_message from here? + /** + * @brief Callback for mavlink status text updates. + * + * @param status text with message type and text. + */ + typedef std::function status_text_callback_t; /** * @brief Subscribe to in-air updates (asynchronous). @@ -557,7 +573,12 @@ class Telemetry : public PluginBase { */ void in_air_async(in_air_callback_t callback); - void mav_message_async(mav_message_callback_t callback); + /** + * @brief Subscribe to status text updates (asynchronous). + * + * @param callback Function to call with updates. + */ + void status_text_async(status_text_callback_t callback); /** * @brief Callback type for armed updates (asynchronous). @@ -878,6 +899,11 @@ bool operator==(const Telemetry::RCStatus &lhs, const Telemetry::RCStatus &rhs); */ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status); -std::ostream &operator<<(std::ostream &str, Telemetry::MavMessage const &mav_message); +/** + * @brief Stream operator to print information about a `Telemetry::StatusText`. + * + * @returns A reference to the stream. +*/ +std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text); } // namespace dronecode_sdk diff --git a/plugins/telemetry/mocks/telemetry_mock.h b/plugins/telemetry/mocks/telemetry_mock.h index 4ccbc42e14..5f57a6b7a7 100644 --- a/plugins/telemetry/mocks/telemetry_mock.h +++ b/plugins/telemetry/mocks/telemetry_mock.h @@ -11,7 +11,7 @@ class MockTelemetry { MOCK_CONST_METHOD1(health_async, void(Telemetry::health_callback_t)){}; MOCK_CONST_METHOD1(home_position_async, void(Telemetry::position_callback_t)){}; MOCK_CONST_METHOD1(in_air_async, void(Telemetry::in_air_callback_t)){}; - MOCK_CONST_METHOD1(mav_message_async, void(Telemetry::mav_message_callback_t)){}; + MOCK_CONST_METHOD1(status_text_async, void(Telemetry::status_text_callback_t)){}; MOCK_CONST_METHOD1(armed_async, void(Telemetry::armed_callback_t)){}; MOCK_CONST_METHOD1(gps_info_async, void(Telemetry::gps_info_callback_t)){}; MOCK_CONST_METHOD1(battery_async, void(Telemetry::battery_callback_t)){}; diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index d27210eb5c..d7608091c3 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -31,9 +31,9 @@ Telemetry::Result Telemetry::set_rate_in_air(double rate_hz) return _impl->set_rate_in_air(rate_hz); } -Telemetry::Result Telemetry::set_rate_mav_message(double rate_hz) +Telemetry::Result Telemetry::set_rate_status_text(double rate_hz) { - return _impl->set_rate_mav_message(rate_hz); + return _impl->set_rate_status_text(rate_hz); } Telemetry::Result Telemetry::set_rate_attitude(double rate_hz) @@ -86,9 +86,9 @@ void Telemetry::set_rate_in_air_async(double rate_hz, result_callback_t callback _impl->set_rate_in_air_async(rate_hz, callback); } -void Telemetry::set_rate_mav_message_async(double rate_hz, result_callback_t callback) +void Telemetry::set_rate_status_text_async(double rate_hz, result_callback_t callback) { - _impl->set_rate_mav_message_async(rate_hz, callback); + _impl->set_rate_status_text_async(rate_hz, callback); } void Telemetry::set_rate_attitude_async(double rate_hz, result_callback_t callback) @@ -141,9 +141,9 @@ bool Telemetry::in_air() const return _impl->in_air(); } -Telemetry::MavMessage Telemetry::mav_message() const +Telemetry::StatusText Telemetry::status_text() const { - return _impl->get_mav_message(); + return _impl->get_status_text(); } bool Telemetry::armed() const @@ -226,9 +226,9 @@ void Telemetry::in_air_async(in_air_callback_t callback) return _impl->in_air_async(callback); } -void Telemetry::mav_message_async(mav_message_callback_t callback) +void Telemetry::status_text_async(status_text_callback_t callback) { - return _impl->mav_message_async(callback); + return _impl->status_text_async(callback); } void Telemetry::armed_async(armed_callback_t callback) @@ -492,15 +492,15 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status << ", signal_strength_percent: " << rc_status.signal_strength_percent << "]"; } -bool operator==(const Telemetry::MavMessage &lhs, const Telemetry::MavMessage &rhs) +bool operator==(const Telemetry::StatusText &lhs, const Telemetry::StatusText &rhs) { - return lhs.message_str == rhs.message_str && - lhs.message_type == rhs.message_type; + return lhs.text == rhs.text && + lhs.type == rhs.type; } -std::ostream &operator<<(std::ostream &str, Telemetry::MavMessage const &mav_message) +std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text) { - return str << "[message: " << mav_message.message_str << "]"; + return str << "[statustext: " << status_text.text << "]"; } } // namespace dronecode_sdk diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index 10962f2eea..e88f69b275 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -156,10 +156,10 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz) _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); } -Telemetry::Result TelemetryImpl::set_rate_mav_message(double rate_hz) +Telemetry::Result TelemetryImpl::set_rate_status_text(double rate_hz) { return telemetry_result_from_command_result( - _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); // Anotacao: MAVLINK_MSG_ID_STATUSTEXT? + _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? } Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz) @@ -238,10 +238,10 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::result_call std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } -void TelemetryImpl::set_rate_mav_message_async(double rate_hz, Telemetry::result_callback_t callback) +void TelemetryImpl::set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback) { _parent->set_msg_rate_async( - MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: Possible error here, since I don't know what to set. + MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: What rate to set here? rate_hz, std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } @@ -512,44 +512,17 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) mavlink_statustext_t statustext; mavlink_msg_statustext_decode(&message, &statustext); - std::string debug_str = "MAVLink: "; - Telemetry::MavMessage::MessageType mav_message_type; + Telemetry::StatusText::StatusType type; switch (statustext.severity) { - case MAV_SEVERITY_EMERGENCY: - mav_message_type = Telemetry::MavMessage::MessageType::EMERGENCY; - // debug_str += "emergency"; - break; - case MAV_SEVERITY_ALERT: - mav_message_type = Telemetry::MavMessage::MessageType::ALERT; - // debug_str += "alert"; - break; - case MAV_SEVERITY_CRITICAL: - mav_message_type = Telemetry::MavMessage::MessageType::CRITICAL; - // debug_str += "critical"; - break; - case MAV_SEVERITY_ERROR: - mav_message_type = Telemetry::MavMessage::MessageType::ERROR; - // debug_str += "error"; - break; case MAV_SEVERITY_WARNING: - mav_message_type = Telemetry::MavMessage::MessageType::WARNING; - // debug_str += "warning"; - break; - case MAV_SEVERITY_NOTICE: - mav_message_type = Telemetry::MavMessage::MessageType::NOTICE; - // debug_str += "notice"; + type = Telemetry::StatusText::StatusType::WARNING; break; - case MAV_SEVERITY_INFO: - mav_message_type = Telemetry::MavMessage::MessageType::INFO; - // debug_str += "info"; - break; - case MAV_SEVERITY_DEBUG: - mav_message_type = Telemetry::MavMessage::MessageType::DEBUS; - // debug_str += "debug"; + case MAV_SEVERITY_CRITICAL: + type = Telemetry::StatusText::StatusType::CRITICAL; break; default: - mav_message_type = Telemetry::MavMessage::MessageType::UNKNOWN; + type = Telemetry::StatusText::StatusType::INFO; break; } @@ -558,12 +531,12 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) char text_with_null[sizeof(statustext.text) + 1]{}; memcpy(text_with_null, statustext.text, sizeof(statustext.text)); - std::string mav_message_str = text_with_null; //debug_str + ": " + text_with_null; + std::string text = text_with_null; - set_mav_message({mav_message_type, mav_message_str}); + set_status_text({type, text}); - if (_mav_message_subscription) { - _mav_message_subscription(get_mav_message()); + if (_status_text_subscription) { + _status_text_subscription(get_status_text()); } } @@ -737,16 +710,16 @@ void TelemetryImpl::set_in_air(bool in_air_new) _in_air = in_air_new; } -void TelemetryImpl::set_mav_message(Telemetry::MavMessage mav_message) +void TelemetryImpl::set_status_text(Telemetry::StatusText status_text) { - std::lock_guard lock(_mav_message_mutex); - _mav_message = mav_message; + std::lock_guard lock(_status_text_mutex); + _status_text = status_text; } -Telemetry::MavMessage TelemetryImpl::get_mav_message() const +Telemetry::StatusText TelemetryImpl::get_status_text() const { - std::lock_guard lock(_mav_message_mutex); - return _mav_message; + std::lock_guard lock(_status_text_mutex); + return _status_text; } void TelemetryImpl::set_armed(bool armed_new) @@ -944,9 +917,9 @@ void TelemetryImpl::in_air_async(Telemetry::in_air_callback_t &callback) _in_air_subscription = callback; } -void TelemetryImpl::mav_message_async(Telemetry::mav_message_callback_t &callback) +void TelemetryImpl::status_text_async(Telemetry::status_text_callback_t &callback) { - _mav_message_subscription = callback; + _status_text_subscription = callback; } void TelemetryImpl::armed_async(Telemetry::armed_callback_t &callback) diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index 827fbcf663..56a11f2383 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -30,7 +30,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Result set_rate_position(double rate_hz); Telemetry::Result set_rate_home_position(double rate_hz); Telemetry::Result set_rate_in_air(double rate_hz); - Telemetry::Result set_rate_mav_message(double rate_hz); + Telemetry::Result set_rate_status_text(double rate_hz); Telemetry::Result set_rate_attitude(double rate_hz); Telemetry::Result set_rate_camera_attitude(double rate_hz); Telemetry::Result set_rate_ground_speed_ned(double rate_hz); @@ -43,7 +43,7 @@ class TelemetryImpl : public PluginImplBase { void set_rate_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_home_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_in_air_async(double rate_hz, Telemetry::result_callback_t callback); - void set_rate_mav_message_async(double rate_hz, Telemetry::result_callback_t callback); + void set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_camera_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_ground_speed_ned_async(double rate_hz, Telemetry::result_callback_t callback); @@ -56,7 +56,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Position get_home_position() const; bool in_air() const; bool armed() const; - Telemetry::MavMessage get_mav_message() const; + Telemetry::StatusText get_status_text() const; Telemetry::EulerAngle get_attitude_euler_angle() const; Telemetry::Quaternion get_attitude_quaternion() const; Telemetry::EulerAngle get_camera_attitude_euler_angle() const; @@ -73,7 +73,7 @@ class TelemetryImpl : public PluginImplBase { void position_async(Telemetry::position_callback_t &callback); void home_position_async(Telemetry::position_callback_t &callback); void in_air_async(Telemetry::in_air_callback_t &callback); - void mav_message_async(Telemetry::mav_message_callback_t &callback); + void status_text_async(Telemetry::status_text_callback_t &callback); void armed_async(Telemetry::armed_callback_t &callback); void attitude_quaternion_async(Telemetry::attitude_quaternion_callback_t &callback); void attitude_euler_angle_async(Telemetry::attitude_euler_angle_callback_t &callback); @@ -95,7 +95,7 @@ class TelemetryImpl : public PluginImplBase { void set_position(Telemetry::Position position); void set_home_position(Telemetry::Position home_position); void set_in_air(bool in_air); - void set_mav_message(Telemetry::MavMessage mav_message); // Anotacao: HERE> + void set_status_text(Telemetry::StatusText status_text); void set_armed(bool armed); void set_attitude_quaternion(Telemetry::Quaternion quaternion); void set_camera_attitude_euler_angle(Telemetry::EulerAngle euler_angle); @@ -121,7 +121,7 @@ class TelemetryImpl : public PluginImplBase { void process_extended_sys_state(const mavlink_message_t &message); void process_sys_status(const mavlink_message_t &message); void process_heartbeat(const mavlink_message_t &message); - void process_statustext(const mavlink_message_t &message); // Anotacao: Problem here? + void process_statustext(const mavlink_message_t &message); void process_rc_channels(const mavlink_message_t &message); void receive_param_cal_gyro(MAVLinkParameters::Result result, int value); @@ -160,9 +160,8 @@ class TelemetryImpl : public PluginImplBase { std::atomic_bool _in_air{false}; std::atomic_bool _armed{false}; - // Anotacao - mutable std::mutex _mav_message_mutex{}; - Telemetry::MavMessage _mav_message{Telemetry::MavMessage::MessageType::UNKNOWN, ""}; + mutable std::mutex _status_text_mutex{}; + Telemetry::StatusText _status_text{Telemetry::StatusText::StatusType::INFO, ""}; mutable std::mutex _attitude_quaternion_mutex{}; Telemetry::Quaternion _attitude_quaternion{NAN, NAN, NAN, NAN}; @@ -194,7 +193,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::position_callback_t _position_subscription{nullptr}; Telemetry::position_callback_t _home_position_subscription{nullptr}; Telemetry::in_air_callback_t _in_air_subscription{nullptr}; - Telemetry::mav_message_callback_t _mav_message_subscription{nullptr}; + Telemetry::status_text_callback_t _status_text_subscription{nullptr}; Telemetry::armed_callback_t _armed_subscription{nullptr}; Telemetry::attitude_quaternion_callback_t _attitude_quaternion_subscription{nullptr}; Telemetry::attitude_euler_angle_callback_t _attitude_euler_angle_subscription{nullptr}; From 9f7b2b6e5c02838b734440dfcfcde59b246efb54 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Fri, 26 Apr 2019 15:04:48 -0700 Subject: [PATCH 04/25] Run ./fix_style.sh . --- .../telemetry/telemetry_service_impl.h | 38 +++++---- core/mavlink_parameters.cpp | 15 ++-- integration_tests/telemetry_async.cpp | 3 +- plugins/camera/camera_impl.cpp | 80 ++++++++++--------- plugins/follow_me/follow_me_impl.cpp | 61 +++++++------- .../include/plugins/telemetry/telemetry.h | 30 +++---- plugins/telemetry/telemetry.cpp | 3 +- plugins/telemetry/telemetry_impl.cpp | 10 ++- 8 files changed, 123 insertions(+), 117 deletions(-) diff --git a/backend/src/plugins/telemetry/telemetry_service_impl.h b/backend/src/plugins/telemetry/telemetry_service_impl.h index d66228e0be..df06f5872e 100644 --- a/backend/src/plugins/telemetry/telemetry_service_impl.h +++ b/backend/src/plugins/telemetry/telemetry_service_impl.h @@ -96,20 +96,21 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet return grpc::Status::OK; } - grpc::Status - SubscribeStatusText(grpc::ServerContext * /* context */, - const dronecode_sdk::rpc::telemetry::SubscribeStatusTextRequest * /* request */, - grpc::ServerWriter *writer) override + grpc::Status SubscribeStatusText( + grpc::ServerContext * /* context */, + const dronecode_sdk::rpc::telemetry::SubscribeStatusTextRequest * /* request */, + grpc::ServerWriter *writer) override { - _telemetry.status_text_async([this, &writer](dronecode_sdk::Telemetry::StatusText status_text) { - auto rpc_status_text = new dronecode_sdk::rpc::telemetry::StatusText(); - rpc_status_text->set_text(status_text.text); - rpc_status_text->set_type(translateStatusTextType(status_text.type)); - - dronecode_sdk::rpc::telemetry::StatusTextResponse rpc_status_text_response; - rpc_status_text_response.set_allocated_status_text(rpc_status_text); - writer->Write(rpc_status_text_response); - }); + _telemetry.status_text_async( + [this, &writer](dronecode_sdk::Telemetry::StatusText status_text) { + auto rpc_status_text = new dronecode_sdk::rpc::telemetry::StatusText(); + rpc_status_text->set_text(status_text.text); + rpc_status_text->set_type(translateStatusTextType(status_text.type)); + + dronecode_sdk::rpc::telemetry::StatusTextResponse rpc_status_text_response; + rpc_status_text_response.set_allocated_status_text(rpc_status_text); + writer->Write(rpc_status_text_response); + }); _stop_future.wait(); return grpc::Status::OK; @@ -121,11 +122,14 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet switch (type) { default: case dronecode_sdk::Telemetry::StatusText::StatusType::INFO: - return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_INFO; + return dronecode_sdk::rpc::telemetry::StatusText::StatusType:: + StatusText_StatusType_INFO; case dronecode_sdk::Telemetry::StatusText::StatusType::WARNING: - return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_WARNING; + return dronecode_sdk::rpc::telemetry::StatusText::StatusType:: + StatusText_StatusType_WARNING; case dronecode_sdk::Telemetry::StatusText::StatusType::CRITICAL: - return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_CRITICAL; + return dronecode_sdk::rpc::telemetry::StatusText::StatusType:: + StatusText_StatusType_CRITICAL; } } @@ -372,7 +376,7 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet }); _stop_future.wait(); - return grpc::Status::OK; + return grpc::Status::OK; } void stop() { _stop_promise.set_value(); } diff --git a/core/mavlink_parameters.cpp b/core/mavlink_parameters.cpp index 8ff1bb783f..eceed5e4f4 100644 --- a/core/mavlink_parameters.cpp +++ b/core/mavlink_parameters.cpp @@ -114,13 +114,14 @@ MAVLinkParameters::get_param(const std::string &name, ParamValue value_type, boo auto prom = std::promise>(); auto res = prom.get_future(); - get_param_async(name, - value_type, - [&prom](Result result, ParamValue value) { - prom.set_value(std::make_pair<>(result, value)); - }, - this, - extended); + get_param_async( + name, + value_type, + [&prom](Result result, ParamValue value) { + prom.set_value(std::make_pair<>(result, value)); + }, + this, + extended); return res.get(); } diff --git a/integration_tests/telemetry_async.cpp b/integration_tests/telemetry_async.cpp index 971b5a5a7c..74eefec18a 100644 --- a/integration_tests/telemetry_async.cpp +++ b/integration_tests/telemetry_async.cpp @@ -245,8 +245,7 @@ void print_rc_status(Telemetry::RCStatus rc_status) void print_status_text(Telemetry::StatusText status_text) { - std::cout << "Status Text [" << status_text.text << "]" - << std::endl; + std::cout << "Status Text [" << status_text.text << "]" << std::endl; _received_status_text = true; } diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index f36eabe4e5..ab971a8b54 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -1259,25 +1259,26 @@ void CameraImpl::set_option_async(const std::string &setting_id, return; } - _parent->set_param_async(setting_id, - value, - [this, callback, setting_id, value](MAVLinkParameters::Result result) { - if (result == MAVLinkParameters::Result::SUCCESS) { - if (this->_camera_definition) { - _camera_definition->set_setting(setting_id, value); - refresh_params(); - } - if (callback) { - callback(Camera::Result::SUCCESS); - } - } else { - if (callback) { - callback(Camera::Result::ERROR); - } - } - }, - this, - true); + _parent->set_param_async( + setting_id, + value, + [this, callback, setting_id, value](MAVLinkParameters::Result result) { + if (result == MAVLinkParameters::Result::SUCCESS) { + if (this->_camera_definition) { + _camera_definition->set_setting(setting_id, value); + refresh_params(); + } + if (callback) { + callback(Camera::Result::SUCCESS); + } + } else { + if (callback) { + callback(Camera::Result::ERROR); + } + } + }, + this, + true); } Camera::Result CameraImpl::get_option(const std::string &setting_id, Camera::Option &option) @@ -1457,26 +1458,27 @@ void CameraImpl::refresh_params() const std::string ¶m_name = param.first; const MAVLinkParameters::ParamValue ¶m_value_type = param.second; const bool is_last = (count + 1 == params.size()); - _parent->get_param_async(param_name, - param_value_type, - [param_name, is_last, this](MAVLinkParameters::Result result, - MAVLinkParameters::ParamValue value) { - if (result != MAVLinkParameters::Result::SUCCESS) { - return; - } - // We need to check again by the time this callback runs - if (!this->_camera_definition) { - return; - } - this->_camera_definition->set_setting(param_name, value); - - if (is_last) { - notify_current_settings(); - notify_possible_setting_options(); - } - }, - this, - true); + _parent->get_param_async( + param_name, + param_value_type, + [param_name, is_last, this](MAVLinkParameters::Result result, + MAVLinkParameters::ParamValue value) { + if (result != MAVLinkParameters::Result::SUCCESS) { + return; + } + // We need to check again by the time this callback runs + if (!this->_camera_definition) { + return; + } + this->_camera_definition->set_setting(param_name, value); + + if (is_last) { + notify_current_settings(); + notify_possible_setting_options(); + } + }, + this, + true); ++count; } } diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp index 39418f40e7..7c1eccae6e 100644 --- a/plugins/follow_me/follow_me_impl.cpp +++ b/plugins/follow_me/follow_me_impl.cpp @@ -34,35 +34,38 @@ void FollowMeImpl::deinit() void FollowMeImpl::enable() { - _parent->get_param_float_async("NAV_MIN_FT_HT", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.min_height_m = value; - } - }, - this); - _parent->get_param_float_async("NAV_FT_DST", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.follow_distance_m = value; - } - }, - this); - _parent->get_param_int_async("NAV_FT_FS", - [this](MAVLinkParameters::Result result, int32_t value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.follow_direction = - static_cast(value); - } - }, - this); - _parent->get_param_float_async("NAV_FT_RS", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.responsiveness = value; - } - }, - this); + _parent->get_param_float_async( + "NAV_MIN_FT_HT", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.min_height_m = value; + } + }, + this); + _parent->get_param_float_async( + "NAV_FT_DST", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.follow_distance_m = value; + } + }, + this); + _parent->get_param_int_async( + "NAV_FT_FS", + [this](MAVLinkParameters::Result result, int32_t value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.follow_direction = static_cast(value); + } + }, + this); + _parent->get_param_float_async( + "NAV_FT_RS", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.responsiveness = value; + } + }, + this); } void FollowMeImpl::disable() diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 94ae8ffcbe..75be676b35 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -130,11 +130,7 @@ class Telemetry : public PluginBase { }; struct StatusText { - enum class StatusType { - INFO, - WARNING, - CRITICAL - } type; + enum class StatusType { INFO, WARNING, CRITICAL } type; std::string text; }; @@ -257,10 +253,10 @@ class Telemetry : public PluginBase { /** * @brief Set rate of status text updates (synchronous). - * + * * @param rate_hz Rate in Hz. * @return Result of request. - */ + */ Result set_rate_status_text(double rate_hz); /** @@ -346,10 +342,10 @@ class Telemetry : public PluginBase { /** * @brief Set rate of status text updates (asynchronous). - * + * * @param rate_hz Rate in Hz. * @param callback Callback to receive request result. - */ + */ void set_rate_status_text_async(double rate_hz, result_callback_t callback); /** @@ -423,9 +419,9 @@ class Telemetry : public PluginBase { /** * @brief Get status text (synchronous). - * + * * @return Status text. - */ + */ StatusText status_text() const; /** @@ -561,10 +557,10 @@ class Telemetry : public PluginBase { /** * @brief Callback for mavlink status text updates. - * + * * @param status text with message type and text. */ - typedef std::function status_text_callback_t; + typedef std::function status_text_callback_t; /** * @brief Subscribe to in-air updates (asynchronous). @@ -575,9 +571,9 @@ class Telemetry : public PluginBase { /** * @brief Subscribe to status text updates (asynchronous). - * + * * @param callback Function to call with updates. - */ + */ void status_text_async(status_text_callback_t callback); /** @@ -901,9 +897,9 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status /** * @brief Stream operator to print information about a `Telemetry::StatusText`. - * + * * @returns A reference to the stream. -*/ + */ std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text); } // namespace dronecode_sdk diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index d7608091c3..62e04fc309 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -494,8 +494,7 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status bool operator==(const Telemetry::StatusText &lhs, const Telemetry::StatusText &rhs) { - return lhs.text == rhs.text && - lhs.type == rhs.type; + return lhs.text == rhs.text && lhs.type == rhs.type; } std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text) diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index e88f69b275..b04499ab5f 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -158,8 +158,9 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz) Telemetry::Result TelemetryImpl::set_rate_status_text(double rate_hz) { - return telemetry_result_from_command_result( - _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? + return telemetry_result_from_command_result(_parent->set_msg_rate( + MAVLINK_MSG_ID_EXTENDED_SYS_STATE, + rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? } Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz) @@ -238,7 +239,8 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::result_call std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } -void TelemetryImpl::set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback) +void TelemetryImpl::set_rate_status_text_async(double rate_hz, + Telemetry::result_callback_t callback) { _parent->set_msg_rate_async( MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: What rate to set here? @@ -516,7 +518,7 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) switch (statustext.severity) { case MAV_SEVERITY_WARNING: - type = Telemetry::StatusText::StatusType::WARNING; + type = Telemetry::StatusText::StatusType::WARNING; break; case MAV_SEVERITY_CRITICAL: type = Telemetry::StatusText::StatusType::CRITICAL; From b49eb7d0c5d81786859ce889c99bf39052c98566 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Mon, 29 Apr 2019 15:47:30 -0700 Subject: [PATCH 05/25] Some changes requested in PR regarding Removing status text rate. --- integration_tests/telemetry_async.cpp | 19 ++++++----- .../include/plugins/telemetry/telemetry.h | 4 +-- plugins/telemetry/telemetry.cpp | 10 ------ plugins/telemetry/telemetry_impl.cpp | 34 +++++++++++-------- plugins/telemetry/telemetry_impl.h | 4 +-- 5 files changed, 33 insertions(+), 38 deletions(-) diff --git a/integration_tests/telemetry_async.cpp b/integration_tests/telemetry_async.cpp index 74eefec18a..1407191338 100644 --- a/integration_tests/telemetry_async.cpp +++ b/integration_tests/telemetry_async.cpp @@ -23,7 +23,7 @@ static void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned); static void print_gps_info(Telemetry::GPSInfo gps_info); static void print_battery(Telemetry::Battery battery); static void print_rc_status(Telemetry::RCStatus rc_status); -static void print_status_text(Telemetry::StatusText status_text); +// static void print_status_text(Telemetry::StatusText status_text); // Anotacao static void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned); static bool _set_rate_error = false; @@ -41,7 +41,7 @@ static bool _received_ground_speed = false; static bool _received_gps_info = false; static bool _received_battery = false; static bool _received_rc_status = false; -static bool _received_status_text = false; +// static bool _received_status_text = false; // Anotacao static bool _received_position_velocity_ned = false; TEST_F(SitlTest, TelemetryAsync) @@ -115,7 +115,7 @@ TEST_F(SitlTest, TelemetryAsync) telemetry->rc_status_async(std::bind(&print_rc_status, _1)); - telemetry->status_text_async(std::bind(&print_status_text, _1)); + // telemetry->status_text_async(std::bind(&print_status_text, _1)); // Anotacao telemetry->position_velocity_ned_async(std::bind(&print_position_velocity_ned, _1)); @@ -136,7 +136,7 @@ TEST_F(SitlTest, TelemetryAsync) EXPECT_TRUE(_received_gps_info); EXPECT_TRUE(_received_battery); EXPECT_TRUE(_received_rc_status); - EXPECT_TRUE(_received_status_text); + // EXPECT_TRUE(_received_status_text); // Anotacao EXPECT_TRUE(_received_position_velocity_ned); } @@ -243,11 +243,12 @@ void print_rc_status(Telemetry::RCStatus rc_status) _received_rc_status = true; } -void print_status_text(Telemetry::StatusText status_text) -{ - std::cout << "Status Text [" << status_text.text << "]" << std::endl; - _received_status_text = true; -} +// Anotacao +// void print_status_text(Telemetry::StatusText status_text) +// { +// std::cout << "Status Text [" << status_text.text << "]" << std::endl; +// _received_status_text = true; +// } void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned) { diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 75be676b35..e7ec794942 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -257,7 +257,7 @@ class Telemetry : public PluginBase { * @param rate_hz Rate in Hz. * @return Result of request. */ - Result set_rate_status_text(double rate_hz); + // Result set_rate_status_text(double rate_hz); // Anotacao /** * @brief Set rate of attitude updates (synchronous). @@ -346,7 +346,7 @@ class Telemetry : public PluginBase { * @param rate_hz Rate in Hz. * @param callback Callback to receive request result. */ - void set_rate_status_text_async(double rate_hz, result_callback_t callback); + // void set_rate_status_text_async(double rate_hz, result_callback_t callback); // Anotacao /** * @brief Set rate of attitude updates (asynchronous). diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index 62e04fc309..f738cb6516 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -31,11 +31,6 @@ Telemetry::Result Telemetry::set_rate_in_air(double rate_hz) return _impl->set_rate_in_air(rate_hz); } -Telemetry::Result Telemetry::set_rate_status_text(double rate_hz) -{ - return _impl->set_rate_status_text(rate_hz); -} - Telemetry::Result Telemetry::set_rate_attitude(double rate_hz) { return _impl->set_rate_attitude(rate_hz); @@ -86,11 +81,6 @@ void Telemetry::set_rate_in_air_async(double rate_hz, result_callback_t callback _impl->set_rate_in_air_async(rate_hz, callback); } -void Telemetry::set_rate_status_text_async(double rate_hz, result_callback_t callback) -{ - _impl->set_rate_status_text_async(rate_hz, callback); -} - void Telemetry::set_rate_attitude_async(double rate_hz, result_callback_t callback) { _impl->set_rate_attitude_async(rate_hz, callback); diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index b04499ab5f..53e9d0e159 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -156,12 +156,12 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz) _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); } -Telemetry::Result TelemetryImpl::set_rate_status_text(double rate_hz) -{ - return telemetry_result_from_command_result(_parent->set_msg_rate( - MAVLINK_MSG_ID_EXTENDED_SYS_STATE, - rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? -} +// Telemetry::Result TelemetryImpl::set_rate_status_text(double rate_hz) +// { +// return telemetry_result_from_command_result(_parent->set_msg_rate( +// MAVLINK_MSG_ID_EXTENDED_SYS_STATE, +// rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? +// } Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz) { @@ -239,14 +239,14 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::result_call std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } -void TelemetryImpl::set_rate_status_text_async(double rate_hz, - Telemetry::result_callback_t callback) -{ - _parent->set_msg_rate_async( - MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: What rate to set here? - rate_hz, - std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); -} +// void TelemetryImpl::set_rate_status_text_async(double rate_hz, +// Telemetry::result_callback_t callback) +// { +// _parent->set_msg_rate_async( +// MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: What rate to set here? +// rate_hz, +// std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); +// } void TelemetryImpl::set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback) { @@ -523,7 +523,11 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) case MAV_SEVERITY_CRITICAL: type = Telemetry::StatusText::StatusType::CRITICAL; break; + case MAV_SEVERITY_INFO: + type = Telemetry::StatusText::StatusType::INFO; + break; default: + LogWarn() << "Unknown StatusText severity"; type = Telemetry::StatusText::StatusType::INFO; break; } @@ -533,7 +537,7 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) char text_with_null[sizeof(statustext.text) + 1]{}; memcpy(text_with_null, statustext.text, sizeof(statustext.text)); - std::string text = text_with_null; + const std::string text = text_with_null; set_status_text({type, text}); diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index 56a11f2383..0e5d6df966 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -30,7 +30,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Result set_rate_position(double rate_hz); Telemetry::Result set_rate_home_position(double rate_hz); Telemetry::Result set_rate_in_air(double rate_hz); - Telemetry::Result set_rate_status_text(double rate_hz); + // Telemetry::Result set_rate_status_text(double rate_hz); Anotacao Telemetry::Result set_rate_attitude(double rate_hz); Telemetry::Result set_rate_camera_attitude(double rate_hz); Telemetry::Result set_rate_ground_speed_ned(double rate_hz); @@ -43,7 +43,7 @@ class TelemetryImpl : public PluginImplBase { void set_rate_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_home_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_in_air_async(double rate_hz, Telemetry::result_callback_t callback); - void set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback); + // void set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback); Anotacao void set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_camera_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_ground_speed_ned_async(double rate_hz, Telemetry::result_callback_t callback); From 4a7f87b9546c1b15102e0167c4efafccfcc14f50 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Tue, 30 Apr 2019 10:17:57 -0700 Subject: [PATCH 06/25] Update branch. --- plugins/telemetry/telemetry_impl.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index 0e5d6df966..0b42ecb196 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -30,7 +30,6 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Result set_rate_position(double rate_hz); Telemetry::Result set_rate_home_position(double rate_hz); Telemetry::Result set_rate_in_air(double rate_hz); - // Telemetry::Result set_rate_status_text(double rate_hz); Anotacao Telemetry::Result set_rate_attitude(double rate_hz); Telemetry::Result set_rate_camera_attitude(double rate_hz); Telemetry::Result set_rate_ground_speed_ned(double rate_hz); @@ -43,7 +42,6 @@ class TelemetryImpl : public PluginImplBase { void set_rate_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_home_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_in_air_async(double rate_hz, Telemetry::result_callback_t callback); - // void set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback); Anotacao void set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_camera_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_ground_speed_ned_async(double rate_hz, Telemetry::result_callback_t callback); From 703e6e8dcd544724ef2ba7d9d2aec91cf641c07c Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Tue, 30 Apr 2019 11:58:11 -0700 Subject: [PATCH 07/25] Remove comments. --- integration_tests/telemetry_async.cpp | 12 ------------ .../include/plugins/telemetry/telemetry.h | 16 ---------------- plugins/telemetry/telemetry_impl.cpp | 16 ---------------- plugins/telemetry/telemetry_impl.h | 1 - 4 files changed, 45 deletions(-) diff --git a/integration_tests/telemetry_async.cpp b/integration_tests/telemetry_async.cpp index 1407191338..93b7021519 100644 --- a/integration_tests/telemetry_async.cpp +++ b/integration_tests/telemetry_async.cpp @@ -23,7 +23,6 @@ static void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned); static void print_gps_info(Telemetry::GPSInfo gps_info); static void print_battery(Telemetry::Battery battery); static void print_rc_status(Telemetry::RCStatus rc_status); -// static void print_status_text(Telemetry::StatusText status_text); // Anotacao static void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned); static bool _set_rate_error = false; @@ -41,7 +40,6 @@ static bool _received_ground_speed = false; static bool _received_gps_info = false; static bool _received_battery = false; static bool _received_rc_status = false; -// static bool _received_status_text = false; // Anotacao static bool _received_position_velocity_ned = false; TEST_F(SitlTest, TelemetryAsync) @@ -115,8 +113,6 @@ TEST_F(SitlTest, TelemetryAsync) telemetry->rc_status_async(std::bind(&print_rc_status, _1)); - // telemetry->status_text_async(std::bind(&print_status_text, _1)); // Anotacao - telemetry->position_velocity_ned_async(std::bind(&print_position_velocity_ned, _1)); std::this_thread::sleep_for(std::chrono::seconds(10)); @@ -136,7 +132,6 @@ TEST_F(SitlTest, TelemetryAsync) EXPECT_TRUE(_received_gps_info); EXPECT_TRUE(_received_battery); EXPECT_TRUE(_received_rc_status); - // EXPECT_TRUE(_received_status_text); // Anotacao EXPECT_TRUE(_received_position_velocity_ned); } @@ -243,13 +238,6 @@ void print_rc_status(Telemetry::RCStatus rc_status) _received_rc_status = true; } -// Anotacao -// void print_status_text(Telemetry::StatusText status_text) -// { -// std::cout << "Status Text [" << status_text.text << "]" << std::endl; -// _received_status_text = true; -// } - void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned) { std::cout << "Got position north: " << position_velocity_ned.position.north_m << " m, " diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index e7ec794942..f5656eb6a0 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -251,14 +251,6 @@ class Telemetry : public PluginBase { */ Result set_rate_in_air(double rate_hz); - /** - * @brief Set rate of status text updates (synchronous). - * - * @param rate_hz Rate in Hz. - * @return Result of request. - */ - // Result set_rate_status_text(double rate_hz); // Anotacao - /** * @brief Set rate of attitude updates (synchronous). * @@ -340,14 +332,6 @@ class Telemetry : public PluginBase { */ void set_rate_in_air_async(double rate_hz, result_callback_t callback); - /** - * @brief Set rate of status text updates (asynchronous). - * - * @param rate_hz Rate in Hz. - * @param callback Callback to receive request result. - */ - // void set_rate_status_text_async(double rate_hz, result_callback_t callback); // Anotacao - /** * @brief Set rate of attitude updates (asynchronous). * diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index 53e9d0e159..c30500e4d9 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -156,13 +156,6 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz) _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); } -// Telemetry::Result TelemetryImpl::set_rate_status_text(double rate_hz) -// { -// return telemetry_result_from_command_result(_parent->set_msg_rate( -// MAVLINK_MSG_ID_EXTENDED_SYS_STATE, -// rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? -// } - Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz) { return telemetry_result_from_command_result( @@ -239,15 +232,6 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::result_call std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } -// void TelemetryImpl::set_rate_status_text_async(double rate_hz, -// Telemetry::result_callback_t callback) -// { -// _parent->set_msg_rate_async( -// MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: What rate to set here? -// rate_hz, -// std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); -// } - void TelemetryImpl::set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback) { _parent->set_msg_rate_async( diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index 0b42ecb196..288631807d 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -212,5 +212,4 @@ class TelemetryImpl : public PluginImplBase { void *_timeout_cookie{nullptr}; }; - } // namespace dronecode_sdk From e0984ab5ad486b8e42d7bffac13eaa264d66fcc8 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Thu, 2 May 2019 10:23:50 -0700 Subject: [PATCH 08/25] Include docstring. --- .../include/plugins/telemetry/telemetry.h | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index f5656eb6a0..0a0f0a800d 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -129,9 +129,23 @@ class Telemetry : public PluginBase { 5: RTK float, 6: RTK fixed). */ }; + /** + * @brief Status Text information type. + */ struct StatusText { - enum class StatusType { INFO, WARNING, CRITICAL } type; - std::string text; + /** + * @brief Status Types. + * + * @note PX4 only supports these 3 status types. + * If other status types are returned for some reason, + * they will be marked as INFO type and logged as a warning. + */ + enum class StatusType { + INFO, + WARNING, + CRITICAL + } type; + std::string text; /**< @brief Mavlink status message. */ }; /** From 03e1a099af6fc3aa166fd3f62864151cf31947c5 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Thu, 2 May 2019 10:31:23 -0700 Subject: [PATCH 09/25] Add docstrings for SatusType enum. --- plugins/telemetry/include/plugins/telemetry/telemetry.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 0a0f0a800d..4485cbdea9 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -141,9 +141,9 @@ class Telemetry : public PluginBase { * they will be marked as INFO type and logged as a warning. */ enum class StatusType { - INFO, - WARNING, - CRITICAL + INFO, /**< @brief Message type is an information or other. */ + WARNING, /**< @brief Message type is a warning. */ + CRITICAL /**< @brief Message type is critical. */ } type; std::string text; /**< @brief Mavlink status message. */ }; From 7c09c149cb9a4f49d45ada9e71810bc8567f2fd4 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Thu, 25 Apr 2019 12:52:51 -0700 Subject: [PATCH 10/25] Telemetry: implemented StatusText. --- .../telemetry/telemetry_service_impl.h | 37 ++++++++++++ .../include/plugins/telemetry/telemetry.h | 47 +++++++++++++++ plugins/telemetry/mocks/telemetry_mock.h | 1 + plugins/telemetry/telemetry.cpp | 20 +++++++ plugins/telemetry/telemetry_impl.cpp | 58 +++++++++++++++++++ plugins/telemetry/telemetry_impl.h | 8 +++ 6 files changed, 171 insertions(+) diff --git a/backend/src/plugins/telemetry/telemetry_service_impl.h b/backend/src/plugins/telemetry/telemetry_service_impl.h index 87861619aa..df06f5872e 100644 --- a/backend/src/plugins/telemetry/telemetry_service_impl.h +++ b/backend/src/plugins/telemetry/telemetry_service_impl.h @@ -96,6 +96,43 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet return grpc::Status::OK; } + grpc::Status SubscribeStatusText( + grpc::ServerContext * /* context */, + const dronecode_sdk::rpc::telemetry::SubscribeStatusTextRequest * /* request */, + grpc::ServerWriter *writer) override + { + _telemetry.status_text_async( + [this, &writer](dronecode_sdk::Telemetry::StatusText status_text) { + auto rpc_status_text = new dronecode_sdk::rpc::telemetry::StatusText(); + rpc_status_text->set_text(status_text.text); + rpc_status_text->set_type(translateStatusTextType(status_text.type)); + + dronecode_sdk::rpc::telemetry::StatusTextResponse rpc_status_text_response; + rpc_status_text_response.set_allocated_status_text(rpc_status_text); + writer->Write(rpc_status_text_response); + }); + + _stop_future.wait(); + return grpc::Status::OK; + } + + dronecode_sdk::rpc::telemetry::StatusText::StatusType + translateStatusTextType(const dronecode_sdk::Telemetry::StatusText::StatusType type) const + { + switch (type) { + default: + case dronecode_sdk::Telemetry::StatusText::StatusType::INFO: + return dronecode_sdk::rpc::telemetry::StatusText::StatusType:: + StatusText_StatusType_INFO; + case dronecode_sdk::Telemetry::StatusText::StatusType::WARNING: + return dronecode_sdk::rpc::telemetry::StatusText::StatusType:: + StatusText_StatusType_WARNING; + case dronecode_sdk::Telemetry::StatusText::StatusType::CRITICAL: + return dronecode_sdk::rpc::telemetry::StatusText::StatusType:: + StatusText_StatusType_CRITICAL; + } + } + grpc::Status SubscribeArmed(grpc::ServerContext * /* context */, const dronecode_sdk::rpc::telemetry::SubscribeArmedRequest * /* request */, diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 3e5c75c251..6fcfa2ccbd 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -129,6 +129,25 @@ class Telemetry : public PluginBase { 5: RTK float, 6: RTK fixed). */ }; + /** + * @brief Status Text information type. + */ + struct StatusText { + /** + * @brief Status Types. + * + * @note PX4 only supports these 3 status types. + * If other status types are returned for some reason, + * they will be marked as INFO type and logged as a warning. + */ + enum class StatusType { + INFO, /**< @brief Message type is an information or other. */ + WARNING, /**< @brief Message type is a warning. */ + CRITICAL /**< @brief Message type is critical. */ + } type; + std::string text; /**< @brief Mavlink status message. */ + }; + /** * @brief Battery type. */ @@ -396,6 +415,13 @@ class Telemetry : public PluginBase { */ Position home_position() const; + /** + * @brief Get status text (synchronous). + * + * @return Status text. + */ + StatusText status_text() const; + /** * @brief Get the in-air status (synchronous). * @@ -527,6 +553,13 @@ class Telemetry : public PluginBase { */ typedef std::function in_air_callback_t; + /** + * @brief Callback for mavlink status text updates. + * + * @param status text with message type and text. + */ + typedef std::function status_text_callback_t; + /** * @brief Subscribe to in-air updates (asynchronous). * @@ -534,6 +567,13 @@ class Telemetry : public PluginBase { */ void in_air_async(in_air_callback_t callback); + /** + * @brief Subscribe to status text updates (asynchronous). + * + * @param callback Function to call with updates. + */ + void status_text_async(status_text_callback_t callback); + /** * @brief Callback type for armed updates (asynchronous). * @@ -853,4 +893,11 @@ bool operator==(const Telemetry::RCStatus &lhs, const Telemetry::RCStatus &rhs); */ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status); +/** + * @brief Stream operator to print information about a `Telemetry::StatusText`. + * + * @returns A reference to the stream. + */ +std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text); + } // namespace dronecode_sdk diff --git a/plugins/telemetry/mocks/telemetry_mock.h b/plugins/telemetry/mocks/telemetry_mock.h index dd5409ee4b..5f57a6b7a7 100644 --- a/plugins/telemetry/mocks/telemetry_mock.h +++ b/plugins/telemetry/mocks/telemetry_mock.h @@ -11,6 +11,7 @@ class MockTelemetry { MOCK_CONST_METHOD1(health_async, void(Telemetry::health_callback_t)){}; MOCK_CONST_METHOD1(home_position_async, void(Telemetry::position_callback_t)){}; MOCK_CONST_METHOD1(in_air_async, void(Telemetry::in_air_callback_t)){}; + MOCK_CONST_METHOD1(status_text_async, void(Telemetry::status_text_callback_t)){}; MOCK_CONST_METHOD1(armed_async, void(Telemetry::armed_callback_t)){}; MOCK_CONST_METHOD1(gps_info_async, void(Telemetry::gps_info_callback_t)){}; MOCK_CONST_METHOD1(battery_async, void(Telemetry::battery_callback_t)){}; diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index b4c705238c..f738cb6516 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -131,6 +131,11 @@ bool Telemetry::in_air() const return _impl->in_air(); } +Telemetry::StatusText Telemetry::status_text() const +{ + return _impl->get_status_text(); +} + bool Telemetry::armed() const { return _impl->armed(); @@ -211,6 +216,11 @@ void Telemetry::in_air_async(in_air_callback_t callback) return _impl->in_air_async(callback); } +void Telemetry::status_text_async(status_text_callback_t callback) +{ + return _impl->status_text_async(callback); +} + void Telemetry::armed_async(armed_callback_t callback) { return _impl->armed_async(callback); @@ -472,4 +482,14 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status << ", signal_strength_percent: " << rc_status.signal_strength_percent << "]"; } +bool operator==(const Telemetry::StatusText &lhs, const Telemetry::StatusText &rhs) +{ + return lhs.text == rhs.text && lhs.type == rhs.type; +} + +std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text) +{ + return str << "[statustext: " << status_text.text << "]"; +} + } // namespace dronecode_sdk diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index 85f7966c5d..c30500e4d9 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -5,6 +5,7 @@ #include "px4_custom_mode.h" #include #include +#include namespace dronecode_sdk { @@ -61,6 +62,9 @@ void TelemetryImpl::init() _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_HEARTBEAT, std::bind(&TelemetryImpl::process_heartbeat, this, _1), this); + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_STATUSTEXT, std::bind(&TelemetryImpl::process_statustext, this, _1), this); + _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_RC_CHANNELS, std::bind(&TelemetryImpl::process_rc_channels, this, _1), this); @@ -489,6 +493,43 @@ void TelemetryImpl::process_heartbeat(const mavlink_message_t &message) } } +void TelemetryImpl::process_statustext(const mavlink_message_t &message) +{ + mavlink_statustext_t statustext; + mavlink_msg_statustext_decode(&message, &statustext); + + Telemetry::StatusText::StatusType type; + + switch (statustext.severity) { + case MAV_SEVERITY_WARNING: + type = Telemetry::StatusText::StatusType::WARNING; + break; + case MAV_SEVERITY_CRITICAL: + type = Telemetry::StatusText::StatusType::CRITICAL; + break; + case MAV_SEVERITY_INFO: + type = Telemetry::StatusText::StatusType::INFO; + break; + default: + LogWarn() << "Unknown StatusText severity"; + type = Telemetry::StatusText::StatusType::INFO; + break; + } + + // statustext.text is not null terminated, therefore we copy it first to + // an array big enough that is zeroed. + char text_with_null[sizeof(statustext.text) + 1]{}; + memcpy(text_with_null, statustext.text, sizeof(statustext.text)); + + const std::string text = text_with_null; + + set_status_text({type, text}); + + if (_status_text_subscription) { + _status_text_subscription(get_status_text()); + } +} + void TelemetryImpl::process_rc_channels(const mavlink_message_t &message) { mavlink_rc_channels_t rc_channels; @@ -659,6 +700,18 @@ void TelemetryImpl::set_in_air(bool in_air_new) _in_air = in_air_new; } +void TelemetryImpl::set_status_text(Telemetry::StatusText status_text) +{ + std::lock_guard lock(_status_text_mutex); + _status_text = status_text; +} + +Telemetry::StatusText TelemetryImpl::get_status_text() const +{ + std::lock_guard lock(_status_text_mutex); + return _status_text; +} + void TelemetryImpl::set_armed(bool armed_new) { _armed = armed_new; @@ -854,6 +907,11 @@ void TelemetryImpl::in_air_async(Telemetry::in_air_callback_t &callback) _in_air_subscription = callback; } +void TelemetryImpl::status_text_async(Telemetry::status_text_callback_t &callback) +{ + _status_text_subscription = callback; +} + void TelemetryImpl::armed_async(Telemetry::armed_callback_t &callback) { _armed_subscription = callback; diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index f110f8bd19..0b42ecb196 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -54,6 +54,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Position get_home_position() const; bool in_air() const; bool armed() const; + Telemetry::StatusText get_status_text() const; Telemetry::EulerAngle get_attitude_euler_angle() const; Telemetry::Quaternion get_attitude_quaternion() const; Telemetry::EulerAngle get_camera_attitude_euler_angle() const; @@ -70,6 +71,7 @@ class TelemetryImpl : public PluginImplBase { void position_async(Telemetry::position_callback_t &callback); void home_position_async(Telemetry::position_callback_t &callback); void in_air_async(Telemetry::in_air_callback_t &callback); + void status_text_async(Telemetry::status_text_callback_t &callback); void armed_async(Telemetry::armed_callback_t &callback); void attitude_quaternion_async(Telemetry::attitude_quaternion_callback_t &callback); void attitude_euler_angle_async(Telemetry::attitude_euler_angle_callback_t &callback); @@ -91,6 +93,7 @@ class TelemetryImpl : public PluginImplBase { void set_position(Telemetry::Position position); void set_home_position(Telemetry::Position home_position); void set_in_air(bool in_air); + void set_status_text(Telemetry::StatusText status_text); void set_armed(bool armed); void set_attitude_quaternion(Telemetry::Quaternion quaternion); void set_camera_attitude_euler_angle(Telemetry::EulerAngle euler_angle); @@ -116,6 +119,7 @@ class TelemetryImpl : public PluginImplBase { void process_extended_sys_state(const mavlink_message_t &message); void process_sys_status(const mavlink_message_t &message); void process_heartbeat(const mavlink_message_t &message); + void process_statustext(const mavlink_message_t &message); void process_rc_channels(const mavlink_message_t &message); void receive_param_cal_gyro(MAVLinkParameters::Result result, int value); @@ -154,6 +158,9 @@ class TelemetryImpl : public PluginImplBase { std::atomic_bool _in_air{false}; std::atomic_bool _armed{false}; + mutable std::mutex _status_text_mutex{}; + Telemetry::StatusText _status_text{Telemetry::StatusText::StatusType::INFO, ""}; + mutable std::mutex _attitude_quaternion_mutex{}; Telemetry::Quaternion _attitude_quaternion{NAN, NAN, NAN, NAN}; @@ -184,6 +191,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::position_callback_t _position_subscription{nullptr}; Telemetry::position_callback_t _home_position_subscription{nullptr}; Telemetry::in_air_callback_t _in_air_subscription{nullptr}; + Telemetry::status_text_callback_t _status_text_subscription{nullptr}; Telemetry::armed_callback_t _armed_subscription{nullptr}; Telemetry::attitude_quaternion_callback_t _attitude_quaternion_subscription{nullptr}; Telemetry::attitude_euler_angle_callback_t _attitude_euler_angle_subscription{nullptr}; From 43f988db9024a42fa1e8027a7046f561fe5dc0f0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 3 May 2019 08:30:12 +0200 Subject: [PATCH 11/25] backend/proto: update submodule This includes the addition for StatusText. --- backend/proto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/backend/proto b/backend/proto index d65288df81..dccc60484a 160000 --- a/backend/proto +++ b/backend/proto @@ -1 +1 @@ -Subproject commit d65288df81b68f270223db14d977ee813b317843 +Subproject commit dccc60484a6c94ee1c11799c013c78c5da315741 From e32c6e07aa9cbadb67899b8c92151fb2a1d4ca49 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Thu, 25 Apr 2019 12:52:51 -0700 Subject: [PATCH 12/25] Implement MavMessage working. String only. --- .../telemetry/telemetry_service_impl.h | 20 ++++- integration_tests/telemetry_async.cpp | 12 +++ .../include/plugins/telemetry/telemetry.h | 16 ++++ plugins/telemetry/mocks/telemetry_mock.h | 1 + plugins/telemetry/telemetry.cpp | 30 +++++++ plugins/telemetry/telemetry_impl.cpp | 85 +++++++++++++++++++ plugins/telemetry/telemetry_impl.h | 11 +++ 7 files changed, 174 insertions(+), 1 deletion(-) diff --git a/backend/src/plugins/telemetry/telemetry_service_impl.h b/backend/src/plugins/telemetry/telemetry_service_impl.h index 87861619aa..02c07ee32b 100644 --- a/backend/src/plugins/telemetry/telemetry_service_impl.h +++ b/backend/src/plugins/telemetry/telemetry_service_impl.h @@ -96,6 +96,24 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet return grpc::Status::OK; } + grpc::Status + SubscribeMavMessage(grpc::ServerContext * /* context */, + const dronecode_sdk::rpc::telemetry::SubscribeMavMessageRequest * /* request */, + grpc::ServerWriter *writer) override + { + _telemetry.mav_message_async([&writer](dronecode_sdk::Telemetry::MavMessage mav_message) { + auto rpc_mav_message = new dronecode_sdk::rpc::telemetry::MavMessage(); + rpc_mav_message->set_message_str(mav_message.message_str); + + dronecode_sdk::rpc::telemetry::MavMessageResponse rpc_mav_message_response; + rpc_mav_message_response.set_allocated_mav_message(rpc_mav_message); + writer->Write(rpc_mav_message_response); + }); + + _stop_future.wait(); + return grpc::Status::OK; + } + grpc::Status SubscribeArmed(grpc::ServerContext * /* context */, const dronecode_sdk::rpc::telemetry::SubscribeArmedRequest * /* request */, @@ -339,7 +357,7 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet }); _stop_future.wait(); - return grpc::Status::OK; + return grpc::Status::OK; } void stop() { _stop_promise.set_value(); } diff --git a/integration_tests/telemetry_async.cpp b/integration_tests/telemetry_async.cpp index 93b7021519..aebbb776b3 100644 --- a/integration_tests/telemetry_async.cpp +++ b/integration_tests/telemetry_async.cpp @@ -23,6 +23,7 @@ static void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned); static void print_gps_info(Telemetry::GPSInfo gps_info); static void print_battery(Telemetry::Battery battery); static void print_rc_status(Telemetry::RCStatus rc_status); +static void print_mav_message(Telemetry::MavMessage mav_message); static void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned); static bool _set_rate_error = false; @@ -40,6 +41,7 @@ static bool _received_ground_speed = false; static bool _received_gps_info = false; static bool _received_battery = false; static bool _received_rc_status = false; +static bool _received_mav_message = false; static bool _received_position_velocity_ned = false; TEST_F(SitlTest, TelemetryAsync) @@ -113,6 +115,8 @@ TEST_F(SitlTest, TelemetryAsync) telemetry->rc_status_async(std::bind(&print_rc_status, _1)); + telemetry->mav_message_async(std::bind(&print_mav_message, _1)); + telemetry->position_velocity_ned_async(std::bind(&print_position_velocity_ned, _1)); std::this_thread::sleep_for(std::chrono::seconds(10)); @@ -132,6 +136,7 @@ TEST_F(SitlTest, TelemetryAsync) EXPECT_TRUE(_received_gps_info); EXPECT_TRUE(_received_battery); EXPECT_TRUE(_received_rc_status); + EXPECT_TRUE(_received_mav_message); EXPECT_TRUE(_received_position_velocity_ned); } @@ -238,6 +243,13 @@ void print_rc_status(Telemetry::RCStatus rc_status) _received_rc_status = true; } +void print_mav_message(Telemetry::MavMessage mav_message) +{ + std::cout << "Mav Message [" << mav_message.message_str << "]" + << std::endl; + _received_mav_message = true; +} + void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned) { std::cout << "Got position north: " << position_velocity_ned.position.north_m << " m, " diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 3e5c75c251..3b1219d70e 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -129,6 +129,10 @@ class Telemetry : public PluginBase { 5: RTK float, 6: RTK fixed). */ }; + struct MavMessage { + std::string message_str; + }; + /** * @brief Battery type. */ @@ -246,6 +250,8 @@ class Telemetry : public PluginBase { */ Result set_rate_in_air(double rate_hz); + Result set_rate_mav_message(double rate_hz); + /** * @brief Set rate of attitude updates (synchronous). * @@ -327,6 +333,8 @@ class Telemetry : public PluginBase { */ void set_rate_in_air_async(double rate_hz, result_callback_t callback); + void set_rate_mav_message_async(double rate_hz, result_callback_t callback); + /** * @brief Set rate of attitude updates (asynchronous). * @@ -403,6 +411,8 @@ class Telemetry : public PluginBase { */ bool in_air() const; + MavMessage mav_message() const; + /** * @brief Get the arming status (synchronous). * @@ -527,6 +537,8 @@ class Telemetry : public PluginBase { */ typedef std::function in_air_callback_t; + typedef std::function mav_message_callback_t; // Anotacao: Remove mav_message from here? + /** * @brief Subscribe to in-air updates (asynchronous). * @@ -534,6 +546,8 @@ class Telemetry : public PluginBase { */ void in_air_async(in_air_callback_t callback); + void mav_message_async(mav_message_callback_t callback); + /** * @brief Callback type for armed updates (asynchronous). * @@ -853,4 +867,6 @@ bool operator==(const Telemetry::RCStatus &lhs, const Telemetry::RCStatus &rhs); */ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status); +std::ostream &operator<<(std::ostream &str, Telemetry::MavMessage const &mav_message); + } // namespace dronecode_sdk diff --git a/plugins/telemetry/mocks/telemetry_mock.h b/plugins/telemetry/mocks/telemetry_mock.h index dd5409ee4b..4ccbc42e14 100644 --- a/plugins/telemetry/mocks/telemetry_mock.h +++ b/plugins/telemetry/mocks/telemetry_mock.h @@ -11,6 +11,7 @@ class MockTelemetry { MOCK_CONST_METHOD1(health_async, void(Telemetry::health_callback_t)){}; MOCK_CONST_METHOD1(home_position_async, void(Telemetry::position_callback_t)){}; MOCK_CONST_METHOD1(in_air_async, void(Telemetry::in_air_callback_t)){}; + MOCK_CONST_METHOD1(mav_message_async, void(Telemetry::mav_message_callback_t)){}; MOCK_CONST_METHOD1(armed_async, void(Telemetry::armed_callback_t)){}; MOCK_CONST_METHOD1(gps_info_async, void(Telemetry::gps_info_callback_t)){}; MOCK_CONST_METHOD1(battery_async, void(Telemetry::battery_callback_t)){}; diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index b4c705238c..1526703545 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -31,6 +31,11 @@ Telemetry::Result Telemetry::set_rate_in_air(double rate_hz) return _impl->set_rate_in_air(rate_hz); } +Telemetry::Result Telemetry::set_rate_mav_message(double rate_hz) +{ + return _impl->set_rate_mav_message(rate_hz); +} + Telemetry::Result Telemetry::set_rate_attitude(double rate_hz) { return _impl->set_rate_attitude(rate_hz); @@ -81,6 +86,11 @@ void Telemetry::set_rate_in_air_async(double rate_hz, result_callback_t callback _impl->set_rate_in_air_async(rate_hz, callback); } +void Telemetry::set_rate_mav_message_async(double rate_hz, result_callback_t callback) +{ + _impl->set_rate_mav_message_async(rate_hz, callback); +} + void Telemetry::set_rate_attitude_async(double rate_hz, result_callback_t callback) { _impl->set_rate_attitude_async(rate_hz, callback); @@ -131,6 +141,11 @@ bool Telemetry::in_air() const return _impl->in_air(); } +Telemetry::MavMessage Telemetry::mav_message() const +{ + return _impl->get_mav_message(); +} + bool Telemetry::armed() const { return _impl->armed(); @@ -211,6 +226,11 @@ void Telemetry::in_air_async(in_air_callback_t callback) return _impl->in_air_async(callback); } +void Telemetry::mav_message_async(mav_message_callback_t callback) +{ + return _impl->mav_message_async(callback); +} + void Telemetry::armed_async(armed_callback_t callback) { return _impl->armed_async(callback); @@ -472,4 +492,14 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status << ", signal_strength_percent: " << rc_status.signal_strength_percent << "]"; } +bool operator==(const Telemetry::MavMessage &lhs, const Telemetry::MavMessage &rhs) +{ + return lhs.message_str == rhs.message_str ; +} + +std::ostream &operator<<(std::ostream &str, Telemetry::MavMessage const &mav_message) +{ + return str << "[message: " << mav_message.message_str << "]"; +} + } // namespace dronecode_sdk diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index 85f7966c5d..c8c23290ec 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -5,6 +5,7 @@ #include "px4_custom_mode.h" #include #include +#include namespace dronecode_sdk { @@ -61,6 +62,9 @@ void TelemetryImpl::init() _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_HEARTBEAT, std::bind(&TelemetryImpl::process_heartbeat, this, _1), this); + _parent->register_mavlink_message_handler( + MAVLINK_MSG_ID_STATUSTEXT, std::bind(&TelemetryImpl::process_statustext, this, _1), this); + _parent->register_mavlink_message_handler( MAVLINK_MSG_ID_RC_CHANNELS, std::bind(&TelemetryImpl::process_rc_channels, this, _1), this); @@ -152,6 +156,12 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz) _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); } +Telemetry::Result TelemetryImpl::set_rate_mav_message(double rate_hz) +{ + return telemetry_result_from_command_result( + _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); // Anotacao: MAVLINK_MSG_ID_STATUSTEXT? +} + Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz) { return telemetry_result_from_command_result( @@ -228,6 +238,14 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::result_call std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } +void TelemetryImpl::set_rate_mav_message_async(double rate_hz, Telemetry::result_callback_t callback) +{ + _parent->set_msg_rate_async( + MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: Possible error here, since I don't know what to set. + rate_hz, + std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); +} + void TelemetryImpl::set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback) { _parent->set_msg_rate_async( @@ -489,6 +507,56 @@ void TelemetryImpl::process_heartbeat(const mavlink_message_t &message) } } +void TelemetryImpl::process_statustext(const mavlink_message_t &message) +{ + mavlink_statustext_t statustext; + mavlink_msg_statustext_decode(&message, &statustext); + + std::string debug_str = "MAVLink: "; + + switch (statustext.severity) { + case MAV_SEVERITY_EMERGENCY: + debug_str += "emergency"; + break; + case MAV_SEVERITY_ALERT: + debug_str += "alert"; + break; + case MAV_SEVERITY_CRITICAL: + debug_str += "critical"; + break; + case MAV_SEVERITY_ERROR: + debug_str += "error"; + break; + case MAV_SEVERITY_WARNING: + debug_str += "warning"; + break; + case MAV_SEVERITY_NOTICE: + debug_str += "notice"; + break; + case MAV_SEVERITY_INFO: + debug_str += "info"; + break; + case MAV_SEVERITY_DEBUG: + debug_str += "debug"; + break; + default: + break; + } + + // statustext.text is not null terminated, therefore we copy it first to + // an array big enough that is zeroed. + char text_with_null[sizeof(statustext.text) + 1]{}; + memcpy(text_with_null, statustext.text, sizeof(statustext.text)); + + std::string mav_message_str = debug_str + ": " + text_with_null; + + set_mav_message({mav_message_str}); + + if (_mav_message_subscription) { + _mav_message_subscription(get_mav_message()); + } +} + void TelemetryImpl::process_rc_channels(const mavlink_message_t &message) { mavlink_rc_channels_t rc_channels; @@ -659,6 +727,18 @@ void TelemetryImpl::set_in_air(bool in_air_new) _in_air = in_air_new; } +void TelemetryImpl::set_mav_message(Telemetry::MavMessage mav_message) +{ + std::lock_guard lock(_mav_message_mutex); + _mav_message = mav_message; +} + +Telemetry::MavMessage TelemetryImpl::get_mav_message() const +{ + std::lock_guard lock(_mav_message_mutex); + return _mav_message; +} + void TelemetryImpl::set_armed(bool armed_new) { _armed = armed_new; @@ -854,6 +934,11 @@ void TelemetryImpl::in_air_async(Telemetry::in_air_callback_t &callback) _in_air_subscription = callback; } +void TelemetryImpl::mav_message_async(Telemetry::mav_message_callback_t &callback) +{ + _mav_message_subscription = callback; +} + void TelemetryImpl::armed_async(Telemetry::armed_callback_t &callback) { _armed_subscription = callback; diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index f110f8bd19..e9f71f34ec 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -30,6 +30,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Result set_rate_position(double rate_hz); Telemetry::Result set_rate_home_position(double rate_hz); Telemetry::Result set_rate_in_air(double rate_hz); + Telemetry::Result set_rate_mav_message(double rate_hz); Telemetry::Result set_rate_attitude(double rate_hz); Telemetry::Result set_rate_camera_attitude(double rate_hz); Telemetry::Result set_rate_ground_speed_ned(double rate_hz); @@ -42,6 +43,7 @@ class TelemetryImpl : public PluginImplBase { void set_rate_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_home_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_in_air_async(double rate_hz, Telemetry::result_callback_t callback); + void set_rate_mav_message_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_camera_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_ground_speed_ned_async(double rate_hz, Telemetry::result_callback_t callback); @@ -54,6 +56,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Position get_home_position() const; bool in_air() const; bool armed() const; + Telemetry::MavMessage get_mav_message() const; Telemetry::EulerAngle get_attitude_euler_angle() const; Telemetry::Quaternion get_attitude_quaternion() const; Telemetry::EulerAngle get_camera_attitude_euler_angle() const; @@ -70,6 +73,7 @@ class TelemetryImpl : public PluginImplBase { void position_async(Telemetry::position_callback_t &callback); void home_position_async(Telemetry::position_callback_t &callback); void in_air_async(Telemetry::in_air_callback_t &callback); + void mav_message_async(Telemetry::mav_message_callback_t &callback); void armed_async(Telemetry::armed_callback_t &callback); void attitude_quaternion_async(Telemetry::attitude_quaternion_callback_t &callback); void attitude_euler_angle_async(Telemetry::attitude_euler_angle_callback_t &callback); @@ -91,6 +95,7 @@ class TelemetryImpl : public PluginImplBase { void set_position(Telemetry::Position position); void set_home_position(Telemetry::Position home_position); void set_in_air(bool in_air); + void set_mav_message(Telemetry::MavMessage mav_message); // Anotacao: HERE> void set_armed(bool armed); void set_attitude_quaternion(Telemetry::Quaternion quaternion); void set_camera_attitude_euler_angle(Telemetry::EulerAngle euler_angle); @@ -116,6 +121,7 @@ class TelemetryImpl : public PluginImplBase { void process_extended_sys_state(const mavlink_message_t &message); void process_sys_status(const mavlink_message_t &message); void process_heartbeat(const mavlink_message_t &message); + void process_statustext(const mavlink_message_t &message); // Anotacao: Problem here? void process_rc_channels(const mavlink_message_t &message); void receive_param_cal_gyro(MAVLinkParameters::Result result, int value); @@ -154,6 +160,10 @@ class TelemetryImpl : public PluginImplBase { std::atomic_bool _in_air{false}; std::atomic_bool _armed{false}; + // Anotacao + mutable std::mutex _mav_message_mutex{}; + Telemetry::MavMessage _mav_message{""}; + mutable std::mutex _attitude_quaternion_mutex{}; Telemetry::Quaternion _attitude_quaternion{NAN, NAN, NAN, NAN}; @@ -184,6 +194,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::position_callback_t _position_subscription{nullptr}; Telemetry::position_callback_t _home_position_subscription{nullptr}; Telemetry::in_air_callback_t _in_air_subscription{nullptr}; + Telemetry::mav_message_callback_t _mav_message_subscription{nullptr}; Telemetry::armed_callback_t _armed_subscription{nullptr}; Telemetry::attitude_quaternion_callback_t _attitude_quaternion_subscription{nullptr}; Telemetry::attitude_euler_angle_callback_t _attitude_euler_angle_subscription{nullptr}; From 2b62f7b9e02e97a17f14789ebc2fcd942f65d0e1 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Thu, 25 Apr 2019 17:45:21 -0700 Subject: [PATCH 13/25] Add enum. --- .../telemetry/telemetry_service_impl.h | 29 +++++++++++++++++- .../include/plugins/telemetry/telemetry.h | 11 +++++++ plugins/telemetry/telemetry.cpp | 3 +- plugins/telemetry/telemetry_impl.cpp | 30 ++++++++++++------- plugins/telemetry/telemetry_impl.h | 2 +- 5 files changed, 62 insertions(+), 13 deletions(-) diff --git a/backend/src/plugins/telemetry/telemetry_service_impl.h b/backend/src/plugins/telemetry/telemetry_service_impl.h index 02c07ee32b..b172317b9d 100644 --- a/backend/src/plugins/telemetry/telemetry_service_impl.h +++ b/backend/src/plugins/telemetry/telemetry_service_impl.h @@ -101,9 +101,10 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet const dronecode_sdk::rpc::telemetry::SubscribeMavMessageRequest * /* request */, grpc::ServerWriter *writer) override { - _telemetry.mav_message_async([&writer](dronecode_sdk::Telemetry::MavMessage mav_message) { + _telemetry.mav_message_async([this, &writer](dronecode_sdk::Telemetry::MavMessage mav_message) { auto rpc_mav_message = new dronecode_sdk::rpc::telemetry::MavMessage(); rpc_mav_message->set_message_str(mav_message.message_str); + rpc_mav_message->set_message_type(translateMavMessageType(mav_message.message_type)); dronecode_sdk::rpc::telemetry::MavMessageResponse rpc_mav_message_response; rpc_mav_message_response.set_allocated_mav_message(rpc_mav_message); @@ -114,6 +115,32 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet return grpc::Status::OK; } + dronecode_sdk::rpc::telemetry::MavMessage::MessageType + translateMavMessageType(const dronecode_sdk::Telemetry::MavMessage::MessageType message_type) const + { + switch (message_type) { + default: + case dronecode_sdk::Telemetry::MavMessage::MessageType::EMERGENCY: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_EMERGENCY; + case dronecode_sdk::Telemetry::MavMessage::MessageType::ALERT: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_ALERT; + case dronecode_sdk::Telemetry::MavMessage::MessageType::CRITICAL: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_CRITICAL; + case dronecode_sdk::Telemetry::MavMessage::MessageType::ERROR: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_ERROR; + case dronecode_sdk::Telemetry::MavMessage::MessageType::WARNING: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_WARNING; + case dronecode_sdk::Telemetry::MavMessage::MessageType::NOTICE: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_NOTICE; + case dronecode_sdk::Telemetry::MavMessage::MessageType::INFO: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_INFO; + case dronecode_sdk::Telemetry::MavMessage::MessageType::DEBUS: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_DEBUS; + case dronecode_sdk::Telemetry::MavMessage::MessageType::UNKNOWN: + return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_UNKNOWN; + } + } + grpc::Status SubscribeArmed(grpc::ServerContext * /* context */, const dronecode_sdk::rpc::telemetry::SubscribeArmedRequest * /* request */, diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 3b1219d70e..0651a71c0f 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -130,6 +130,17 @@ class Telemetry : public PluginBase { }; struct MavMessage { + enum class MessageType { + EMERGENCY, + ALERT, + CRITICAL, + ERROR, + WARNING, + NOTICE, + INFO, + DEBUS, + UNKNOWN + } message_type; std::string message_str; }; diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index 1526703545..d27210eb5c 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -494,7 +494,8 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status bool operator==(const Telemetry::MavMessage &lhs, const Telemetry::MavMessage &rhs) { - return lhs.message_str == rhs.message_str ; + return lhs.message_str == rhs.message_str && + lhs.message_type == rhs.message_type; } std::ostream &operator<<(std::ostream &str, Telemetry::MavMessage const &mav_message) diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index c8c23290ec..10962f2eea 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -513,33 +513,43 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) mavlink_msg_statustext_decode(&message, &statustext); std::string debug_str = "MAVLink: "; + Telemetry::MavMessage::MessageType mav_message_type; switch (statustext.severity) { case MAV_SEVERITY_EMERGENCY: - debug_str += "emergency"; + mav_message_type = Telemetry::MavMessage::MessageType::EMERGENCY; + // debug_str += "emergency"; break; case MAV_SEVERITY_ALERT: - debug_str += "alert"; + mav_message_type = Telemetry::MavMessage::MessageType::ALERT; + // debug_str += "alert"; break; case MAV_SEVERITY_CRITICAL: - debug_str += "critical"; + mav_message_type = Telemetry::MavMessage::MessageType::CRITICAL; + // debug_str += "critical"; break; case MAV_SEVERITY_ERROR: - debug_str += "error"; + mav_message_type = Telemetry::MavMessage::MessageType::ERROR; + // debug_str += "error"; break; case MAV_SEVERITY_WARNING: - debug_str += "warning"; + mav_message_type = Telemetry::MavMessage::MessageType::WARNING; + // debug_str += "warning"; break; case MAV_SEVERITY_NOTICE: - debug_str += "notice"; + mav_message_type = Telemetry::MavMessage::MessageType::NOTICE; + // debug_str += "notice"; break; case MAV_SEVERITY_INFO: - debug_str += "info"; + mav_message_type = Telemetry::MavMessage::MessageType::INFO; + // debug_str += "info"; break; case MAV_SEVERITY_DEBUG: - debug_str += "debug"; + mav_message_type = Telemetry::MavMessage::MessageType::DEBUS; + // debug_str += "debug"; break; default: + mav_message_type = Telemetry::MavMessage::MessageType::UNKNOWN; break; } @@ -548,9 +558,9 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) char text_with_null[sizeof(statustext.text) + 1]{}; memcpy(text_with_null, statustext.text, sizeof(statustext.text)); - std::string mav_message_str = debug_str + ": " + text_with_null; + std::string mav_message_str = text_with_null; //debug_str + ": " + text_with_null; - set_mav_message({mav_message_str}); + set_mav_message({mav_message_type, mav_message_str}); if (_mav_message_subscription) { _mav_message_subscription(get_mav_message()); diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index e9f71f34ec..827fbcf663 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -162,7 +162,7 @@ class TelemetryImpl : public PluginImplBase { // Anotacao mutable std::mutex _mav_message_mutex{}; - Telemetry::MavMessage _mav_message{""}; + Telemetry::MavMessage _mav_message{Telemetry::MavMessage::MessageType::UNKNOWN, ""}; mutable std::mutex _attitude_quaternion_mutex{}; Telemetry::Quaternion _attitude_quaternion{NAN, NAN, NAN, NAN}; From be3023890253bb0c88579200004b40420acbc964 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Fri, 26 Apr 2019 15:01:19 -0700 Subject: [PATCH 14/25] Rename it to StatusText. --- .../telemetry/telemetry_service_impl.h | 52 ++++++-------- integration_tests/telemetry_async.cpp | 14 ++-- .../include/plugins/telemetry/telemetry.h | 66 ++++++++++++------ plugins/telemetry/mocks/telemetry_mock.h | 2 +- plugins/telemetry/telemetry.cpp | 26 +++---- plugins/telemetry/telemetry_impl.cpp | 69 ++++++------------- plugins/telemetry/telemetry_impl.h | 19 +++-- 7 files changed, 117 insertions(+), 131 deletions(-) diff --git a/backend/src/plugins/telemetry/telemetry_service_impl.h b/backend/src/plugins/telemetry/telemetry_service_impl.h index b172317b9d..d66228e0be 100644 --- a/backend/src/plugins/telemetry/telemetry_service_impl.h +++ b/backend/src/plugins/telemetry/telemetry_service_impl.h @@ -97,47 +97,35 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet } grpc::Status - SubscribeMavMessage(grpc::ServerContext * /* context */, - const dronecode_sdk::rpc::telemetry::SubscribeMavMessageRequest * /* request */, - grpc::ServerWriter *writer) override + SubscribeStatusText(grpc::ServerContext * /* context */, + const dronecode_sdk::rpc::telemetry::SubscribeStatusTextRequest * /* request */, + grpc::ServerWriter *writer) override { - _telemetry.mav_message_async([this, &writer](dronecode_sdk::Telemetry::MavMessage mav_message) { - auto rpc_mav_message = new dronecode_sdk::rpc::telemetry::MavMessage(); - rpc_mav_message->set_message_str(mav_message.message_str); - rpc_mav_message->set_message_type(translateMavMessageType(mav_message.message_type)); - - dronecode_sdk::rpc::telemetry::MavMessageResponse rpc_mav_message_response; - rpc_mav_message_response.set_allocated_mav_message(rpc_mav_message); - writer->Write(rpc_mav_message_response); + _telemetry.status_text_async([this, &writer](dronecode_sdk::Telemetry::StatusText status_text) { + auto rpc_status_text = new dronecode_sdk::rpc::telemetry::StatusText(); + rpc_status_text->set_text(status_text.text); + rpc_status_text->set_type(translateStatusTextType(status_text.type)); + + dronecode_sdk::rpc::telemetry::StatusTextResponse rpc_status_text_response; + rpc_status_text_response.set_allocated_status_text(rpc_status_text); + writer->Write(rpc_status_text_response); }); _stop_future.wait(); return grpc::Status::OK; } - dronecode_sdk::rpc::telemetry::MavMessage::MessageType - translateMavMessageType(const dronecode_sdk::Telemetry::MavMessage::MessageType message_type) const + dronecode_sdk::rpc::telemetry::StatusText::StatusType + translateStatusTextType(const dronecode_sdk::Telemetry::StatusText::StatusType type) const { - switch (message_type) { + switch (type) { default: - case dronecode_sdk::Telemetry::MavMessage::MessageType::EMERGENCY: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_EMERGENCY; - case dronecode_sdk::Telemetry::MavMessage::MessageType::ALERT: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_ALERT; - case dronecode_sdk::Telemetry::MavMessage::MessageType::CRITICAL: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_CRITICAL; - case dronecode_sdk::Telemetry::MavMessage::MessageType::ERROR: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_ERROR; - case dronecode_sdk::Telemetry::MavMessage::MessageType::WARNING: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_WARNING; - case dronecode_sdk::Telemetry::MavMessage::MessageType::NOTICE: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_NOTICE; - case dronecode_sdk::Telemetry::MavMessage::MessageType::INFO: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_INFO; - case dronecode_sdk::Telemetry::MavMessage::MessageType::DEBUS: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_DEBUS; - case dronecode_sdk::Telemetry::MavMessage::MessageType::UNKNOWN: - return dronecode_sdk::rpc::telemetry::MavMessage::MessageType::MavMessage_MessageType_UNKNOWN; + case dronecode_sdk::Telemetry::StatusText::StatusType::INFO: + return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_INFO; + case dronecode_sdk::Telemetry::StatusText::StatusType::WARNING: + return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_WARNING; + case dronecode_sdk::Telemetry::StatusText::StatusType::CRITICAL: + return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_CRITICAL; } } diff --git a/integration_tests/telemetry_async.cpp b/integration_tests/telemetry_async.cpp index aebbb776b3..971b5a5a7c 100644 --- a/integration_tests/telemetry_async.cpp +++ b/integration_tests/telemetry_async.cpp @@ -23,7 +23,7 @@ static void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned); static void print_gps_info(Telemetry::GPSInfo gps_info); static void print_battery(Telemetry::Battery battery); static void print_rc_status(Telemetry::RCStatus rc_status); -static void print_mav_message(Telemetry::MavMessage mav_message); +static void print_status_text(Telemetry::StatusText status_text); static void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned); static bool _set_rate_error = false; @@ -41,7 +41,7 @@ static bool _received_ground_speed = false; static bool _received_gps_info = false; static bool _received_battery = false; static bool _received_rc_status = false; -static bool _received_mav_message = false; +static bool _received_status_text = false; static bool _received_position_velocity_ned = false; TEST_F(SitlTest, TelemetryAsync) @@ -115,7 +115,7 @@ TEST_F(SitlTest, TelemetryAsync) telemetry->rc_status_async(std::bind(&print_rc_status, _1)); - telemetry->mav_message_async(std::bind(&print_mav_message, _1)); + telemetry->status_text_async(std::bind(&print_status_text, _1)); telemetry->position_velocity_ned_async(std::bind(&print_position_velocity_ned, _1)); @@ -136,7 +136,7 @@ TEST_F(SitlTest, TelemetryAsync) EXPECT_TRUE(_received_gps_info); EXPECT_TRUE(_received_battery); EXPECT_TRUE(_received_rc_status); - EXPECT_TRUE(_received_mav_message); + EXPECT_TRUE(_received_status_text); EXPECT_TRUE(_received_position_velocity_ned); } @@ -243,11 +243,11 @@ void print_rc_status(Telemetry::RCStatus rc_status) _received_rc_status = true; } -void print_mav_message(Telemetry::MavMessage mav_message) +void print_status_text(Telemetry::StatusText status_text) { - std::cout << "Mav Message [" << mav_message.message_str << "]" + std::cout << "Status Text [" << status_text.text << "]" << std::endl; - _received_mav_message = true; + _received_status_text = true; } void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned) diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 0651a71c0f..94ae8ffcbe 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -129,19 +129,13 @@ class Telemetry : public PluginBase { 5: RTK float, 6: RTK fixed). */ }; - struct MavMessage { - enum class MessageType { - EMERGENCY, - ALERT, - CRITICAL, - ERROR, - WARNING, - NOTICE, - INFO, - DEBUS, - UNKNOWN - } message_type; - std::string message_str; + struct StatusText { + enum class StatusType { + INFO, + WARNING, + CRITICAL + } type; + std::string text; }; /** @@ -261,7 +255,13 @@ class Telemetry : public PluginBase { */ Result set_rate_in_air(double rate_hz); - Result set_rate_mav_message(double rate_hz); + /** + * @brief Set rate of status text updates (synchronous). + * + * @param rate_hz Rate in Hz. + * @return Result of request. + */ + Result set_rate_status_text(double rate_hz); /** * @brief Set rate of attitude updates (synchronous). @@ -344,7 +344,13 @@ class Telemetry : public PluginBase { */ void set_rate_in_air_async(double rate_hz, result_callback_t callback); - void set_rate_mav_message_async(double rate_hz, result_callback_t callback); + /** + * @brief Set rate of status text updates (asynchronous). + * + * @param rate_hz Rate in Hz. + * @param callback Callback to receive request result. + */ + void set_rate_status_text_async(double rate_hz, result_callback_t callback); /** * @brief Set rate of attitude updates (asynchronous). @@ -415,6 +421,13 @@ class Telemetry : public PluginBase { */ Position home_position() const; + /** + * @brief Get status text (synchronous). + * + * @return Status text. + */ + StatusText status_text() const; + /** * @brief Get the in-air status (synchronous). * @@ -422,8 +435,6 @@ class Telemetry : public PluginBase { */ bool in_air() const; - MavMessage mav_message() const; - /** * @brief Get the arming status (synchronous). * @@ -548,7 +559,12 @@ class Telemetry : public PluginBase { */ typedef std::function in_air_callback_t; - typedef std::function mav_message_callback_t; // Anotacao: Remove mav_message from here? + /** + * @brief Callback for mavlink status text updates. + * + * @param status text with message type and text. + */ + typedef std::function status_text_callback_t; /** * @brief Subscribe to in-air updates (asynchronous). @@ -557,7 +573,12 @@ class Telemetry : public PluginBase { */ void in_air_async(in_air_callback_t callback); - void mav_message_async(mav_message_callback_t callback); + /** + * @brief Subscribe to status text updates (asynchronous). + * + * @param callback Function to call with updates. + */ + void status_text_async(status_text_callback_t callback); /** * @brief Callback type for armed updates (asynchronous). @@ -878,6 +899,11 @@ bool operator==(const Telemetry::RCStatus &lhs, const Telemetry::RCStatus &rhs); */ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status); -std::ostream &operator<<(std::ostream &str, Telemetry::MavMessage const &mav_message); +/** + * @brief Stream operator to print information about a `Telemetry::StatusText`. + * + * @returns A reference to the stream. +*/ +std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text); } // namespace dronecode_sdk diff --git a/plugins/telemetry/mocks/telemetry_mock.h b/plugins/telemetry/mocks/telemetry_mock.h index 4ccbc42e14..5f57a6b7a7 100644 --- a/plugins/telemetry/mocks/telemetry_mock.h +++ b/plugins/telemetry/mocks/telemetry_mock.h @@ -11,7 +11,7 @@ class MockTelemetry { MOCK_CONST_METHOD1(health_async, void(Telemetry::health_callback_t)){}; MOCK_CONST_METHOD1(home_position_async, void(Telemetry::position_callback_t)){}; MOCK_CONST_METHOD1(in_air_async, void(Telemetry::in_air_callback_t)){}; - MOCK_CONST_METHOD1(mav_message_async, void(Telemetry::mav_message_callback_t)){}; + MOCK_CONST_METHOD1(status_text_async, void(Telemetry::status_text_callback_t)){}; MOCK_CONST_METHOD1(armed_async, void(Telemetry::armed_callback_t)){}; MOCK_CONST_METHOD1(gps_info_async, void(Telemetry::gps_info_callback_t)){}; MOCK_CONST_METHOD1(battery_async, void(Telemetry::battery_callback_t)){}; diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index d27210eb5c..d7608091c3 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -31,9 +31,9 @@ Telemetry::Result Telemetry::set_rate_in_air(double rate_hz) return _impl->set_rate_in_air(rate_hz); } -Telemetry::Result Telemetry::set_rate_mav_message(double rate_hz) +Telemetry::Result Telemetry::set_rate_status_text(double rate_hz) { - return _impl->set_rate_mav_message(rate_hz); + return _impl->set_rate_status_text(rate_hz); } Telemetry::Result Telemetry::set_rate_attitude(double rate_hz) @@ -86,9 +86,9 @@ void Telemetry::set_rate_in_air_async(double rate_hz, result_callback_t callback _impl->set_rate_in_air_async(rate_hz, callback); } -void Telemetry::set_rate_mav_message_async(double rate_hz, result_callback_t callback) +void Telemetry::set_rate_status_text_async(double rate_hz, result_callback_t callback) { - _impl->set_rate_mav_message_async(rate_hz, callback); + _impl->set_rate_status_text_async(rate_hz, callback); } void Telemetry::set_rate_attitude_async(double rate_hz, result_callback_t callback) @@ -141,9 +141,9 @@ bool Telemetry::in_air() const return _impl->in_air(); } -Telemetry::MavMessage Telemetry::mav_message() const +Telemetry::StatusText Telemetry::status_text() const { - return _impl->get_mav_message(); + return _impl->get_status_text(); } bool Telemetry::armed() const @@ -226,9 +226,9 @@ void Telemetry::in_air_async(in_air_callback_t callback) return _impl->in_air_async(callback); } -void Telemetry::mav_message_async(mav_message_callback_t callback) +void Telemetry::status_text_async(status_text_callback_t callback) { - return _impl->mav_message_async(callback); + return _impl->status_text_async(callback); } void Telemetry::armed_async(armed_callback_t callback) @@ -492,15 +492,15 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status << ", signal_strength_percent: " << rc_status.signal_strength_percent << "]"; } -bool operator==(const Telemetry::MavMessage &lhs, const Telemetry::MavMessage &rhs) +bool operator==(const Telemetry::StatusText &lhs, const Telemetry::StatusText &rhs) { - return lhs.message_str == rhs.message_str && - lhs.message_type == rhs.message_type; + return lhs.text == rhs.text && + lhs.type == rhs.type; } -std::ostream &operator<<(std::ostream &str, Telemetry::MavMessage const &mav_message) +std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text) { - return str << "[message: " << mav_message.message_str << "]"; + return str << "[statustext: " << status_text.text << "]"; } } // namespace dronecode_sdk diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index 10962f2eea..e88f69b275 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -156,10 +156,10 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz) _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); } -Telemetry::Result TelemetryImpl::set_rate_mav_message(double rate_hz) +Telemetry::Result TelemetryImpl::set_rate_status_text(double rate_hz) { return telemetry_result_from_command_result( - _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); // Anotacao: MAVLINK_MSG_ID_STATUSTEXT? + _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? } Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz) @@ -238,10 +238,10 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::result_call std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } -void TelemetryImpl::set_rate_mav_message_async(double rate_hz, Telemetry::result_callback_t callback) +void TelemetryImpl::set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback) { _parent->set_msg_rate_async( - MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: Possible error here, since I don't know what to set. + MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: What rate to set here? rate_hz, std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } @@ -512,44 +512,17 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) mavlink_statustext_t statustext; mavlink_msg_statustext_decode(&message, &statustext); - std::string debug_str = "MAVLink: "; - Telemetry::MavMessage::MessageType mav_message_type; + Telemetry::StatusText::StatusType type; switch (statustext.severity) { - case MAV_SEVERITY_EMERGENCY: - mav_message_type = Telemetry::MavMessage::MessageType::EMERGENCY; - // debug_str += "emergency"; - break; - case MAV_SEVERITY_ALERT: - mav_message_type = Telemetry::MavMessage::MessageType::ALERT; - // debug_str += "alert"; - break; - case MAV_SEVERITY_CRITICAL: - mav_message_type = Telemetry::MavMessage::MessageType::CRITICAL; - // debug_str += "critical"; - break; - case MAV_SEVERITY_ERROR: - mav_message_type = Telemetry::MavMessage::MessageType::ERROR; - // debug_str += "error"; - break; case MAV_SEVERITY_WARNING: - mav_message_type = Telemetry::MavMessage::MessageType::WARNING; - // debug_str += "warning"; - break; - case MAV_SEVERITY_NOTICE: - mav_message_type = Telemetry::MavMessage::MessageType::NOTICE; - // debug_str += "notice"; + type = Telemetry::StatusText::StatusType::WARNING; break; - case MAV_SEVERITY_INFO: - mav_message_type = Telemetry::MavMessage::MessageType::INFO; - // debug_str += "info"; - break; - case MAV_SEVERITY_DEBUG: - mav_message_type = Telemetry::MavMessage::MessageType::DEBUS; - // debug_str += "debug"; + case MAV_SEVERITY_CRITICAL: + type = Telemetry::StatusText::StatusType::CRITICAL; break; default: - mav_message_type = Telemetry::MavMessage::MessageType::UNKNOWN; + type = Telemetry::StatusText::StatusType::INFO; break; } @@ -558,12 +531,12 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) char text_with_null[sizeof(statustext.text) + 1]{}; memcpy(text_with_null, statustext.text, sizeof(statustext.text)); - std::string mav_message_str = text_with_null; //debug_str + ": " + text_with_null; + std::string text = text_with_null; - set_mav_message({mav_message_type, mav_message_str}); + set_status_text({type, text}); - if (_mav_message_subscription) { - _mav_message_subscription(get_mav_message()); + if (_status_text_subscription) { + _status_text_subscription(get_status_text()); } } @@ -737,16 +710,16 @@ void TelemetryImpl::set_in_air(bool in_air_new) _in_air = in_air_new; } -void TelemetryImpl::set_mav_message(Telemetry::MavMessage mav_message) +void TelemetryImpl::set_status_text(Telemetry::StatusText status_text) { - std::lock_guard lock(_mav_message_mutex); - _mav_message = mav_message; + std::lock_guard lock(_status_text_mutex); + _status_text = status_text; } -Telemetry::MavMessage TelemetryImpl::get_mav_message() const +Telemetry::StatusText TelemetryImpl::get_status_text() const { - std::lock_guard lock(_mav_message_mutex); - return _mav_message; + std::lock_guard lock(_status_text_mutex); + return _status_text; } void TelemetryImpl::set_armed(bool armed_new) @@ -944,9 +917,9 @@ void TelemetryImpl::in_air_async(Telemetry::in_air_callback_t &callback) _in_air_subscription = callback; } -void TelemetryImpl::mav_message_async(Telemetry::mav_message_callback_t &callback) +void TelemetryImpl::status_text_async(Telemetry::status_text_callback_t &callback) { - _mav_message_subscription = callback; + _status_text_subscription = callback; } void TelemetryImpl::armed_async(Telemetry::armed_callback_t &callback) diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index 827fbcf663..56a11f2383 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -30,7 +30,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Result set_rate_position(double rate_hz); Telemetry::Result set_rate_home_position(double rate_hz); Telemetry::Result set_rate_in_air(double rate_hz); - Telemetry::Result set_rate_mav_message(double rate_hz); + Telemetry::Result set_rate_status_text(double rate_hz); Telemetry::Result set_rate_attitude(double rate_hz); Telemetry::Result set_rate_camera_attitude(double rate_hz); Telemetry::Result set_rate_ground_speed_ned(double rate_hz); @@ -43,7 +43,7 @@ class TelemetryImpl : public PluginImplBase { void set_rate_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_home_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_in_air_async(double rate_hz, Telemetry::result_callback_t callback); - void set_rate_mav_message_async(double rate_hz, Telemetry::result_callback_t callback); + void set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_camera_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_ground_speed_ned_async(double rate_hz, Telemetry::result_callback_t callback); @@ -56,7 +56,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Position get_home_position() const; bool in_air() const; bool armed() const; - Telemetry::MavMessage get_mav_message() const; + Telemetry::StatusText get_status_text() const; Telemetry::EulerAngle get_attitude_euler_angle() const; Telemetry::Quaternion get_attitude_quaternion() const; Telemetry::EulerAngle get_camera_attitude_euler_angle() const; @@ -73,7 +73,7 @@ class TelemetryImpl : public PluginImplBase { void position_async(Telemetry::position_callback_t &callback); void home_position_async(Telemetry::position_callback_t &callback); void in_air_async(Telemetry::in_air_callback_t &callback); - void mav_message_async(Telemetry::mav_message_callback_t &callback); + void status_text_async(Telemetry::status_text_callback_t &callback); void armed_async(Telemetry::armed_callback_t &callback); void attitude_quaternion_async(Telemetry::attitude_quaternion_callback_t &callback); void attitude_euler_angle_async(Telemetry::attitude_euler_angle_callback_t &callback); @@ -95,7 +95,7 @@ class TelemetryImpl : public PluginImplBase { void set_position(Telemetry::Position position); void set_home_position(Telemetry::Position home_position); void set_in_air(bool in_air); - void set_mav_message(Telemetry::MavMessage mav_message); // Anotacao: HERE> + void set_status_text(Telemetry::StatusText status_text); void set_armed(bool armed); void set_attitude_quaternion(Telemetry::Quaternion quaternion); void set_camera_attitude_euler_angle(Telemetry::EulerAngle euler_angle); @@ -121,7 +121,7 @@ class TelemetryImpl : public PluginImplBase { void process_extended_sys_state(const mavlink_message_t &message); void process_sys_status(const mavlink_message_t &message); void process_heartbeat(const mavlink_message_t &message); - void process_statustext(const mavlink_message_t &message); // Anotacao: Problem here? + void process_statustext(const mavlink_message_t &message); void process_rc_channels(const mavlink_message_t &message); void receive_param_cal_gyro(MAVLinkParameters::Result result, int value); @@ -160,9 +160,8 @@ class TelemetryImpl : public PluginImplBase { std::atomic_bool _in_air{false}; std::atomic_bool _armed{false}; - // Anotacao - mutable std::mutex _mav_message_mutex{}; - Telemetry::MavMessage _mav_message{Telemetry::MavMessage::MessageType::UNKNOWN, ""}; + mutable std::mutex _status_text_mutex{}; + Telemetry::StatusText _status_text{Telemetry::StatusText::StatusType::INFO, ""}; mutable std::mutex _attitude_quaternion_mutex{}; Telemetry::Quaternion _attitude_quaternion{NAN, NAN, NAN, NAN}; @@ -194,7 +193,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::position_callback_t _position_subscription{nullptr}; Telemetry::position_callback_t _home_position_subscription{nullptr}; Telemetry::in_air_callback_t _in_air_subscription{nullptr}; - Telemetry::mav_message_callback_t _mav_message_subscription{nullptr}; + Telemetry::status_text_callback_t _status_text_subscription{nullptr}; Telemetry::armed_callback_t _armed_subscription{nullptr}; Telemetry::attitude_quaternion_callback_t _attitude_quaternion_subscription{nullptr}; Telemetry::attitude_euler_angle_callback_t _attitude_euler_angle_subscription{nullptr}; From 2aec7c9ee9d17f11edfc0182c203cd00a81d0f01 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Fri, 26 Apr 2019 15:04:48 -0700 Subject: [PATCH 15/25] Run ./fix_style.sh . --- .../telemetry/telemetry_service_impl.h | 38 +++++---- core/mavlink_parameters.cpp | 15 ++-- integration_tests/telemetry_async.cpp | 3 +- plugins/camera/camera_impl.cpp | 80 ++++++++++--------- plugins/follow_me/follow_me_impl.cpp | 61 +++++++------- .../include/plugins/telemetry/telemetry.h | 30 +++---- plugins/telemetry/telemetry.cpp | 3 +- plugins/telemetry/telemetry_impl.cpp | 10 ++- 8 files changed, 123 insertions(+), 117 deletions(-) diff --git a/backend/src/plugins/telemetry/telemetry_service_impl.h b/backend/src/plugins/telemetry/telemetry_service_impl.h index d66228e0be..df06f5872e 100644 --- a/backend/src/plugins/telemetry/telemetry_service_impl.h +++ b/backend/src/plugins/telemetry/telemetry_service_impl.h @@ -96,20 +96,21 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet return grpc::Status::OK; } - grpc::Status - SubscribeStatusText(grpc::ServerContext * /* context */, - const dronecode_sdk::rpc::telemetry::SubscribeStatusTextRequest * /* request */, - grpc::ServerWriter *writer) override + grpc::Status SubscribeStatusText( + grpc::ServerContext * /* context */, + const dronecode_sdk::rpc::telemetry::SubscribeStatusTextRequest * /* request */, + grpc::ServerWriter *writer) override { - _telemetry.status_text_async([this, &writer](dronecode_sdk::Telemetry::StatusText status_text) { - auto rpc_status_text = new dronecode_sdk::rpc::telemetry::StatusText(); - rpc_status_text->set_text(status_text.text); - rpc_status_text->set_type(translateStatusTextType(status_text.type)); - - dronecode_sdk::rpc::telemetry::StatusTextResponse rpc_status_text_response; - rpc_status_text_response.set_allocated_status_text(rpc_status_text); - writer->Write(rpc_status_text_response); - }); + _telemetry.status_text_async( + [this, &writer](dronecode_sdk::Telemetry::StatusText status_text) { + auto rpc_status_text = new dronecode_sdk::rpc::telemetry::StatusText(); + rpc_status_text->set_text(status_text.text); + rpc_status_text->set_type(translateStatusTextType(status_text.type)); + + dronecode_sdk::rpc::telemetry::StatusTextResponse rpc_status_text_response; + rpc_status_text_response.set_allocated_status_text(rpc_status_text); + writer->Write(rpc_status_text_response); + }); _stop_future.wait(); return grpc::Status::OK; @@ -121,11 +122,14 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet switch (type) { default: case dronecode_sdk::Telemetry::StatusText::StatusType::INFO: - return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_INFO; + return dronecode_sdk::rpc::telemetry::StatusText::StatusType:: + StatusText_StatusType_INFO; case dronecode_sdk::Telemetry::StatusText::StatusType::WARNING: - return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_WARNING; + return dronecode_sdk::rpc::telemetry::StatusText::StatusType:: + StatusText_StatusType_WARNING; case dronecode_sdk::Telemetry::StatusText::StatusType::CRITICAL: - return dronecode_sdk::rpc::telemetry::StatusText::StatusType::StatusText_StatusType_CRITICAL; + return dronecode_sdk::rpc::telemetry::StatusText::StatusType:: + StatusText_StatusType_CRITICAL; } } @@ -372,7 +376,7 @@ class TelemetryServiceImpl final : public dronecode_sdk::rpc::telemetry::Telemet }); _stop_future.wait(); - return grpc::Status::OK; + return grpc::Status::OK; } void stop() { _stop_promise.set_value(); } diff --git a/core/mavlink_parameters.cpp b/core/mavlink_parameters.cpp index 8ff1bb783f..eceed5e4f4 100644 --- a/core/mavlink_parameters.cpp +++ b/core/mavlink_parameters.cpp @@ -114,13 +114,14 @@ MAVLinkParameters::get_param(const std::string &name, ParamValue value_type, boo auto prom = std::promise>(); auto res = prom.get_future(); - get_param_async(name, - value_type, - [&prom](Result result, ParamValue value) { - prom.set_value(std::make_pair<>(result, value)); - }, - this, - extended); + get_param_async( + name, + value_type, + [&prom](Result result, ParamValue value) { + prom.set_value(std::make_pair<>(result, value)); + }, + this, + extended); return res.get(); } diff --git a/integration_tests/telemetry_async.cpp b/integration_tests/telemetry_async.cpp index 971b5a5a7c..74eefec18a 100644 --- a/integration_tests/telemetry_async.cpp +++ b/integration_tests/telemetry_async.cpp @@ -245,8 +245,7 @@ void print_rc_status(Telemetry::RCStatus rc_status) void print_status_text(Telemetry::StatusText status_text) { - std::cout << "Status Text [" << status_text.text << "]" - << std::endl; + std::cout << "Status Text [" << status_text.text << "]" << std::endl; _received_status_text = true; } diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index f36eabe4e5..ab971a8b54 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -1259,25 +1259,26 @@ void CameraImpl::set_option_async(const std::string &setting_id, return; } - _parent->set_param_async(setting_id, - value, - [this, callback, setting_id, value](MAVLinkParameters::Result result) { - if (result == MAVLinkParameters::Result::SUCCESS) { - if (this->_camera_definition) { - _camera_definition->set_setting(setting_id, value); - refresh_params(); - } - if (callback) { - callback(Camera::Result::SUCCESS); - } - } else { - if (callback) { - callback(Camera::Result::ERROR); - } - } - }, - this, - true); + _parent->set_param_async( + setting_id, + value, + [this, callback, setting_id, value](MAVLinkParameters::Result result) { + if (result == MAVLinkParameters::Result::SUCCESS) { + if (this->_camera_definition) { + _camera_definition->set_setting(setting_id, value); + refresh_params(); + } + if (callback) { + callback(Camera::Result::SUCCESS); + } + } else { + if (callback) { + callback(Camera::Result::ERROR); + } + } + }, + this, + true); } Camera::Result CameraImpl::get_option(const std::string &setting_id, Camera::Option &option) @@ -1457,26 +1458,27 @@ void CameraImpl::refresh_params() const std::string ¶m_name = param.first; const MAVLinkParameters::ParamValue ¶m_value_type = param.second; const bool is_last = (count + 1 == params.size()); - _parent->get_param_async(param_name, - param_value_type, - [param_name, is_last, this](MAVLinkParameters::Result result, - MAVLinkParameters::ParamValue value) { - if (result != MAVLinkParameters::Result::SUCCESS) { - return; - } - // We need to check again by the time this callback runs - if (!this->_camera_definition) { - return; - } - this->_camera_definition->set_setting(param_name, value); - - if (is_last) { - notify_current_settings(); - notify_possible_setting_options(); - } - }, - this, - true); + _parent->get_param_async( + param_name, + param_value_type, + [param_name, is_last, this](MAVLinkParameters::Result result, + MAVLinkParameters::ParamValue value) { + if (result != MAVLinkParameters::Result::SUCCESS) { + return; + } + // We need to check again by the time this callback runs + if (!this->_camera_definition) { + return; + } + this->_camera_definition->set_setting(param_name, value); + + if (is_last) { + notify_current_settings(); + notify_possible_setting_options(); + } + }, + this, + true); ++count; } } diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp index 39418f40e7..7c1eccae6e 100644 --- a/plugins/follow_me/follow_me_impl.cpp +++ b/plugins/follow_me/follow_me_impl.cpp @@ -34,35 +34,38 @@ void FollowMeImpl::deinit() void FollowMeImpl::enable() { - _parent->get_param_float_async("NAV_MIN_FT_HT", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.min_height_m = value; - } - }, - this); - _parent->get_param_float_async("NAV_FT_DST", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.follow_distance_m = value; - } - }, - this); - _parent->get_param_int_async("NAV_FT_FS", - [this](MAVLinkParameters::Result result, int32_t value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.follow_direction = - static_cast(value); - } - }, - this); - _parent->get_param_float_async("NAV_FT_RS", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.responsiveness = value; - } - }, - this); + _parent->get_param_float_async( + "NAV_MIN_FT_HT", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.min_height_m = value; + } + }, + this); + _parent->get_param_float_async( + "NAV_FT_DST", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.follow_distance_m = value; + } + }, + this); + _parent->get_param_int_async( + "NAV_FT_FS", + [this](MAVLinkParameters::Result result, int32_t value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.follow_direction = static_cast(value); + } + }, + this); + _parent->get_param_float_async( + "NAV_FT_RS", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.responsiveness = value; + } + }, + this); } void FollowMeImpl::disable() diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 94ae8ffcbe..75be676b35 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -130,11 +130,7 @@ class Telemetry : public PluginBase { }; struct StatusText { - enum class StatusType { - INFO, - WARNING, - CRITICAL - } type; + enum class StatusType { INFO, WARNING, CRITICAL } type; std::string text; }; @@ -257,10 +253,10 @@ class Telemetry : public PluginBase { /** * @brief Set rate of status text updates (synchronous). - * + * * @param rate_hz Rate in Hz. * @return Result of request. - */ + */ Result set_rate_status_text(double rate_hz); /** @@ -346,10 +342,10 @@ class Telemetry : public PluginBase { /** * @brief Set rate of status text updates (asynchronous). - * + * * @param rate_hz Rate in Hz. * @param callback Callback to receive request result. - */ + */ void set_rate_status_text_async(double rate_hz, result_callback_t callback); /** @@ -423,9 +419,9 @@ class Telemetry : public PluginBase { /** * @brief Get status text (synchronous). - * + * * @return Status text. - */ + */ StatusText status_text() const; /** @@ -561,10 +557,10 @@ class Telemetry : public PluginBase { /** * @brief Callback for mavlink status text updates. - * + * * @param status text with message type and text. */ - typedef std::function status_text_callback_t; + typedef std::function status_text_callback_t; /** * @brief Subscribe to in-air updates (asynchronous). @@ -575,9 +571,9 @@ class Telemetry : public PluginBase { /** * @brief Subscribe to status text updates (asynchronous). - * + * * @param callback Function to call with updates. - */ + */ void status_text_async(status_text_callback_t callback); /** @@ -901,9 +897,9 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status /** * @brief Stream operator to print information about a `Telemetry::StatusText`. - * + * * @returns A reference to the stream. -*/ + */ std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text); } // namespace dronecode_sdk diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index d7608091c3..62e04fc309 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -494,8 +494,7 @@ std::ostream &operator<<(std::ostream &str, Telemetry::RCStatus const &rc_status bool operator==(const Telemetry::StatusText &lhs, const Telemetry::StatusText &rhs) { - return lhs.text == rhs.text && - lhs.type == rhs.type; + return lhs.text == rhs.text && lhs.type == rhs.type; } std::ostream &operator<<(std::ostream &str, Telemetry::StatusText const &status_text) diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index e88f69b275..b04499ab5f 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -158,8 +158,9 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz) Telemetry::Result TelemetryImpl::set_rate_status_text(double rate_hz) { - return telemetry_result_from_command_result( - _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? + return telemetry_result_from_command_result(_parent->set_msg_rate( + MAVLINK_MSG_ID_EXTENDED_SYS_STATE, + rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? } Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz) @@ -238,7 +239,8 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::result_call std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } -void TelemetryImpl::set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback) +void TelemetryImpl::set_rate_status_text_async(double rate_hz, + Telemetry::result_callback_t callback) { _parent->set_msg_rate_async( MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: What rate to set here? @@ -516,7 +518,7 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) switch (statustext.severity) { case MAV_SEVERITY_WARNING: - type = Telemetry::StatusText::StatusType::WARNING; + type = Telemetry::StatusText::StatusType::WARNING; break; case MAV_SEVERITY_CRITICAL: type = Telemetry::StatusText::StatusType::CRITICAL; From cf792dbe897dd9219edc033e7ba8cd45d0be44f6 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Mon, 29 Apr 2019 15:47:30 -0700 Subject: [PATCH 16/25] Some changes requested in PR regarding Removing status text rate. --- integration_tests/telemetry_async.cpp | 19 ++++++----- .../include/plugins/telemetry/telemetry.h | 4 +-- plugins/telemetry/telemetry.cpp | 10 ------ plugins/telemetry/telemetry_impl.cpp | 34 +++++++++++-------- plugins/telemetry/telemetry_impl.h | 4 +-- 5 files changed, 33 insertions(+), 38 deletions(-) diff --git a/integration_tests/telemetry_async.cpp b/integration_tests/telemetry_async.cpp index 74eefec18a..1407191338 100644 --- a/integration_tests/telemetry_async.cpp +++ b/integration_tests/telemetry_async.cpp @@ -23,7 +23,7 @@ static void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned); static void print_gps_info(Telemetry::GPSInfo gps_info); static void print_battery(Telemetry::Battery battery); static void print_rc_status(Telemetry::RCStatus rc_status); -static void print_status_text(Telemetry::StatusText status_text); +// static void print_status_text(Telemetry::StatusText status_text); // Anotacao static void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned); static bool _set_rate_error = false; @@ -41,7 +41,7 @@ static bool _received_ground_speed = false; static bool _received_gps_info = false; static bool _received_battery = false; static bool _received_rc_status = false; -static bool _received_status_text = false; +// static bool _received_status_text = false; // Anotacao static bool _received_position_velocity_ned = false; TEST_F(SitlTest, TelemetryAsync) @@ -115,7 +115,7 @@ TEST_F(SitlTest, TelemetryAsync) telemetry->rc_status_async(std::bind(&print_rc_status, _1)); - telemetry->status_text_async(std::bind(&print_status_text, _1)); + // telemetry->status_text_async(std::bind(&print_status_text, _1)); // Anotacao telemetry->position_velocity_ned_async(std::bind(&print_position_velocity_ned, _1)); @@ -136,7 +136,7 @@ TEST_F(SitlTest, TelemetryAsync) EXPECT_TRUE(_received_gps_info); EXPECT_TRUE(_received_battery); EXPECT_TRUE(_received_rc_status); - EXPECT_TRUE(_received_status_text); + // EXPECT_TRUE(_received_status_text); // Anotacao EXPECT_TRUE(_received_position_velocity_ned); } @@ -243,11 +243,12 @@ void print_rc_status(Telemetry::RCStatus rc_status) _received_rc_status = true; } -void print_status_text(Telemetry::StatusText status_text) -{ - std::cout << "Status Text [" << status_text.text << "]" << std::endl; - _received_status_text = true; -} +// Anotacao +// void print_status_text(Telemetry::StatusText status_text) +// { +// std::cout << "Status Text [" << status_text.text << "]" << std::endl; +// _received_status_text = true; +// } void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned) { diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 75be676b35..e7ec794942 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -257,7 +257,7 @@ class Telemetry : public PluginBase { * @param rate_hz Rate in Hz. * @return Result of request. */ - Result set_rate_status_text(double rate_hz); + // Result set_rate_status_text(double rate_hz); // Anotacao /** * @brief Set rate of attitude updates (synchronous). @@ -346,7 +346,7 @@ class Telemetry : public PluginBase { * @param rate_hz Rate in Hz. * @param callback Callback to receive request result. */ - void set_rate_status_text_async(double rate_hz, result_callback_t callback); + // void set_rate_status_text_async(double rate_hz, result_callback_t callback); // Anotacao /** * @brief Set rate of attitude updates (asynchronous). diff --git a/plugins/telemetry/telemetry.cpp b/plugins/telemetry/telemetry.cpp index 62e04fc309..f738cb6516 100644 --- a/plugins/telemetry/telemetry.cpp +++ b/plugins/telemetry/telemetry.cpp @@ -31,11 +31,6 @@ Telemetry::Result Telemetry::set_rate_in_air(double rate_hz) return _impl->set_rate_in_air(rate_hz); } -Telemetry::Result Telemetry::set_rate_status_text(double rate_hz) -{ - return _impl->set_rate_status_text(rate_hz); -} - Telemetry::Result Telemetry::set_rate_attitude(double rate_hz) { return _impl->set_rate_attitude(rate_hz); @@ -86,11 +81,6 @@ void Telemetry::set_rate_in_air_async(double rate_hz, result_callback_t callback _impl->set_rate_in_air_async(rate_hz, callback); } -void Telemetry::set_rate_status_text_async(double rate_hz, result_callback_t callback) -{ - _impl->set_rate_status_text_async(rate_hz, callback); -} - void Telemetry::set_rate_attitude_async(double rate_hz, result_callback_t callback) { _impl->set_rate_attitude_async(rate_hz, callback); diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index b04499ab5f..53e9d0e159 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -156,12 +156,12 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz) _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); } -Telemetry::Result TelemetryImpl::set_rate_status_text(double rate_hz) -{ - return telemetry_result_from_command_result(_parent->set_msg_rate( - MAVLINK_MSG_ID_EXTENDED_SYS_STATE, - rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? -} +// Telemetry::Result TelemetryImpl::set_rate_status_text(double rate_hz) +// { +// return telemetry_result_from_command_result(_parent->set_msg_rate( +// MAVLINK_MSG_ID_EXTENDED_SYS_STATE, +// rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? +// } Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz) { @@ -239,14 +239,14 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::result_call std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } -void TelemetryImpl::set_rate_status_text_async(double rate_hz, - Telemetry::result_callback_t callback) -{ - _parent->set_msg_rate_async( - MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: What rate to set here? - rate_hz, - std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); -} +// void TelemetryImpl::set_rate_status_text_async(double rate_hz, +// Telemetry::result_callback_t callback) +// { +// _parent->set_msg_rate_async( +// MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: What rate to set here? +// rate_hz, +// std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); +// } void TelemetryImpl::set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback) { @@ -523,7 +523,11 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) case MAV_SEVERITY_CRITICAL: type = Telemetry::StatusText::StatusType::CRITICAL; break; + case MAV_SEVERITY_INFO: + type = Telemetry::StatusText::StatusType::INFO; + break; default: + LogWarn() << "Unknown StatusText severity"; type = Telemetry::StatusText::StatusType::INFO; break; } @@ -533,7 +537,7 @@ void TelemetryImpl::process_statustext(const mavlink_message_t &message) char text_with_null[sizeof(statustext.text) + 1]{}; memcpy(text_with_null, statustext.text, sizeof(statustext.text)); - std::string text = text_with_null; + const std::string text = text_with_null; set_status_text({type, text}); diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index 56a11f2383..0e5d6df966 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -30,7 +30,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Result set_rate_position(double rate_hz); Telemetry::Result set_rate_home_position(double rate_hz); Telemetry::Result set_rate_in_air(double rate_hz); - Telemetry::Result set_rate_status_text(double rate_hz); + // Telemetry::Result set_rate_status_text(double rate_hz); Anotacao Telemetry::Result set_rate_attitude(double rate_hz); Telemetry::Result set_rate_camera_attitude(double rate_hz); Telemetry::Result set_rate_ground_speed_ned(double rate_hz); @@ -43,7 +43,7 @@ class TelemetryImpl : public PluginImplBase { void set_rate_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_home_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_in_air_async(double rate_hz, Telemetry::result_callback_t callback); - void set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback); + // void set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback); Anotacao void set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_camera_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_ground_speed_ned_async(double rate_hz, Telemetry::result_callback_t callback); From 2d71f47705e347e253de90574cead54d208f0bff Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Tue, 30 Apr 2019 10:17:57 -0700 Subject: [PATCH 17/25] Update branch. --- plugins/telemetry/telemetry_impl.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index 0e5d6df966..0b42ecb196 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -30,7 +30,6 @@ class TelemetryImpl : public PluginImplBase { Telemetry::Result set_rate_position(double rate_hz); Telemetry::Result set_rate_home_position(double rate_hz); Telemetry::Result set_rate_in_air(double rate_hz); - // Telemetry::Result set_rate_status_text(double rate_hz); Anotacao Telemetry::Result set_rate_attitude(double rate_hz); Telemetry::Result set_rate_camera_attitude(double rate_hz); Telemetry::Result set_rate_ground_speed_ned(double rate_hz); @@ -43,7 +42,6 @@ class TelemetryImpl : public PluginImplBase { void set_rate_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_home_position_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_in_air_async(double rate_hz, Telemetry::result_callback_t callback); - // void set_rate_status_text_async(double rate_hz, Telemetry::result_callback_t callback); Anotacao void set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_camera_attitude_async(double rate_hz, Telemetry::result_callback_t callback); void set_rate_ground_speed_ned_async(double rate_hz, Telemetry::result_callback_t callback); From 893689ead17ecbc2486af9dc8457ba9945054d87 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Tue, 30 Apr 2019 11:58:11 -0700 Subject: [PATCH 18/25] Remove comments. --- integration_tests/telemetry_async.cpp | 12 ------------ .../include/plugins/telemetry/telemetry.h | 16 ---------------- plugins/telemetry/telemetry_impl.cpp | 16 ---------------- plugins/telemetry/telemetry_impl.h | 1 - 4 files changed, 45 deletions(-) diff --git a/integration_tests/telemetry_async.cpp b/integration_tests/telemetry_async.cpp index 1407191338..93b7021519 100644 --- a/integration_tests/telemetry_async.cpp +++ b/integration_tests/telemetry_async.cpp @@ -23,7 +23,6 @@ static void print_ground_speed_ned(Telemetry::GroundSpeedNED ground_speed_ned); static void print_gps_info(Telemetry::GPSInfo gps_info); static void print_battery(Telemetry::Battery battery); static void print_rc_status(Telemetry::RCStatus rc_status); -// static void print_status_text(Telemetry::StatusText status_text); // Anotacao static void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned); static bool _set_rate_error = false; @@ -41,7 +40,6 @@ static bool _received_ground_speed = false; static bool _received_gps_info = false; static bool _received_battery = false; static bool _received_rc_status = false; -// static bool _received_status_text = false; // Anotacao static bool _received_position_velocity_ned = false; TEST_F(SitlTest, TelemetryAsync) @@ -115,8 +113,6 @@ TEST_F(SitlTest, TelemetryAsync) telemetry->rc_status_async(std::bind(&print_rc_status, _1)); - // telemetry->status_text_async(std::bind(&print_status_text, _1)); // Anotacao - telemetry->position_velocity_ned_async(std::bind(&print_position_velocity_ned, _1)); std::this_thread::sleep_for(std::chrono::seconds(10)); @@ -136,7 +132,6 @@ TEST_F(SitlTest, TelemetryAsync) EXPECT_TRUE(_received_gps_info); EXPECT_TRUE(_received_battery); EXPECT_TRUE(_received_rc_status); - // EXPECT_TRUE(_received_status_text); // Anotacao EXPECT_TRUE(_received_position_velocity_ned); } @@ -243,13 +238,6 @@ void print_rc_status(Telemetry::RCStatus rc_status) _received_rc_status = true; } -// Anotacao -// void print_status_text(Telemetry::StatusText status_text) -// { -// std::cout << "Status Text [" << status_text.text << "]" << std::endl; -// _received_status_text = true; -// } - void print_position_velocity_ned(Telemetry::PositionVelocityNED position_velocity_ned) { std::cout << "Got position north: " << position_velocity_ned.position.north_m << " m, " diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index e7ec794942..f5656eb6a0 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -251,14 +251,6 @@ class Telemetry : public PluginBase { */ Result set_rate_in_air(double rate_hz); - /** - * @brief Set rate of status text updates (synchronous). - * - * @param rate_hz Rate in Hz. - * @return Result of request. - */ - // Result set_rate_status_text(double rate_hz); // Anotacao - /** * @brief Set rate of attitude updates (synchronous). * @@ -340,14 +332,6 @@ class Telemetry : public PluginBase { */ void set_rate_in_air_async(double rate_hz, result_callback_t callback); - /** - * @brief Set rate of status text updates (asynchronous). - * - * @param rate_hz Rate in Hz. - * @param callback Callback to receive request result. - */ - // void set_rate_status_text_async(double rate_hz, result_callback_t callback); // Anotacao - /** * @brief Set rate of attitude updates (asynchronous). * diff --git a/plugins/telemetry/telemetry_impl.cpp b/plugins/telemetry/telemetry_impl.cpp index 53e9d0e159..c30500e4d9 100644 --- a/plugins/telemetry/telemetry_impl.cpp +++ b/plugins/telemetry/telemetry_impl.cpp @@ -156,13 +156,6 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz) _parent->set_msg_rate(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, rate_hz)); } -// Telemetry::Result TelemetryImpl::set_rate_status_text(double rate_hz) -// { -// return telemetry_result_from_command_result(_parent->set_msg_rate( -// MAVLINK_MSG_ID_EXTENDED_SYS_STATE, -// rate_hz)); // Anotacao: What rate to set here? MAVLINK_MSG_ID_STATUSTEXT? -// } - Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz) { return telemetry_result_from_command_result( @@ -239,15 +232,6 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::result_call std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); } -// void TelemetryImpl::set_rate_status_text_async(double rate_hz, -// Telemetry::result_callback_t callback) -// { -// _parent->set_msg_rate_async( -// MAVLINK_MSG_ID_EXTENDED_SYS_STATE, // Anotacao: What rate to set here? -// rate_hz, -// std::bind(&TelemetryImpl::command_result_callback, std::placeholders::_1, callback)); -// } - void TelemetryImpl::set_rate_attitude_async(double rate_hz, Telemetry::result_callback_t callback) { _parent->set_msg_rate_async( diff --git a/plugins/telemetry/telemetry_impl.h b/plugins/telemetry/telemetry_impl.h index 0b42ecb196..288631807d 100644 --- a/plugins/telemetry/telemetry_impl.h +++ b/plugins/telemetry/telemetry_impl.h @@ -212,5 +212,4 @@ class TelemetryImpl : public PluginImplBase { void *_timeout_cookie{nullptr}; }; - } // namespace dronecode_sdk From ff4906a1a7311141d30158d69c125d7939bda568 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Thu, 2 May 2019 10:23:50 -0700 Subject: [PATCH 19/25] Include docstring. --- .../include/plugins/telemetry/telemetry.h | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index f5656eb6a0..0a0f0a800d 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -129,9 +129,23 @@ class Telemetry : public PluginBase { 5: RTK float, 6: RTK fixed). */ }; + /** + * @brief Status Text information type. + */ struct StatusText { - enum class StatusType { INFO, WARNING, CRITICAL } type; - std::string text; + /** + * @brief Status Types. + * + * @note PX4 only supports these 3 status types. + * If other status types are returned for some reason, + * they will be marked as INFO type and logged as a warning. + */ + enum class StatusType { + INFO, + WARNING, + CRITICAL + } type; + std::string text; /**< @brief Mavlink status message. */ }; /** From cce168e1faad7b1defec48d8ffda3e10220a708f Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Thu, 2 May 2019 10:31:23 -0700 Subject: [PATCH 20/25] Add docstrings for SatusType enum. --- plugins/telemetry/include/plugins/telemetry/telemetry.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 0a0f0a800d..4485cbdea9 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -141,9 +141,9 @@ class Telemetry : public PluginBase { * they will be marked as INFO type and logged as a warning. */ enum class StatusType { - INFO, - WARNING, - CRITICAL + INFO, /**< @brief Message type is an information or other. */ + WARNING, /**< @brief Message type is a warning. */ + CRITICAL /**< @brief Message type is critical. */ } type; std::string text; /**< @brief Mavlink status message. */ }; From 9ee7822c923586ec37c6042b8a38b4b845c3cdc0 Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Thu, 25 Apr 2019 12:52:51 -0700 Subject: [PATCH 21/25] Telemetry: implemented StatusText. --- .../telemetry/include/plugins/telemetry/telemetry.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 4485cbdea9..22d5559c64 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -135,12 +135,12 @@ class Telemetry : public PluginBase { struct StatusText { /** * @brief Status Types. - * - * @note PX4 only supports these 3 status types. - * If other status types are returned for some reason, + * + * @note PX4 only supports these 3 status types. + * If other status types are returned for some reason, * they will be marked as INFO type and logged as a warning. - */ - enum class StatusType { + */ + enum class StatusType { INFO, /**< @brief Message type is an information or other. */ WARNING, /**< @brief Message type is a warning. */ CRITICAL /**< @brief Message type is critical. */ From 18bcc005a808a3ef4895cf439ce19cae84d6d73a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 3 May 2019 08:30:12 +0200 Subject: [PATCH 22/25] backend/proto: update submodule This includes the addition for StatusText. --- backend/proto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/backend/proto b/backend/proto index 71c2e73500..dccc60484a 160000 --- a/backend/proto +++ b/backend/proto @@ -1 +1 @@ -Subproject commit 71c2e7350015f358f972d9b32a5b24cc500eba74 +Subproject commit dccc60484a6c94ee1c11799c013c78c5da315741 From f8d20ca224e81faa368fb28df14842146f81d2d4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 6 May 2019 12:14:54 +0200 Subject: [PATCH 23/25] telemetry: fix docs --- plugins/telemetry/include/plugins/telemetry/telemetry.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 22d5559c64..fea79d13d2 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -144,7 +144,7 @@ class Telemetry : public PluginBase { INFO, /**< @brief Message type is an information or other. */ WARNING, /**< @brief Message type is a warning. */ CRITICAL /**< @brief Message type is critical. */ - } type; + } type; /**< @brief Message type. */ std::string text; /**< @brief Mavlink status message. */ }; From 2a3497529560a0e6039aeb72a1b8a4edcc6412ff Mon Sep 17 00:00:00 2001 From: douglaswsilva Date: Mon, 6 May 2019 09:48:03 -0700 Subject: [PATCH 24/25] Add missing doc for dronecode_sdk::Telemetry::StatusText. --- plugins/telemetry/include/plugins/telemetry/telemetry.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/telemetry/include/plugins/telemetry/telemetry.h b/plugins/telemetry/include/plugins/telemetry/telemetry.h index 6fcfa2ccbd..a55e448b50 100644 --- a/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -144,7 +144,7 @@ class Telemetry : public PluginBase { INFO, /**< @brief Message type is an information or other. */ WARNING, /**< @brief Message type is a warning. */ CRITICAL /**< @brief Message type is critical. */ - } type; + } type; /**< @brief Status Text type. */ std::string text; /**< @brief Mavlink status message. */ }; From fcf91d84c9ae42ad7f5c84ab73e033e089f43220 Mon Sep 17 00:00:00 2001 From: Jonas Vautherin Date: Sat, 4 May 2019 01:55:07 +0200 Subject: [PATCH 25/25] fix style with clang-format 6 (clang-format 8 having a bug) --- core/mavlink_parameters.cpp | 15 +++--- plugins/camera/camera_impl.cpp | 80 ++++++++++++++-------------- plugins/follow_me/follow_me_impl.cpp | 61 ++++++++++----------- 3 files changed, 75 insertions(+), 81 deletions(-) diff --git a/core/mavlink_parameters.cpp b/core/mavlink_parameters.cpp index eceed5e4f4..8ff1bb783f 100644 --- a/core/mavlink_parameters.cpp +++ b/core/mavlink_parameters.cpp @@ -114,14 +114,13 @@ MAVLinkParameters::get_param(const std::string &name, ParamValue value_type, boo auto prom = std::promise>(); auto res = prom.get_future(); - get_param_async( - name, - value_type, - [&prom](Result result, ParamValue value) { - prom.set_value(std::make_pair<>(result, value)); - }, - this, - extended); + get_param_async(name, + value_type, + [&prom](Result result, ParamValue value) { + prom.set_value(std::make_pair<>(result, value)); + }, + this, + extended); return res.get(); } diff --git a/plugins/camera/camera_impl.cpp b/plugins/camera/camera_impl.cpp index ab971a8b54..f36eabe4e5 100644 --- a/plugins/camera/camera_impl.cpp +++ b/plugins/camera/camera_impl.cpp @@ -1259,26 +1259,25 @@ void CameraImpl::set_option_async(const std::string &setting_id, return; } - _parent->set_param_async( - setting_id, - value, - [this, callback, setting_id, value](MAVLinkParameters::Result result) { - if (result == MAVLinkParameters::Result::SUCCESS) { - if (this->_camera_definition) { - _camera_definition->set_setting(setting_id, value); - refresh_params(); - } - if (callback) { - callback(Camera::Result::SUCCESS); - } - } else { - if (callback) { - callback(Camera::Result::ERROR); - } - } - }, - this, - true); + _parent->set_param_async(setting_id, + value, + [this, callback, setting_id, value](MAVLinkParameters::Result result) { + if (result == MAVLinkParameters::Result::SUCCESS) { + if (this->_camera_definition) { + _camera_definition->set_setting(setting_id, value); + refresh_params(); + } + if (callback) { + callback(Camera::Result::SUCCESS); + } + } else { + if (callback) { + callback(Camera::Result::ERROR); + } + } + }, + this, + true); } Camera::Result CameraImpl::get_option(const std::string &setting_id, Camera::Option &option) @@ -1458,27 +1457,26 @@ void CameraImpl::refresh_params() const std::string ¶m_name = param.first; const MAVLinkParameters::ParamValue ¶m_value_type = param.second; const bool is_last = (count + 1 == params.size()); - _parent->get_param_async( - param_name, - param_value_type, - [param_name, is_last, this](MAVLinkParameters::Result result, - MAVLinkParameters::ParamValue value) { - if (result != MAVLinkParameters::Result::SUCCESS) { - return; - } - // We need to check again by the time this callback runs - if (!this->_camera_definition) { - return; - } - this->_camera_definition->set_setting(param_name, value); - - if (is_last) { - notify_current_settings(); - notify_possible_setting_options(); - } - }, - this, - true); + _parent->get_param_async(param_name, + param_value_type, + [param_name, is_last, this](MAVLinkParameters::Result result, + MAVLinkParameters::ParamValue value) { + if (result != MAVLinkParameters::Result::SUCCESS) { + return; + } + // We need to check again by the time this callback runs + if (!this->_camera_definition) { + return; + } + this->_camera_definition->set_setting(param_name, value); + + if (is_last) { + notify_current_settings(); + notify_possible_setting_options(); + } + }, + this, + true); ++count; } } diff --git a/plugins/follow_me/follow_me_impl.cpp b/plugins/follow_me/follow_me_impl.cpp index 7c1eccae6e..39418f40e7 100644 --- a/plugins/follow_me/follow_me_impl.cpp +++ b/plugins/follow_me/follow_me_impl.cpp @@ -34,38 +34,35 @@ void FollowMeImpl::deinit() void FollowMeImpl::enable() { - _parent->get_param_float_async( - "NAV_MIN_FT_HT", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.min_height_m = value; - } - }, - this); - _parent->get_param_float_async( - "NAV_FT_DST", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.follow_distance_m = value; - } - }, - this); - _parent->get_param_int_async( - "NAV_FT_FS", - [this](MAVLinkParameters::Result result, int32_t value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.follow_direction = static_cast(value); - } - }, - this); - _parent->get_param_float_async( - "NAV_FT_RS", - [this](MAVLinkParameters::Result result, float value) { - if (result == MAVLinkParameters::Result::SUCCESS) { - _config.responsiveness = value; - } - }, - this); + _parent->get_param_float_async("NAV_MIN_FT_HT", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.min_height_m = value; + } + }, + this); + _parent->get_param_float_async("NAV_FT_DST", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.follow_distance_m = value; + } + }, + this); + _parent->get_param_int_async("NAV_FT_FS", + [this](MAVLinkParameters::Result result, int32_t value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.follow_direction = + static_cast(value); + } + }, + this); + _parent->get_param_float_async("NAV_FT_RS", + [this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::SUCCESS) { + _config.responsiveness = value; + } + }, + this); } void FollowMeImpl::disable()