Skip to content

Commit

Permalink
Add support for all motors and servos control for all UAV airframes
Browse files Browse the repository at this point in the history
  • Loading branch information
haitomatic committed Aug 19, 2024
1 parent f2da227 commit a22d53e
Show file tree
Hide file tree
Showing 4 changed files with 81 additions and 41 deletions.
13 changes: 8 additions & 5 deletions include/gazebo_mavlink_interface.h
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,8 @@ namespace mavlink_interface
gz::common::ConnectionPtr sigIntConnection_;
std::shared_ptr<MavlinkInterface> mavlink_interface_;
bool received_first_actuator_{false};
Eigen::VectorXd input_reference_;
Eigen::VectorXd motor_input_reference_;
Eigen::VectorXd servo_input_reference_;

gz::sim::Entity entity_{gz::sim::kNullEntity};
gz::sim::Model model_{gz::sim::kNullEntity};
Expand All @@ -143,8 +144,9 @@ namespace mavlink_interface
void MagnetometerCallback(const gz::msgs::Magnetometer &_msg);
void GpsCallback(const gz::msgs::NavSat &_msg);
void SendSensorMessages(const gz::sim::UpdateInfo &_info);
void PublishRotorVelocities(gz::sim::EntityComponentManager &_ecm,
void PublishMotorVelocities(gz::sim::EntityComponentManager &_ecm,
const Eigen::VectorXd &_vels);
void PublishServoVelocities(const Eigen::VectorXd &_vels);
void handle_actuator_controls(const gz::sim::UpdateInfo &_info);
void handle_control(double _dt);
void onSigInt();
Expand All @@ -158,15 +160,16 @@ namespace mavlink_interface
static const unsigned n_out_max = 16;

double input_offset_[n_out_max];
Eigen::VectorXd input_scaling_;
std::string joint_control_type_[n_out_max];
std::string gztopic_[n_out_max];
double zero_position_disarmed_[n_out_max];
double zero_position_armed_[n_out_max];
int input_index_[n_out_max];
int motor_input_index_[n_out_max];
int servo_input_index_[n_out_max];

/// \brief gz communication node.
gz::transport::Node node;
gz::transport::Node::Publisher servo_control_pub_[n_out_max];

std::string pose_sub_topic_{kDefaultPoseTopic};
std::string imu_sub_topic_{kDefaultImuTopic};
Expand All @@ -180,7 +183,7 @@ namespace mavlink_interface
std::mutex last_imu_message_mutex_ {};

gz::msgs::IMU last_imu_message_;
gz::msgs::Actuators rotor_velocity_message_;
gz::msgs::Actuators motor_velocity_message_;

std::chrono::steady_clock::duration last_imu_time_{0};
std::chrono::steady_clock::duration lastControllerUpdateTime{0};
Expand Down
3 changes: 2 additions & 1 deletion include/mavlink_interface.h
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,7 @@ class MavlinkInterface {
void UpdateIMU(const SensorData::Imu &data);
void UpdateMag(const SensorData::Magnetometer &data);
Eigen::VectorXd GetActuatorControls();
bool IsInputMotorAtIndex(int index);
bool GetArmedState();
void onSigInt();
uint16_t FinalizeOutgoingMessage(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
Expand Down Expand Up @@ -186,7 +187,7 @@ class MavlinkInterface {

static const unsigned n_out_max = 16;

int input_index_[n_out_max];
bool input_is_motor_[n_out_max];

struct sockaddr_in local_simulator_addr_;
socklen_t local_simulator_addr_len_;
Expand Down
94 changes: 64 additions & 30 deletions src/gazebo_mavlink_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include <gz/sim/components/Magnetometer.hh>
#include <gz/sim/components/Imu.hh>
#include <gz/sim/components/Pose.hh>
#include <gz/transport/Discovery.hh>

GZ_ADD_PLUGIN(
mavlink_interface::GazeboMavlinkInterface,
Expand All @@ -40,8 +41,8 @@ GZ_ADD_PLUGIN(
using namespace mavlink_interface;

GazeboMavlinkInterface::GazeboMavlinkInterface() :
input_scaling_ {},
input_index_ {}
motor_input_index_ {},
servo_input_index_ {}
{
mavlink_interface_ = std::make_shared<MavlinkInterface>();
}
Expand Down Expand Up @@ -80,15 +81,9 @@ 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, "baroSubTopic", baro_sub_topic_, baro_sub_topic_);

// set input_reference_ from inputs.control
input_reference_.resize(n_out_max);

///TODO: Parse input reference
input_scaling_.resize(n_out_max);
input_scaling_(0) = 1000;
input_scaling_(1) = 1000;
input_scaling_(2) = 1000;
input_scaling_(3) = 1000;
// set motor and servo input_reference_ from inputs.control
motor_input_reference_.resize(n_out_max);
servo_input_reference_.resize(n_out_max);

bool use_tcp = false;
if (_sdf->HasElement("use_tcp"))
Expand Down Expand Up @@ -134,9 +129,17 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity,

auto world_name = "/" + gz::sim::scopedName(gz::sim::worldEntity(_ecm), _ecm);

auto vehicle_scope_prefix = world_name + gz::sim::topicFromScopedName(
auto model_name = gz::sim::topicFromScopedName(
_ecm.EntityByComponents(gz::sim::components::Name(model_name_)), _ecm, false);

auto vehicle_scope_prefix = world_name + model_name;

// Publish to servo control
auto servo_control_topic = model_name + "/servo_";
for (int i = 0; i < servo_input_reference_.size(); i++) {
servo_control_pub_[i] = node.Advertise<gz::msgs::Double>(servo_control_topic + std::to_string(i));
}

// Subscribe to messages of sensors.
auto imu_topic = vehicle_scope_prefix + imu_sub_topic_;
node.Subscribe(imu_topic, &GazeboMavlinkInterface::ImuCallback, this);
Expand Down Expand Up @@ -241,7 +244,8 @@ void GazeboMavlinkInterface::PreUpdate(const gz::sim::UpdateInfo &_info,
handle_control(dt);

if (received_first_actuator_) {
PublishRotorVelocities(_ecm, input_reference_);
PublishMotorVelocities(_ecm, motor_input_reference_);
PublishServoVelocities(servo_input_reference_);
}
}

Expand Down Expand Up @@ -417,30 +421,50 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo

last_actuator_time_ = _info.simTime;

for (unsigned i = 0; i < n_out_max; i++) {
input_index_[i] = i;
}
// Read Input References
input_reference_.resize(n_out_max);

Eigen::VectorXd actuator_controls = mavlink_interface_->GetActuatorControls();
if (actuator_controls.size() < n_out_max) return; //TODO: Handle this properly

for (int i = 0; i < input_reference_.size(); i++) {
// Read Input References for motors
unsigned n_motors = 0;
for (unsigned i = 0; i < n_out_max; i++) {
if (mavlink_interface_->IsInputMotorAtIndex(i)) {
motor_input_index_[n_motors++] = i;
}
}
motor_input_reference_.resize(n_motors);
for (int i = 0; i < motor_input_reference_.size(); i++) {
if (armed) {
input_reference_[i] = actuator_controls[input_index_[i]] * input_scaling_(i);
motor_input_reference_[i] = actuator_controls[motor_input_index_[i]] * 1000;
} else {
input_reference_[i] = 0;
// std::cout << input_reference_ << ", ";
motor_input_reference_[i] = 0;
// std::cout << motor_input_reference_ << ", ";
}
}

// Read Input References for servos
unsigned n_servos = 0;
for (unsigned i = 0; i < n_out_max; i++) {
if (!mavlink_interface_->IsInputMotorAtIndex(i)) {
servo_input_index_[n_servos++] = i;
}
}
servo_input_reference_.resize(n_servos);
for (int i = 0; i < servo_input_reference_.size(); i++) {
if (armed) {
servo_input_reference_[i] = actuator_controls[servo_input_index_[i]];
} else {
servo_input_reference_[i] = 0;
// std::cout << servo_input_reference_ << ", ";
}
}

received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator();
}

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

Expand All @@ -454,17 +478,17 @@ void GazeboMavlinkInterface::onSigInt() {
}

// The following snippet was copied from https://github.com/gzrobotics/ign-gazebo/blob/ign-gazebo4/src/systems/multicopter_control/MulticopterVelocityControl.cc
void GazeboMavlinkInterface::PublishRotorVelocities(
void GazeboMavlinkInterface::PublishMotorVelocities(
gz::sim::EntityComponentManager &_ecm,
const Eigen::VectorXd &_vels)
{
if (_vels.size() != rotor_velocity_message_.velocity_size())
if (_vels.size() != motor_velocity_message_.velocity_size())
{
rotor_velocity_message_.mutable_velocity()->Resize(_vels.size(), 0);
motor_velocity_message_.mutable_velocity()->Resize(_vels.size(), 0);
}
for (int i = 0; i < _vels.size(); ++i)
{
rotor_velocity_message_.set_velocity(i, _vels(i));
motor_velocity_message_.set_velocity(i, _vels(i));
}
// Publish the message by setting the Actuators component on the model entity.
// This assumes that the MulticopterMotorModel system is attached to this
Expand All @@ -479,15 +503,25 @@ void GazeboMavlinkInterface::PublishRotorVelocities(
return std::equal(_a.velocity().begin(), _a.velocity().end(),
_b.velocity().begin());
};
auto state = actuatorMsgComp->SetData(this->rotor_velocity_message_, compFunc)
auto state = actuatorMsgComp->SetData(this->motor_velocity_message_, compFunc)
? gz::sim::ComponentState::PeriodicChange
: gz::sim::ComponentState::NoChange;
_ecm.SetChanged(model_.Entity(), gz::sim::components::Actuators::typeId, state);
}
else
{
_ecm.CreateComponent(model_.Entity(),
gz::sim::components::Actuators(this->rotor_velocity_message_));
gz::sim::components::Actuators(this->motor_velocity_message_));
}
}

// Publish to servo control topic (gz.msgs.Double)
void GazeboMavlinkInterface::PublishServoVelocities(const Eigen::VectorXd &_vels)
{
for (int i = 0; i < _vels.size(); i++) {
gz::msgs::Double servo_input;
servo_input.set_data(_vels(i));
servo_control_pub_[i].Publish(servo_input);
}
}

Expand Down
12 changes: 7 additions & 5 deletions src/mavlink_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -476,13 +476,10 @@ void MavlinkInterface::handle_actuator_controls(mavlink_message_t *msg)

armed_ = (controls.mode & MAV_MODE_FLAG_SAFETY_ARMED);

for (unsigned i = 0; i < n_out_max; i++) {
input_index_[i] = i;
}

// set rotor speeds, controller targets
// set rotor and servo speeds, controller targets
input_reference_.resize(n_out_max);
for (int i = 0; i < input_reference_.size(); i++) {
input_is_motor_[i] = controls.flags & (1 << i);
input_reference_[i] = controls.controls[i];
}
received_actuator_ = true;
Expand Down Expand Up @@ -553,6 +550,11 @@ Eigen::VectorXd MavlinkInterface::GetActuatorControls() {
return input_reference_;
}

bool MavlinkInterface::IsInputMotorAtIndex(int index) {
const std::lock_guard<std::mutex> lock(actuator_mutex_);
return input_is_motor_[index];
}

bool MavlinkInterface::GetArmedState() {
const std::lock_guard<std::mutex> lock(actuator_mutex_);
return armed_;
Expand Down

0 comments on commit a22d53e

Please sign in to comment.