diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index 103f8dd7994..cb7e267fc03 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -11,6 +11,8 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz} PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} +param set-default SIM_GZ_EN 1 + param set-default CA_AIRFRAME 0 param set-default CA_ROTOR_COUNT 4 @@ -30,19 +32,19 @@ param set-default CA_ROTOR3_PX -0.13 param set-default CA_ROTOR3_PY 0.20 param set-default CA_ROTOR3_KM -0.05 -param set-default SIM_GZ_FUNC1 101 -param set-default SIM_GZ_FUNC2 102 -param set-default SIM_GZ_FUNC3 103 -param set-default SIM_GZ_FUNC4 104 +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 -param set-default SIM_GZ_MIN1 150 -param set-default SIM_GZ_MIN2 150 -param set-default SIM_GZ_MIN3 150 -param set-default SIM_GZ_MIN4 150 +param set-default SIM_GZ_EC_MIN1 150 +param set-default SIM_GZ_EC_MIN2 150 +param set-default SIM_GZ_EC_MIN3 150 +param set-default SIM_GZ_EC_MIN4 150 -param set-default SIM_GZ_MAX1 1000 -param set-default SIM_GZ_MAX2 1000 -param set-default SIM_GZ_MAX3 1000 -param set-default SIM_GZ_MAX4 1000 +param set-default SIM_GZ_EC_MAX1 1000 +param set-default SIM_GZ_EC_MAX2 1000 +param set-default SIM_GZ_EC_MAX3 1000 +param set-default SIM_GZ_EC_MAX4 1000 param set-default MPC_THR_HOVER 0.60 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4002_gz_x500_depth b/ROMFS/px4fmu_common/init.d-posix/airframes/4002_gz_x500_depth index 802ea75c447..f36b40faa5f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4002_gz_x500_depth +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4002_gz_x500_depth @@ -11,6 +11,8 @@ PX4_SIMULATOR=${PX4_SIMULATOR:=gz} PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_depth} +param set-default SIM_GZ_EN 1 + param set-default CA_AIRFRAME 0 param set-default CA_ROTOR_COUNT 4 @@ -30,19 +32,19 @@ param set-default CA_ROTOR3_PX -0.13 param set-default CA_ROTOR3_PY 0.20 param set-default CA_ROTOR3_KM -0.05 -param set-default SIM_GZ_FUNC1 101 -param set-default SIM_GZ_FUNC2 102 -param set-default SIM_GZ_FUNC3 103 -param set-default SIM_GZ_FUNC4 104 +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 -param set-default SIM_GZ_MIN1 150 -param set-default SIM_GZ_MIN2 150 -param set-default SIM_GZ_MIN3 150 -param set-default SIM_GZ_MIN4 150 +param set-default SIM_GZ_EC_MIN1 150 +param set-default SIM_GZ_EC_MIN2 150 +param set-default SIM_GZ_EC_MIN3 150 +param set-default SIM_GZ_EC_MIN4 150 -param set-default SIM_GZ_MAX1 1000 -param set-default SIM_GZ_MAX2 1000 -param set-default SIM_GZ_MAX3 1000 -param set-default SIM_GZ_MAX4 1000 +param set-default SIM_GZ_EC_MAX1 1000 +param set-default SIM_GZ_EC_MAX2 1000 +param set-default SIM_GZ_EC_MAX3 1000 +param set-default SIM_GZ_EC_MAX4 1000 param set-default MPC_THR_HOVER 0.60 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 68810a980b2..e8719549403 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -19,7 +19,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" exit 1 fi -elif [ "$PX4_SIMULATOR" = "gz" ]; then +elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then # source generated gz_env.sh for IGN_GAZEBO_RESOURCE_PATH if [ -f ./gz_env.sh ]; then diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index a4c2af7904f..1524166f457 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -62,6 +62,10 @@ if(gz-transport_FOUND) SRCS GZBridge.cpp GZBridge.hpp + GZMixingInterfaceESC.cpp + GZMixingInterfaceESC.hpp + GZMixingInterfaceServo.cpp + GZMixingInterfaceServo.hpp DEPENDS mixer_module px4_work_queue diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index e21554bd42c..765c051e2a1 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,13 +44,14 @@ GZBridge::GZBridge(const char *world, const char *name, const char *model, const char *pose_str) : - OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _world_name(world), _model_name(name), _model_sim(model), _model_pose(pose_str) { - pthread_mutex_init(&_mutex, nullptr); + pthread_mutex_init(&_node_mutex, nullptr); updateParams(); } @@ -151,25 +152,13 @@ int GZBridge::init() return PX4_ERROR; } - // ESC feedback: /x500/command/motor_speed - std::string motor_speed_topic = "/" + _model_name + "/command/motor_speed"; - - if (!_node.Subscribe(motor_speed_topic, &GZBridge::motorSpeedCallback, this)) { - PX4_ERR("failed to subscribe to %s", motor_speed_topic.c_str()); + if (!_mixing_interface_esc.init(_model_name)) { + PX4_ERR("failed to init ESC output"); return PX4_ERROR; } - // list all subscriptions - for (auto &sub_topic : _node.SubscribedTopics()) { - PX4_INFO("subscribed: %s", sub_topic.c_str()); - } - - // output eg /X500/command/motor_speed - std::string actuator_topic = "/" + _model_name + "/command/motor_speed"; - _actuators_pub = _node.Advertise(actuator_topic); - - if (!_actuators_pub.Valid()) { - PX4_ERR("failed to advertise %s", actuator_topic.c_str()); + if (!_mixing_interface_servo.init(_model_name)) { + PX4_ERR("failed to init servo output"); return PX4_ERROR; } @@ -317,7 +306,7 @@ bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec) void GZBridge::clockCallback(const ignition::msgs::Clock &clock) { - pthread_mutex_lock(&_mutex); + pthread_mutex_lock(&_node_mutex); const uint64_t time_us = (clock.sim().sec() * 1000000) + (clock.sim().nsec() / 1000); @@ -325,7 +314,7 @@ void GZBridge::clockCallback(const ignition::msgs::Clock &clock) updateClock(clock.sim().sec(), clock.sim().nsec()); } - pthread_mutex_unlock(&_mutex); + pthread_mutex_unlock(&_node_mutex); } void GZBridge::imuCallback(const ignition::msgs::IMU &imu) @@ -334,7 +323,7 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu) return; } - pthread_mutex_lock(&_mutex); + pthread_mutex_lock(&_node_mutex); const uint64_t time_us = (imu.header().stamp().sec() * 1000000) + (imu.header().stamp().nsec() / 1000); @@ -390,7 +379,7 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu) sensor_gyro.samples = 1; _sensor_gyro_pub.publish(sensor_gyro); - pthread_mutex_unlock(&_mutex); + pthread_mutex_unlock(&_node_mutex); } void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose) @@ -399,7 +388,7 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose) return; } - pthread_mutex_lock(&_mutex); + pthread_mutex_lock(&_node_mutex); for (int p = 0; p < pose.pose_size(); p++) { if (pose.pose(p).name() == _model_name) { @@ -519,106 +508,50 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose) _gpos_ground_truth_pub.publish(global_position_groundtruth); } - pthread_mutex_unlock(&_mutex); + pthread_mutex_unlock(&_node_mutex); return; } } - pthread_mutex_unlock(&_mutex); -} - -void GZBridge::motorSpeedCallback(const ignition::msgs::Actuators &actuators) -{ - if (hrt_absolute_time() == 0) { - return; - } - - pthread_mutex_lock(&_mutex); - - esc_status_s esc_status{}; - esc_status.esc_count = actuators.velocity_size(); - - for (int i = 0; i < actuators.velocity_size(); i++) { - esc_status.esc[i].timestamp = hrt_absolute_time(); - esc_status.esc[i].esc_rpm = actuators.velocity(i); - esc_status.esc_online_flags |= 1 << i; - - if (actuators.velocity(i) > 0) { - esc_status.esc_armed_flags |= 1 << i; - } - } - - if (esc_status.esc_count > 0) { - esc_status.timestamp = hrt_absolute_time(); - _esc_status_pub.publish(esc_status); - } - - pthread_mutex_unlock(&_mutex); -} - -bool GZBridge::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, - unsigned num_control_groups_updated) -{ - unsigned active_output_count = 0; - - for (unsigned i = 0; i < num_outputs; i++) { - if (_mixing_output.isFunctionSet(i)) { - active_output_count++; - - } else { - break; - } - } - - if (active_output_count > 0) { - ignition::msgs::Actuators rotor_velocity_message; - rotor_velocity_message.mutable_velocity()->Resize(active_output_count, 0); - - for (unsigned i = 0; i < active_output_count; i++) { - rotor_velocity_message.set_velocity(i, outputs[i]); - } - - if (_actuators_pub.Valid()) { - return _actuators_pub.Publish(rotor_velocity_message); - } - } - - return false; + pthread_mutex_unlock(&_node_mutex); } void GZBridge::Run() { if (should_exit()) { ScheduleClear(); - _mixing_output.unregister(); + + _mixing_interface_esc.stop(); + _mixing_interface_servo.stop(); exit_and_cleanup(); return; } - pthread_mutex_lock(&_mutex); + pthread_mutex_lock(&_node_mutex); if (_parameter_update_sub.updated()) { parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); updateParams(); + + _mixing_interface_esc.updateParams(); + _mixing_interface_servo.updateParams(); } - _mixing_output.update(); + ScheduleDelayed(10_ms); - //ScheduleDelayed(1000_us); - - // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) - _mixing_output.updateSubscriptions(true); - - pthread_mutex_unlock(&_mutex); + pthread_mutex_unlock(&_node_mutex); } int GZBridge::print_status() { - //perf_print_counter(_cycle_perf); - _mixing_output.printStatus(); + PX4_INFO_RAW("ESC outputs:\n"); + _mixing_interface_esc.mixingOutput().printStatus(); + + PX4_INFO_RAW("Servo outputs:\n"); + _mixing_interface_servo.mixingOutput().printStatus(); return 0; } diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index e25cfaf0446..b63d6447aa5 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,6 +33,9 @@ #pragma once +#include "GZMixingInterfaceESC.hpp" +#include "GZMixingInterfaceServo.hpp" + #include #include #include @@ -40,11 +43,9 @@ #include #include #include -#include #include #include #include -#include #include #include #include @@ -60,7 +61,7 @@ using namespace time_literals; -class GZBridge : public ModuleBase, public OutputModuleInterface +class GZBridge : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: GZBridge(const char *world, const char *name, const char *model, const char *pose_str); @@ -84,9 +85,6 @@ public: private: - bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; - void Run() override; bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec); @@ -94,12 +92,10 @@ private: void clockCallback(const ignition::msgs::Clock &clock); void imuCallback(const ignition::msgs::IMU &imu); void poseInfoCallback(const ignition::msgs::Pose_V &pose); - void motorSpeedCallback(const ignition::msgs::Actuators &actuators); // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; @@ -108,9 +104,12 @@ private: uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; + GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; + px4::atomic _world_time_us{0}; - pthread_mutex_t _mutex; + pthread_mutex_t _node_mutex; MapProjection _pos_ref{}; @@ -124,10 +123,7 @@ private: const std::string _model_sim; const std::string _model_pose; - MixingOutput _mixing_output{"SIM_GZ", 8, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; - ignition::transport::Node _node; - ignition::transport::Node::Publisher _actuators_pub; DEFINE_PARAMETERS( (ParamFloat) _param_sim_home_lat, diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp new file mode 100644 index 00000000000..9adbbbdcac6 --- /dev/null +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "GZMixingInterfaceESC.hpp" + +bool GZMixingInterfaceESC::init(const std::string &model_name) +{ + + // ESC feedback: /x500/command/motor_speed + std::string motor_speed_topic = "/" + model_name + "/command/motor_speed"; + + if (!_node.Subscribe(motor_speed_topic, &GZMixingInterfaceESC::motorSpeedCallback, this)) { + PX4_ERR("failed to subscribe to %s", motor_speed_topic.c_str()); + return false; + } + + // output eg /X500/command/motor_speed + std::string actuator_topic = "/" + model_name + "/command/motor_speed"; + _actuators_pub = _node.Advertise(actuator_topic); + + if (!_actuators_pub.Valid()) { + PX4_ERR("failed to advertise %s", actuator_topic.c_str()); + return false; + } + + ScheduleNow(); + + return true; +} + +bool GZMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) +{ + unsigned active_output_count = 0; + + for (unsigned i = 0; i < num_outputs; i++) { + if (_mixing_output.isFunctionSet(i)) { + active_output_count++; + + } else { + break; + } + } + + if (active_output_count > 0) { + ignition::msgs::Actuators rotor_velocity_message; + rotor_velocity_message.mutable_velocity()->Resize(active_output_count, 0); + + for (unsigned i = 0; i < active_output_count; i++) { + rotor_velocity_message.set_velocity(i, outputs[i]); + } + + if (_actuators_pub.Valid()) { + return _actuators_pub.Publish(rotor_velocity_message); + } + } + + return false; +} + +void GZMixingInterfaceESC::Run() +{ + pthread_mutex_lock(&_node_mutex); + _mixing_output.update(); + _mixing_output.updateSubscriptions(false); + pthread_mutex_unlock(&_node_mutex); +} + +void GZMixingInterfaceESC::motorSpeedCallback(const ignition::msgs::Actuators &actuators) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_node_mutex); + + esc_status_s esc_status{}; + esc_status.esc_count = actuators.velocity_size(); + + for (int i = 0; i < actuators.velocity_size(); i++) { + esc_status.esc[i].timestamp = hrt_absolute_time(); + esc_status.esc[i].esc_rpm = actuators.velocity(i); + esc_status.esc_online_flags |= 1 << i; + + if (actuators.velocity(i) > 0) { + esc_status.esc_armed_flags |= 1 << i; + } + } + + if (esc_status.esc_count > 0) { + esc_status.timestamp = hrt_absolute_time(); + _esc_status_pub.publish(esc_status); + } + + pthread_mutex_unlock(&_node_mutex); +} diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp new file mode 100644 index 00000000000..ab88f1867f3 --- /dev/null +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +#include +#include + + +// GZBridge mixing class for ESCs. +// It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling +// All work items are expected to run on the same work queue. +class GZMixingInterfaceESC : public OutputModuleInterface +{ +public: + static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; + + GZMixingInterfaceESC(ignition::transport::Node &node, pthread_mutex_t &node_mutex) : + OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::rate_ctrl), + _node(node), + _node_mutex(node_mutex) + {} + + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + MixingOutput &mixingOutput() { return _mixing_output; } + + bool init(const std::string &model_name); + + void stop() + { + _mixing_output.unregister(); + ScheduleClear(); + } + +private: + friend class GZBridge; + + void Run() override; + + void motorSpeedCallback(const ignition::msgs::Actuators &actuators); + + ignition::transport::Node &_node; + pthread_mutex_t &_node_mutex; + + MixingOutput _mixing_output{"SIM_GZ_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + + ignition::transport::Node::Publisher _actuators_pub; + + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; + +}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp new file mode 100644 index 00000000000..783bd460fb4 --- /dev/null +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "GZMixingInterfaceServo.hpp" + +bool GZMixingInterfaceServo::init(const std::string &model_name) +{ +#if 0 // TODO + // output eg /X500/command/motor_speed + std::string actuator_topic = "/" + model_name + "/command/motor_speed"; + _actuators_pub = _node.Advertise(actuator_topic); + + if (!_actuators_pub.Valid()) { + PX4_ERR("failed to advertise %s", actuator_topic.c_str()); + return false; + } + +#endif + + ScheduleNow(); + + return true; +} + +bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) +{ + + // cmd.command_value = (float)outputs[i] / 500.f - 1.f; // [-1, 1] + +#if 0 // TODO + unsigned active_output_count = 0; + + for (unsigned i = 0; i < num_outputs; i++) { + if (_mixing_output.isFunctionSet(i)) { + active_output_count++; + + } else { + break; + } + } + + if (active_output_count > 0) { + ignition::msgs::Actuators rotor_velocity_message; + rotor_velocity_message.mutable_velocity()->Resize(active_output_count, 0); + + for (unsigned i = 0; i < active_output_count; i++) { + rotor_velocity_message.set_velocity(i, outputs[i]); + } + + if (_actuators_pub.Valid()) { + return _actuators_pub.Publish(rotor_velocity_message); + } + } + +#endif + + return false; +} + +void GZMixingInterfaceServo::Run() +{ + pthread_mutex_lock(&_node_mutex); + _mixing_output.update(); + _mixing_output.updateSubscriptions(false); + pthread_mutex_unlock(&_node_mutex); +} diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp new file mode 100644 index 00000000000..d9e59a0222e --- /dev/null +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +// GZBridge mixing class for Servos. +// It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling +// All work items are expected to run on the same work queue. +class GZMixingInterfaceServo : public OutputModuleInterface +{ +public: + GZMixingInterfaceServo(ignition::transport::Node &node, pthread_mutex_t &node_mutex) : + OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::rate_ctrl), + _node(node), + _node_mutex(node_mutex) + {} + + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + MixingOutput &mixingOutput() { return _mixing_output; } + + bool init(const std::string &model_name); + + void stop() + { + _mixing_output.unregister(); + ScheduleClear(); + } + +private: + friend class GZBridge; + + void Run() override; + + ignition::transport::Node &_node; + pthread_mutex_t &_node_mutex; + + MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + + //ignition::transport::Node::Publisher _actuators_pub; +}; diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index 674a4ed3f92..d4258bd0e99 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -1,10 +1,27 @@ module_name: SIM_GZ actuator_output: + show_subgroups_if: 'SIM_GZ_EN>=1' + config_parameters: + - param: 'SIM_GZ_EN' + label: 'Configure' + function: 'enable' + output_groups: - - param_prefix: SIM_GZ - channel_label: Channel - num_channels: 8 + - param_prefix: SIM_GZ_EC + group_label: 'ESCs' + channel_label: 'ESC' standard_params: disarmed: { min: 0, max: 1000, default: 0 } - min: { min: 0, max: 1000, default: 10 } - max: { min: 100, max: 1000, default: 1000 } + min: { min: 0, max: 1000, default: 0 } + max: { min: 0, max: 1000, default: 1000 } + failsafe: { min: 0, max: 1000 } + num_channels: 8 + - param_prefix: SIM_GZ_SV + group_label: 'Servos' + channel_label: 'Servo' + standard_params: + disarmed: { min: 0, max: 1000, default: 500 } + min: { min: 0, max: 1000, default: 0 } + max: { min: 0, max: 1000, default: 1000 } + failsafe: { min: 0, max: 1000 } + num_channels: 8 diff --git a/src/modules/simulation/gz_bridge/parameters.c b/src/modules/simulation/gz_bridge/parameters.c index cc45872f07a..513c67436ce 100644 --- a/src/modules/simulation/gz_bridge/parameters.c +++ b/src/modules/simulation/gz_bridge/parameters.c @@ -31,6 +31,15 @@ * ****************************************************************************/ +/** + * Simulator Gazebo bridge enable + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(SIM_GZ_EN, 0); + /** * simulator origin latitude *