Skip to content

Commit

Permalink
Fix motor vel scalings
Browse files Browse the repository at this point in the history
  • Loading branch information
@haitomatic
haitomatic committed Sep 26, 2024
1 parent 7ae6aea commit 7c4bdeb
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 36 deletions.
5 changes: 2 additions & 3 deletions include/gazebo_mavlink_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,14 +152,14 @@ namespace mavlink_interface
void PublishServoVelocities(const Eigen::VectorXd &_vels);
void PublishCmdVelocities(const float _thrust, const float _torque);
void handle_actuator_controls(const gz::sim::UpdateInfo &_info);
void handle_control(double _dt);
void onSigInt();
bool IsRunning();
bool resolveHostName();
void ResolveWorker();
float AddSimpleNoise(float value, float mean, float stddev);
void RotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED,
const gz::math::Quaterniond q_FLU_to_ENU);
void ParseMulticopterMotorModelPlugins(const std::string &sdfFilePath);

static const unsigned n_out_max = 16;

Expand All @@ -169,6 +169,7 @@ namespace mavlink_interface
double zero_position_disarmed_[n_out_max];
double zero_position_armed_[n_out_max];
int motor_input_index_[n_out_max];
double motor_vel_scalings_[n_out_max] {1.0};
int servo_input_index_[n_out_max];
bool input_is_cmd_vel_{false};

Expand All @@ -186,8 +187,6 @@ namespace mavlink_interface
std::string mag_sub_topic_{kDefaultMagTopic};
std::string baro_sub_topic_{kDefaultBarometerTopic};
std::string cmd_vel_sub_topic_{kDefaultCmdVelTopic};
int mc_motor_vel_scaling_{1000};
int fw_motor_vel_scaling_{3500};

std::mutex last_imu_message_mutex_ {};

Expand Down
90 changes: 57 additions & 33 deletions src/gazebo_mavlink_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,11 +41,11 @@ GZ_ADD_PLUGIN(
using namespace mavlink_interface;

GazeboMavlinkInterface::GazeboMavlinkInterface() :
motor_input_index_ {},
servo_input_index_ {}
{
mavlink_interface_ = std::make_shared<MavlinkInterface>();
}
motor_input_index_ {},
servo_input_index_ {}
{
mavlink_interface_ = std::make_shared<MavlinkInterface>();
}

GazeboMavlinkInterface::~GazeboMavlinkInterface() {
mavlink_interface_->close();
Expand All @@ -71,7 +71,6 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
protocol_version_ = _sdf->Get<float>("protocol_version");
}


gazebo::getSdfParam<std::string>(_sdf, "poseSubTopic", pose_sub_topic_, pose_sub_topic_);
gazebo::getSdfParam<std::string>(_sdf, "gpsSubTopic", gps_sub_topic_, gps_sub_topic_);
gazebo::getSdfParam<std::string>(_sdf, "visionSubTopic", vision_sub_topic_, vision_sub_topic_);
Expand All @@ -81,13 +80,14 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
gazebo::getSdfParam<std::string>(_sdf, "magSubTopic", mag_sub_topic_, mag_sub_topic_);
gazebo::getSdfParam<std::string>(_sdf, "cmdVelSubTopic", cmd_vel_sub_topic_, cmd_vel_sub_topic_);
gazebo::getSdfParam<std::string>(_sdf, "baroSubTopic", baro_sub_topic_, baro_sub_topic_);
gazebo::getSdfParam<int>(_sdf, "mc_motor_vel_scaling", mc_motor_vel_scaling_, mc_motor_vel_scaling_);
gazebo::getSdfParam<int>(_sdf, "fw_motor_vel_scaling", fw_motor_vel_scaling_, fw_motor_vel_scaling_);

// set motor and servo input_reference_ from inputs.control
// Set motor and servo input_reference_ from inputs.control
motor_input_reference_.resize(n_out_max);
servo_input_reference_.resize(n_out_max);

// Parse the MulticopterMotorModel plugins to get the motor velocity scalings
ParseMulticopterMotorModelPlugins(model_.SourceFilePath(_ecm));

bool use_tcp = false;
if (_sdf->HasElement("use_tcp"))
{
Expand Down Expand Up @@ -127,7 +127,7 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,
gzmsg << "Speed factor set to: " << speed_factor_ << std::endl;
}

// // Listen to Ctrl+C / SIGINT.
// Listen to Ctrl+C / SIGINT.
sigIntConnection_ = _em.Connect<gz::sim::events::Stop>(std::bind(&GazeboMavlinkInterface::onSigInt, this));

auto world_name = "/" + gz::sim::scopedName(gz::sim::worldEntity(_ecm), _ecm);
Expand Down Expand Up @@ -248,8 +248,6 @@ void GazeboMavlinkInterface::PreUpdate(const gz::sim::UpdateInfo &_info,

handle_actuator_controls(_info);

handle_control(dt);

if (received_first_actuator_) {
if (input_is_cmd_vel_) {
PublishCmdVelocities(cmd_vel_thrust_, cmd_vel_torque_);
Expand Down Expand Up @@ -478,19 +476,7 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo

for (int i = 0; i < motor_input_reference_.size(); i++) {
if (armed) {
if (i == motor_input_reference_.size() - 1) {
// Set pusher motor velocity scaling if airframe is either fw or vtol. This assumes the pusher motor is the last motor!
double scaling;
if (servo_input_reference_.size() > 0 && servo_input_reference_.size() != n_out_max) {
scaling = static_cast<double>(fw_motor_vel_scaling_);
} else {
scaling = static_cast<double>(mc_motor_vel_scaling_);
}
motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * scaling;
}
else {
motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * static_cast<double>(mc_motor_vel_scaling_);
}
motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * motor_vel_scalings_[i];
} else {
motor_input_reference_[i] = 0;
}
Expand All @@ -499,13 +485,6 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo
received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator();
}

void GazeboMavlinkInterface::handle_control(double _dt)
{
// set joint positions
// for (int i = 0; i < motor_input_reference_.size(); i++) {
// }
}

bool GazeboMavlinkInterface::IsRunning()
{
return true; //TODO;
Expand Down Expand Up @@ -627,4 +606,49 @@ void GazeboMavlinkInterface::RotateQuaternion(gz::math::Quaterniond &q_FRD_to_NE

// final rotation composition
q_FRD_to_NED = q_ENU_to_NED * q_FLU_to_ENU * q_FLU_to_FRD.Inverse();
}
}

void GazeboMavlinkInterface::ParseMulticopterMotorModelPlugins(const std::string &sdfFilePath)
{
// Load the SDF file
sdf::Root root;
sdf::Errors errors = root.Load(sdfFilePath);
if (!errors.empty())
{
for (const auto &error : errors)
{
gzerr << "[gazebo_mavlink_interface] Error: " << error.Message() << std::endl;
}
return;
}

// Load the model
const sdf::Model *model = root.Model();
if (!model)
{
gzerr << "[gazebo_mavlink_interface] No models found in SDF file." << std::endl;
return;
}

// Iterate through all plugins in the model
for (const sdf::Plugin plugin : model->Plugins())
{
// Check if the plugin is a MulticopterMotorModel
if (plugin.Name() == "gz::sim::systems::MulticopterMotorModel") {
if (plugin.Element()->HasElement("motorNumber"))
{
const int motorNumber = plugin.Element()->Get<int>("motorNumber");
if (motorNumber >= n_out_max)
{
gzerr << "[gazebo_mavlink_interface] Motor number " << motorNumber
<< " exceeds maximum number of motors " << n_out_max << std::endl;
continue;
}
if (plugin.Element()->HasElement("motorNumber"))
{
motor_vel_scalings_[motorNumber] = plugin.Element()->Get<double>("maxRotVelocity");
}
}
}
}
}

0 comments on commit 7c4bdeb

Please sign in to comment.