diff --git a/src/backend/proto b/src/backend/proto index d2042e2dc1..12f26d3b60 160000 --- a/src/backend/proto +++ b/src/backend/proto @@ -1 +1 @@ -Subproject commit d2042e2dc116529db30d0b0c31aa7121cdc7e4b7 +Subproject commit 12f26d3b60fd718f0e6199a453f0d25dbfbbb8cc diff --git a/src/backend/src/plugins/offboard/offboard_service_impl.h b/src/backend/src/plugins/offboard/offboard_service_impl.h index 7880b63930..5618d5cc04 100644 --- a/src/backend/src/plugins/offboard/offboard_service_impl.h +++ b/src/backend/src/plugins/offboard/offboard_service_impl.h @@ -63,6 +63,38 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service return grpc::Status::OK; } + grpc::Status SetActuatorControl( + grpc::ServerContext* /* context */, + const rpc::offboard::SetActuatorControlRequest* request, + rpc::offboard::SetActuatorControlResponse* /* response */) override + { + if (request != nullptr) { + auto requested_actuator_control = + translateRPCActuatorControl(request->actuator_control()); + _offboard.set_actuator_control(requested_actuator_control); + } + + return grpc::Status::OK; + } + + static mavsdk::Offboard::ActuatorControl + translateRPCActuatorControl(const rpc::offboard::ActuatorControl& rpc_actuator_control) + { + mavsdk::Offboard::ActuatorControl actuator_control{std::numeric_limits::quiet_NaN()}; + + int num_groups = std::min(2, rpc_actuator_control.groups_size()); + + for (int i = 0; i < num_groups; i++) { + int num_controls = std::min(8, rpc_actuator_control.groups(i).controls_size()); + for (int j = 0; j < num_controls; j++) { + // https://developers.google.com/protocol-buffers/docs/reference/cpp/google.protobuf.repeated_field# + actuator_control.groups[i].controls[j] = rpc_actuator_control.groups(i).controls(j); + } + } + + return actuator_control; + } + grpc::Status SetAttitude( grpc::ServerContext* /* context */, const rpc::offboard::SetAttitudeRequest* request, diff --git a/src/backend/test/offboard_service_impl_test.cpp b/src/backend/test/offboard_service_impl_test.cpp index e822537c2d..47bd7c5d95 100644 --- a/src/backend/test/offboard_service_impl_test.cpp +++ b/src/backend/test/offboard_service_impl_test.cpp @@ -16,6 +16,14 @@ using OffboardServiceImpl = mavsdk::backend::OffboardServiceImpl; using OffboardResult = mavsdk::rpc::offboard::OffboardResult; using InputPair = std::pair; +static constexpr float ARBITRARY_ACTUATOR_CONTROL_0 = -0.42f; +static constexpr float ARBITRARY_ACTUATOR_CONTROL_1 = 0.15f; +static constexpr float ARBITRARY_ACTUATOR_CONTROL_2 = 0.56f; +static constexpr float ARBITRARY_ACTUATOR_CONTROL_3 = -0.95f; +static constexpr float ARBITRARY_ACTUATOR_CONTROL_4 = 0.34f; +static constexpr float ARBITRARY_ACTUATOR_CONTROL_5 = 0.98f; +static constexpr float ARBITRARY_ACTUATOR_CONTROL_6 = -0.15f; +static constexpr float ARBITRARY_ACTUATOR_CONTROL_7 = 0.15f; static constexpr float ARBITRARY_ROLL = 25.0f; static constexpr float ARBITRARY_PITCH = 40.0f; static constexpr float ARBITRARY_YAW = 37.0f; @@ -46,6 +54,8 @@ class OffboardServiceImplTest : public ::testing::TestWithParam { std::unique_ptr createArbitraryRPCVelocityBodyYawspeed() const; std::unique_ptr createArbitraryRPCVelocityNedYaw() const; + std::unique_ptr + createArbitraryRPCActuatorControl() const; }; TEST_P(OffboardServiceImplTest, startResultIsTranslatedCorrectly) @@ -155,6 +165,73 @@ TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithAllNullParams) offboardService.SetAttitudeRate(nullptr, nullptr, nullptr); } +TEST_F(OffboardServiceImplTest, setActuatorControlDoesNotFailWithAllNullParams) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + + offboardService.SetActuatorControl(nullptr, nullptr, nullptr); +} + +TEST_F(OffboardServiceImplTest, setActuatorControlDoesNotFailWithNullResponse) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + mavsdk::rpc::offboard::SetActuatorControlRequest request; + + auto rpc_actuator_control = createArbitraryRPCActuatorControl(); + request.set_allocated_actuator_control(rpc_actuator_control.release()); + + offboardService.SetActuatorControl(nullptr, &request, nullptr); +} + +TEST_F(OffboardServiceImplTest, setsActuatorControlCorrectly) +{ + MockOffboard offboard; + OffboardServiceImpl offboardService(offboard); + mavsdk::rpc::offboard::SetActuatorControlRequest request; + + auto rpc_actuator_control = createArbitraryRPCActuatorControl(); + const auto expected_actuator_control = + OffboardServiceImpl::translateRPCActuatorControl(*rpc_actuator_control); + EXPECT_CALL(offboard, set_actuator_control(expected_actuator_control)).Times(1); + + request.set_allocated_actuator_control(rpc_actuator_control.release()); + + offboardService.SetActuatorControl(nullptr, &request, nullptr); +} + +std::unique_ptr +OffboardServiceImplTest::createArbitraryRPCActuatorControl() const +{ + auto rpc_actuator_control = std::unique_ptr( + new mavsdk::rpc::offboard::ActuatorControl()); + + auto rpc_actuator_group_0 = rpc_actuator_control.get()->add_groups(); + + rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_0); + rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_1); + rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_2); + rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_3); + rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_4); + rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_5); + rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_6); + rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_7); + + auto rpc_actuator_group_1 = rpc_actuator_control.get()->add_groups(); + + rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_0 + 0.01f); + rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_1 + 0.02f); + rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_2 + 0.03f); + rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_3 + 0.04f); + rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_4 + 0.05f); + rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_5 + 0.06f); + rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_6 + 0.07f); + rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_7 + 0.08f); + + return rpc_actuator_control; +} + TEST_F(OffboardServiceImplTest, setAttitudeDoesNotFailWithNullResponse) { MockOffboard offboard; diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h index 4520a07f50..7aa3e62041 100644 --- a/src/plugins/offboard/include/plugins/offboard/offboard.h +++ b/src/plugins/offboard/include/plugins/offboard/offboard.h @@ -131,6 +131,34 @@ class Offboard : public PluginBase { percent). */ }; + /** + * @brief Type for actuator control command. + * controls members should be normed to -1..+1 where 0 is neutral position. + * Throttle for single rotation direction motors is 0..1, negative range for reverse direction. + * + * One group support eight controls. + * + * Up to 16 actuator controls can be set. To ignore an output group, set all it conrols to NaN. + * If one or more controls in group is not NaN, then all NaN controls will sent as zero. + * The first 8 actuator controls internally map to control group 0, the latter 8 actuator + * controls map to control group 1. Depending on what controls are set (instead of NaN) 1 or 2 + * MAVLink messages are actually sent. + * + * In PX4 v1.9.0 Only first four Control Groups are supported + * (https://github.com/PX4/Firmware/blob/v1.9.0/src/modules/mavlink/mavlink_receiver.cpp#L980). + */ + struct ActuatorControl { + /** + * @brief Eight controls that will be given to the group. Each control is a normalized + * (-1..+1) command value, which will be mapped and scaled through the mixer. + */ + struct Group { + float controls[8]; /**< @brief Controls in the group. */ + }; + Group groups[2]; /**< @brief Control Groups. In order not to send a group, set all its + values to NaN. */ + }; + /** * @brief Start offboard control (synchronous). * @@ -214,6 +242,16 @@ class Offboard : public PluginBase { */ void set_attitude_rate(AttitudeRate attitude_rate); + /** + * @brief Set direct actuator control values to groups #0 and #1. + * First 8 controls will go to control group 0, the following 8 controls to control group 1 (if + * actuator_control.num_controls more than 8). + * + * @param actuator_control actuators control values + */ + + void set_actuator_control(const ActuatorControl actuator_control); + /** * @brief Copy constructor (object is not copyable). */ @@ -228,6 +266,13 @@ class Offboard : public PluginBase { std::unique_ptr _impl; }; +/** + * @brief Equal operator to compare two `Offboard::ActuatorControl` objects. + * + * @return `true` if items are equal. + */ +bool operator==(const Offboard::ActuatorControl& lhs, const Offboard::ActuatorControl& rhs); + /** * @brief Equal operator to compare two `Offboard::Attitude` objects. * @@ -242,6 +287,13 @@ bool operator==(const Offboard::Attitude& lhs, const Offboard::Attitude& rhs); */ bool operator==(const Offboard::AttitudeRate& lhs, const Offboard::AttitudeRate& rhs); +/** + * @brief Stream operator to print information about a `Offboard::ActuatorControl`. + * + * @return A reference to the stream. + */ +std::ostream& operator<<(std::ostream& str, Offboard::ActuatorControl const& actuator_control); + /** * @brief Stream operator to print information about a `Offboard::Attitude`. * diff --git a/src/plugins/offboard/mocks/offboard_mock.h b/src/plugins/offboard/mocks/offboard_mock.h index d520b824a2..ef05bec54e 100644 --- a/src/plugins/offboard/mocks/offboard_mock.h +++ b/src/plugins/offboard/mocks/offboard_mock.h @@ -15,6 +15,7 @@ class MockOffboard { MOCK_CONST_METHOD1(set_position_ned, void(Offboard::PositionNEDYaw)){}; MOCK_CONST_METHOD1(set_velocity_body, void(Offboard::VelocityBodyYawspeed)){}; MOCK_CONST_METHOD1(set_velocity_ned, void(Offboard::VelocityNEDYaw)){}; + MOCK_CONST_METHOD1(set_actuator_control, void(Offboard::ActuatorControl)){}; }; } // namespace testing diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp index 4aacec87e3..1a77187029 100644 --- a/src/plugins/offboard/offboard.cpp +++ b/src/plugins/offboard/offboard.cpp @@ -1,5 +1,6 @@ #include "plugins/offboard/offboard.h" #include "offboard_impl.h" +#include namespace mavsdk { @@ -57,6 +58,11 @@ void Offboard::set_attitude_rate(Offboard::AttitudeRate attitude_rate) return _impl->set_attitude_rate(attitude_rate); } +void Offboard::set_actuator_control(Offboard::ActuatorControl actuator_control) +{ + return _impl->set_actuator_control(actuator_control); +} + const char* Offboard::result_str(Result result) { switch (result) { @@ -80,6 +86,41 @@ const char* Offboard::result_str(Result result) } } +bool operator==(const Offboard::ActuatorControl& lhs, const Offboard::ActuatorControl& rhs) +{ + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 8; j++) { + if ((std::isnan(lhs.groups[i].controls[j]) && std::isnan(rhs.groups[i].controls[j])) || + lhs.groups[i].controls[j] == rhs.groups[i].controls[j]) { + continue; + } + return false; + } + } + + return true; +} + +std::ostream& operator<<(std::ostream& str, Offboard::ActuatorControl const& actuator_control) +{ + return str << "[group: " << 0 << ", Command port 0: " << actuator_control.groups[0].controls[0] + << ", Command port 1: " << actuator_control.groups[0].controls[1] + << ", Command port 2: " << actuator_control.groups[0].controls[2] + << ", Command port 3: " << actuator_control.groups[0].controls[3] + << ", Command port 4: " << actuator_control.groups[0].controls[4] + << ", Command port 5: " << actuator_control.groups[0].controls[5] + << ", Command port 6: " << actuator_control.groups[0].controls[6] + << ", Command port 7: " << actuator_control.groups[0].controls[7] << "], " + << "[group: " << 1 << ", Command port 0: " << actuator_control.groups[1].controls[0] + << ", Command port 1: " << actuator_control.groups[1].controls[1] + << ", Command port 2: " << actuator_control.groups[1].controls[2] + << ", Command port 3: " << actuator_control.groups[1].controls[3] + << ", Command port 4: " << actuator_control.groups[1].controls[4] + << ", Command port 5: " << actuator_control.groups[1].controls[5] + << ", Command port 6: " << actuator_control.groups[1].controls[6] + << ", Command port 7: " << actuator_control.groups[1].controls[7] << "]"; +} + bool operator==(const Offboard::Attitude& lhs, const Offboard::Attitude& rhs) { return lhs.roll_deg == rhs.roll_deg && lhs.pitch_deg == rhs.pitch_deg && diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp index 104ba6420d..bcaf531aaa 100644 --- a/src/plugins/offboard/offboard_impl.cpp +++ b/src/plugins/offboard/offboard_impl.cpp @@ -1,3 +1,4 @@ +#include #include "global_include.h" #include "offboard_impl.h" #include "mavsdk_impl.h" @@ -241,6 +242,33 @@ void OffboardImpl::set_attitude_rate(Offboard::AttitudeRate attitude_rate) send_attitude_rate(); } +void OffboardImpl::set_actuator_control(Offboard::ActuatorControl actuator_control) +{ + _mutex.lock(); + _actuator_control = actuator_control; + + if (_mode != Mode::ACTUATOR_CONTROL) { + if (_call_every_cookie) { + // If we're already sending other setpoints, stop that now. + _parent->remove_call_every(_call_every_cookie); + _call_every_cookie = nullptr; + } + // We automatically send motor rate values from now on. + _parent->add_call_every( + [this]() { send_actuator_control(); }, SEND_INTERVAL_S, &_call_every_cookie); + + _mode = Mode::ACTUATOR_CONTROL; + } else { + // We're already sending these kind of values. Since the value changes, let's + // reschedule the next call, so we don't send values too often. + _parent->reset_call_every(_call_every_cookie); + } + _mutex.unlock(); + + // also send it right now to reduce latency + send_actuator_control(); +} + void OffboardImpl::send_position_ned() { // const static uint16_t IGNORE_X = (1 << 0); @@ -484,6 +512,41 @@ void OffboardImpl::send_attitude_rate() _parent->send_message(message); } +void OffboardImpl::send_actuator_control_message(const float* controls, uint8_t group_number) +{ + mavlink_message_t message; + mavlink_msg_set_actuator_control_target_pack( + _parent->get_own_system_id(), + _parent->get_own_component_id(), + &message, + static_cast(_parent->get_time().elapsed_s() * 1e3), + group_number, + _parent->get_system_id(), + _parent->get_autopilot_id(), + controls); + _parent->send_message(message); +} + +void OffboardImpl::send_actuator_control() +{ + _mutex.lock(); + Offboard::ActuatorControl actuator_control = _actuator_control; + _mutex.unlock(); + + for (int i = 0; i < 2; i++) { + int nan_count = 0; + for (int j = 0; j < 8; j++) { + if (std::isnan(actuator_control.groups[i].controls[j])) { + nan_count++; + actuator_control.groups[i].controls[j] = 0.0f; + } + } + if (nan_count < 8) { + send_actuator_control_message(&actuator_control.groups[i].controls[0], i); + } + } +} + void OffboardImpl::process_heartbeat(const mavlink_message_t& message) { mavlink_heartbeat_t heartbeat; diff --git a/src/plugins/offboard/offboard_impl.h b/src/plugins/offboard/offboard_impl.h index b99e50e6c3..3e16d55835 100644 --- a/src/plugins/offboard/offboard_impl.h +++ b/src/plugins/offboard/offboard_impl.h @@ -33,6 +33,7 @@ class OffboardImpl : public PluginImplBase { void set_velocity_body(Offboard::VelocityBodyYawspeed velocity_body_yawspeed); void set_attitude(Offboard::Attitude attitude); void set_attitude_rate(Offboard::AttitudeRate attitude_rate); + void set_actuator_control(Offboard::ActuatorControl actuator_control); OffboardImpl(const OffboardImpl&) = delete; OffboardImpl& operator=(const OffboardImpl&) = delete; @@ -43,6 +44,8 @@ class OffboardImpl : public PluginImplBase { void send_velocity_body(); void send_attitude_rate(); void send_attitude(); + void send_actuator_control(); + void send_actuator_control_message(const float* controls, uint8_t group_number = 0); void process_heartbeat(const mavlink_message_t& message); void receive_command_result( @@ -59,13 +62,15 @@ class OffboardImpl : public PluginImplBase { VELOCITY_NED, VELOCITY_BODY, ATTITUDE, - ATTITUDE_RATE + ATTITUDE_RATE, + ACTUATOR_CONTROL } _mode = Mode::NOT_ACTIVE; Offboard::PositionNEDYaw _position_ned_yaw{}; Offboard::VelocityNEDYaw _velocity_ned_yaw{}; Offboard::VelocityBodyYawspeed _velocity_body_yawspeed{}; Offboard::Attitude _attitude{}; Offboard::AttitudeRate _attitude_rate{}; + Offboard::ActuatorControl _actuator_control{}; void* _call_every_cookie = nullptr;