dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

simulator/gz_bridge: split actuator outputs for ESCs and servos (#20979) 

- existing SIM_GZ outputs -> SIM_GZ_EC (ESCs)
 - new SIM_GZ_SV for servos (not fully implemented)
This commit is contained in:
Daniel Agar 2023-01-21 14:44:09 -05:00 committed by GitHub
parent 9ac6b3d3c5
commit 3f842f01a0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 488 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500
View File

@ -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

26
ROMFS/px4fmu_common/init.d-posix/airframes/4002_gz_x500_depth
View File

@ -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

2
ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
View File

@ -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

4
src/modules/simulation/gz_bridge/CMakeLists.txt
View File

@ -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

125
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

@ -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<ignition::msgs::Actuators>(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;
}

22
src/modules/simulation/gz_bridge/GZBridge.hpp
View File

@ -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 <px4_platform_common/atomic.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
@ -40,11 +43,9 @@
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/geo/geo.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
@ -60,7 +61,7 @@
using namespace time_literals;
class GZBridge : public ModuleBase<GZBridge>, public OutputModuleInterface
class GZBridge : public ModuleBase<GZBridge>, 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_s> _esc_status_pub{ORB_ID(esc_status)};
uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
uORB::Publication<vehicle_attitude_s> _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
uORB::Publication<vehicle_global_position_s> _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
@ -108,9 +104,12 @@ private:
uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
uORB::PublicationMulti<sensor_gyro_s> _sensor_gyro_pub{ORB_ID(sensor_gyro)};
GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex};
GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};
px4::atomic<uint64_t> _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<px4::params::SIM_GZ_HOME_LAT>) _param_sim_home_lat,

126
src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp Normal file
View File

@ -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<ignition::msgs::Actuators>(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);
}

87
src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp Normal file
View File

@ -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 <lib/mixer_module/mixer_module.hpp>
#include <ignition/transport.hh>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/esc_status.h>
// 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_s> _esc_status_pub{ORB_ID(esc_status)};
};

97
src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp Normal file
View File

@ -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<ignition::msgs::Actuators>(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);
}

76
src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp Normal file
View File

@ -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 <lib/mixer_module/mixer_module.hpp>
#include <ignition/transport.hh>
// 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;
};

27
src/modules/simulation/gz_bridge/module.yaml
View File

@ -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

9
src/modules/simulation/gz_bridge/parameters.c
View File

@ -31,6 +31,15 @@
*
****************************************************************************/
/**
* Simulator Gazebo bridge enable
*
* @boolean
* @reboot_required true
* @group UAVCAN
*/
PARAM_DEFINE_INT32(SIM_GZ_EN, 0);
/**
* simulator origin latitude
*