diff --git a/build.sh b/build.sh index 279f8cf..f243e4a 100755 --- a/build.sh +++ b/build.sh @@ -1,10 +1,21 @@ #!/bin/bash +# present working directory +PWD=$(pwd) + +# Clone c_library_v2 commit matching with current px4-firmware mavlink commit +# => mavlink/c_library_v2:fbdb7c29 is built from mavlink/mavlink:08112084 +git clone -q https://github.com/mavlink/c_library_v2.git ${PWD}/mavlink && \ + cd ${PWD}/mavlink && git checkout -q fbdb7c29e47902d44eeaa58b4395678a9b78f3ae && \ + rm -rf ${PWD}/mavlink/.git && cd ${PWD} + +export _MAVLINK_INCLUDE_DIR=${PWD}/mavlink + + if [ ! -e build ]; then mkdir build fi cd build cmake .. make -cpack -G DEB diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index d94ad8f..ee94224 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -65,7 +65,8 @@ #include #include -#include +#include +#include #include @@ -86,6 +87,7 @@ static const std::string kDefaultNamespace = ""; // ConsPtr passing, such that the original commands don't have to go n_motors-times over the wire. static const std::string kDefaultMotorVelocityReferencePubTopic = "/gazebo/command/motor_speed"; +static const std::string kDefaultPoseTopic = "/pose"; static const std::string kDefaultImuTopic = "/imu"; static const std::string kDefaultOpticalFlowTopic = "/px4flow/link/opticalFlow"; static const std::string kDefaultIRLockTopic = "/camera/link/irlock"; @@ -94,11 +96,6 @@ static const std::string kDefaultVisionTopic = "/vision_odom"; static const std::string kDefaultMagTopic = "/magnetometer"; static const std::string kDefaultBarometerTopic = "/air_pressure"; -static const std::string kDefaultImuSensorName = "imu_sensor"; -static const std::string kDefaultGPSSensorName= "gps"; -static const std::string kDefaultMagSensorName = "mag_sensor"; -static const std::string kDefaultBarometerSensorName = "air_pressure_sensor"; - namespace mavlink_interface { class GZ_SIM_VISIBLE GazeboMavlinkInterface: @@ -132,7 +129,6 @@ namespace mavlink_interface float protocol_version_{2.0}; std::string namespace_{kDefaultNamespace}; - std::string motor_velocity_reference_pub_topic_{kDefaultMotorVelocityReferencePubTopic}; std::string mavlink_control_sub_topic_; std::string link_name_; @@ -141,12 +137,12 @@ namespace mavlink_interface bool use_left_elevon_pid_{false}; bool use_right_elevon_pid_{false}; + void PoseCallback(const gz::msgs::Pose_V &_msg); void ImuCallback(const gz::msgs::IMU &_msg); void BarometerCallback(const gz::msgs::FluidPressure &_msg); void MagnetometerCallback(const gz::msgs::Magnetometer &_msg); void GpsCallback(const gz::msgs::NavSat &_msg); void SendSensorMessages(const gz::sim::UpdateInfo &_info); - void SendGroundTruth(); void PublishRotorVelocities(gz::sim::EntityComponentManager &_ecm, const Eigen::VectorXd &_vels); void handle_actuator_controls(const gz::sim::UpdateInfo &_info); @@ -156,6 +152,8 @@ namespace mavlink_interface 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); static const unsigned n_out_max = 16; @@ -170,20 +168,15 @@ namespace mavlink_interface /// \brief gz communication node. gz::transport::Node node; + std::string pose_sub_topic_{kDefaultPoseTopic}; std::string imu_sub_topic_{kDefaultImuTopic}; std::string opticalFlow_sub_topic_{kDefaultOpticalFlowTopic}; std::string irlock_sub_topic_{kDefaultIRLockTopic}; std::string gps_sub_topic_{kDefaultGPSTopic}; - std::string groundtruth_sub_topic_; std::string vision_sub_topic_{kDefaultVisionTopic}; std::string mag_sub_topic_{kDefaultMagTopic}; std::string baro_sub_topic_{kDefaultBarometerTopic}; - std::string imu_sensor_name_{kDefaultImuSensorName}; - std::string gps_sensor_name_{kDefaultGPSSensorName}; - std::string mag_sensor_name_{kDefaultMagSensorName}; - std::string baro_sensor_name_{kDefaultBarometerSensorName}; - std::mutex last_imu_message_mutex_ {}; gz::msgs::IMU last_imu_message_; @@ -192,16 +185,11 @@ namespace mavlink_interface std::chrono::steady_clock::duration last_imu_time_{0}; std::chrono::steady_clock::duration lastControllerUpdateTime{0}; std::chrono::steady_clock::duration last_actuator_time_{0}; - std::chrono::steady_clock::duration last_heartbeat_sent_time_{0}; bool mag_updated_{false}; bool baro_updated_; bool diff_press_updated_; - double groundtruth_lat_rad{0.0}; - double groundtruth_lon_rad{0.0}; - double groundtruth_altitude{0.0}; - double imu_update_interval_ = 0.004; ///< Used for non-lockstep gz::math::Vector3d gravity_W_{gz::math::Vector3d(0.0, 0.0, -9.8)}; @@ -218,14 +206,10 @@ namespace mavlink_interface double sonar_distance; bool enable_lockstep_ = false; - bool serial_enabled_; double speed_factor_ = 1.0; uint8_t previous_imu_seq_ = 0; uint8_t update_skip_factor_ = 1; - bool hil_mode_{false}; - bool hil_state_level_{false}; - std::string mavlink_hostname_str_; struct hostent *hostptr_{nullptr}; bool mavlink_loaded_{false}; diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h index eb89fe4..c3e0a25 100644 --- a/include/mavlink_interface.h +++ b/include/mavlink_interface.h @@ -58,8 +58,6 @@ static const uint32_t kDefaultMavlinkUdpRemotePort = 14560; static const uint32_t kDefaultMavlinkUdpLocalPort = 0; static const uint32_t kDefaultMavlinkTcpPort = 4560; -static const uint32_t kDefaultQGCUdpPort = 14550; -static const uint32_t kDefaultSDKUdpPort = 14540; static const size_t kMaxRecvBufferSize = 20; static const size_t kMaxSendBufferSize = 30; @@ -144,7 +142,6 @@ class MavlinkInterface { void open(); void close(); void Load(); - void SendHeartbeat(); void SendSensorMessages(const uint64_t time_usec); void UpdateBarometer(const SensorData::Barometer &data); void UpdateAirspeed(const SensorData::Airspeed &data); @@ -157,7 +154,6 @@ class MavlinkInterface { uint8_t min_length, uint8_t length, uint8_t crc_extra); bool GetReceivedFirstActuator() {return received_first_actuator_;} void SetBaudrate(int baudrate) {baudrate_ = baudrate;} - void SetSerialEnabled(bool serial_enabled) {serial_enabled_ = serial_enabled;} void SetUseTcp(bool use_tcp) {use_tcp_ = use_tcp;} void SetUseTcpClientMode(bool tcp_client_mode) {tcp_client_mode_ = tcp_client_mode;} void SetDevice(std::string device) {device_ = device;} @@ -166,15 +162,8 @@ class MavlinkInterface { void SetMavlinkTcpPort(int mavlink_tcp_port) {mavlink_tcp_port_ = mavlink_tcp_port;} void SetMavlinkUdpRemotePort(int mavlink_udp_port) {mavlink_udp_remote_port_ = mavlink_udp_port;} void SetMavlinkUdpLocalPort(int mavlink_udp_port) {mavlink_udp_local_port_ = mavlink_udp_port;} - void SetQgcAddr(std::string qgc_addr) {qgc_addr_ = qgc_addr;} - void SetQgcUdpPort(int qgc_udp_port) {qgc_udp_port_ = qgc_udp_port;} - void SetSdkAddr(std::string sdk_addr) {sdk_addr_ = sdk_addr;} - void SetSdkUdpPort(int sdk_udp_port) {sdk_udp_port_ = sdk_udp_port;} - void SetHILMode(bool hil_mode) {hil_mode_ = hil_mode;} - void SetHILStateLevel(bool hil_state_level) {hil_state_level_ = hil_state_level;} bool IsRecvBuffEmpty() {return receiver_buffer_.empty();} - bool SerialEnabled() const { return serial_enabled_; } bool ReceivedHeartbeats() const { return received_heartbeats_; } private: @@ -191,15 +180,6 @@ class MavlinkInterface { void RegisterNewHILSensorInstance(int id); bool tryConnect(); - // Serial interface - void open_serial(); - void do_serial_read(); - void parse_serial_buffer(const boost::system::error_code& err, std::size_t bytes_t); - inline bool is_serial_open(){ - return serial_dev_.is_open(); - } - void do_serial_write(bool check_tx_state); - // UDP/TCP send/receive thread workers void ReceiveWorker(); void SendWorker(); @@ -213,20 +193,6 @@ class MavlinkInterface { struct sockaddr_in remote_simulator_addr_; socklen_t remote_simulator_addr_len_; - int qgc_udp_port_{kDefaultQGCUdpPort}; - struct sockaddr_in remote_qgc_addr_; - socklen_t remote_qgc_addr_len_; - struct sockaddr_in local_qgc_addr_; - std::string qgc_addr_{"INADDR_ANY"}; - socklen_t local_qgc_addr_len_; - - int sdk_udp_port_{kDefaultSDKUdpPort}; - struct sockaddr_in remote_sdk_addr_; - socklen_t remote_sdk_addr_len_; - struct sockaddr_in local_sdk_addr_; - socklen_t local_sdk_addr_len_; - std::string sdk_addr_{"INADDR_ANY"}; - unsigned char buf_[65535]; enum FD_TYPES { LISTEN_FD, @@ -248,16 +214,8 @@ class MavlinkInterface { int simulator_socket_fd_{0}; int simulator_tcp_client_fd_{0}; - int qgc_socket_fd_{0}; - int sdk_socket_fd_{0}; - bool enable_lockstep_{false}; - // Serial interface - boost::asio::io_service io_service_{}; - boost::asio::serial_port serial_dev_; - bool serial_enabled_{false}; - mavlink_status_t m_status_{}; mavlink_message_t m_buffer_{}; std::thread io_thread_; @@ -272,9 +230,6 @@ class MavlinkInterface { std::atomic tx_in_progress_; std::deque tx_q_{}; - bool hil_mode_; - bool hil_state_level_; - bool baro_updated_; bool diff_press_updated_; bool mag_updated_; diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index 8949d8e..69bae1f 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -39,7 +39,7 @@ GZ_ADD_PLUGIN( mavlink_interface::GazeboMavlinkInterface::ISystemPostUpdate) using namespace mavlink_interface; -GazeboMavlinkInterface::GazeboMavlinkInterface() : +GazeboMavlinkInterface::GazeboMavlinkInterface() : input_offset_ {}, input_scaling_ {}, zero_position_disarmed_ {}, @@ -65,27 +65,24 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, gzerr << "[gazebo_mavlink_interface] Please specify a robotNamespace." << std::endl; } + entity_ = _entity; + model_ = gz::sim::Model(_entity); + model_name_ = model_.Name(_ecm); + if (_sdf->HasElement("protocol_version")) { protocol_version_ = _sdf->Get("protocol_version"); } - gazebo::getSdfParam(_sdf, "motorSpeedCommandPubTopic", motor_velocity_reference_pub_topic_, - motor_velocity_reference_pub_topic_); + + gazebo::getSdfParam(_sdf, "poseSubTopic", pose_sub_topic_, pose_sub_topic_); gazebo::getSdfParam(_sdf, "gpsSubTopic", gps_sub_topic_, gps_sub_topic_); gazebo::getSdfParam(_sdf, "visionSubTopic", vision_sub_topic_, vision_sub_topic_); - gazebo::getSdfParam(_sdf, "opticalFlowSubTopic", - opticalFlow_sub_topic_, opticalFlow_sub_topic_); + gazebo::getSdfParam(_sdf, "opticalFlowSubTopic", opticalFlow_sub_topic_, opticalFlow_sub_topic_); gazebo::getSdfParam(_sdf, "irlockSubTopic", irlock_sub_topic_, irlock_sub_topic_); gazebo::getSdfParam(_sdf, "imuSubTopic", imu_sub_topic_, imu_sub_topic_); gazebo::getSdfParam(_sdf, "magSubTopic", mag_sub_topic_, mag_sub_topic_); gazebo::getSdfParam(_sdf, "baroSubTopic", baro_sub_topic_, baro_sub_topic_); - gazebo::getSdfParam(_sdf, "imuSensorName", imu_sensor_name_, imu_sensor_name_); - gazebo::getSdfParam(_sdf, "gpsSensorName", gps_sensor_name_, gps_sensor_name_); - gazebo::getSdfParam(_sdf, "magSensorName", mag_sensor_name_, mag_sensor_name_); - gazebo::getSdfParam(_sdf, "baroSensorName", baro_sensor_name_, baro_sensor_name_); - groundtruth_sub_topic_ = "/groundtruth"; - // set input_reference_ from inputs.control input_reference_.resize(n_out_max); @@ -96,42 +93,22 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, input_scaling_(2) = 1000; input_scaling_(3) = 1000; - if(_sdf->HasElement("hil_mode")) - { - hil_mode_ = _sdf->Get("hil_mode"); - mavlink_interface_->SetHILMode(hil_mode_); - } - - if(_sdf->HasElement("hil_state_level")) - { - hil_state_level_ = _sdf->Get("hil_state_level"); - mavlink_interface_->SetHILStateLevel(hil_state_level_); - } - - bool serial_enabled=false; - if(_sdf->HasElement("serialEnabled")) - { - serial_enabled = _sdf->Get("serialEnabled"); - mavlink_interface_->SetSerialEnabled(serial_enabled); - } - bool use_tcp = false; - if (!serial_enabled && _sdf->HasElement("use_tcp")) + if (_sdf->HasElement("use_tcp")) { use_tcp = _sdf->Get("use_tcp"); mavlink_interface_->SetUseTcp(use_tcp); } bool tcp_client_mode = false; - if (!serial_enabled && _sdf->HasElement("tcp_client_mode")) + if (_sdf->HasElement("tcp_client_mode")) { tcp_client_mode = _sdf->Get("tcp_client_mode"); mavlink_interface_->SetUseTcpClientMode(tcp_client_mode); } - gzmsg << "Connecting to PX4 SITL using " << (mavlink_interface_->SerialEnabled() ? - "serial" : (use_tcp ? (tcp_client_mode ? "TCP (client mode)" : "TCP (server mode)") : "UDP")) << std::endl; + gzmsg << "Connecting to PX4 SITL using " << (use_tcp ? (tcp_client_mode ? "TCP (client mode)" : "TCP (server mode)") : "UDP") << std::endl; - if (!hil_mode_ && _sdf->HasElement("enable_lockstep")) + if (_sdf->HasElement("enable_lockstep")) { enable_lockstep_ = _sdf->Get("enable_lockstep"); mavlink_interface_->SetEnableLockstep(enable_lockstep_); @@ -153,8 +130,6 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, } } gzmsg << "Speed factor set to: " << speed_factor_ << std::endl; - - } // // Listen to Ctrl+C / SIGINT. @@ -162,24 +137,26 @@ 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( + _ecm.EntityByComponents(gz::sim::components::Name(model_name_)), _ecm, false); + // Subscribe to messages of sensors. - //auto imu_entity = _ecm.EntityByComponents(gz::sim::components::Name(imu_sensor_name_)); - auto imu_topic = world_name + gz::sim::topicFromScopedName( - _ecm.EntityByComponents(gz::sim::components::Name(imu_sensor_name_)), _ecm, false) + imu_sub_topic_; + auto imu_topic = vehicle_scope_prefix + imu_sub_topic_; node.Subscribe(imu_topic, &GazeboMavlinkInterface::ImuCallback, this); - auto baro_topic = world_name + gz::sim::topicFromScopedName( - _ecm.EntityByComponents(gz::sim::components::Name(baro_sensor_name_)), _ecm, false) + baro_sub_topic_; + auto baro_topic = vehicle_scope_prefix + baro_sub_topic_; node.Subscribe(baro_topic, &GazeboMavlinkInterface::BarometerCallback, this); - auto mag_topic = world_name + gz::sim::topicFromScopedName( - _ecm.EntityByComponents(gz::sim::components::Name(mag_sensor_name_)), _ecm, false) + mag_sub_topic_; + auto mag_topic = vehicle_scope_prefix + mag_sub_topic_; node.Subscribe(mag_topic, &GazeboMavlinkInterface::MagnetometerCallback, this); - auto gps_topic = world_name + gz::sim::topicFromScopedName( - _ecm.EntityByComponents(gz::sim::components::Name(gps_sensor_name_)), _ecm, false) + gps_sub_topic_; + auto gps_topic = vehicle_scope_prefix + gps_sub_topic_; node.Subscribe(gps_topic, &GazeboMavlinkInterface::GpsCallback, this); + // Subscribe to entity pose info message + auto pose_topic = world_name + pose_sub_topic_; + node.Subscribe(pose_topic, &GazeboMavlinkInterface::PoseCallback, this); + // This doesn't seem to be used anywhere but we leave it here // for potential compatibility if (_sdf->HasElement("imu_rate")) { @@ -188,7 +165,7 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, if (_sdf->HasElement("mavlink_hostname")) { mavlink_hostname_str_ = _sdf->Get("mavlink_hostname"); - if (! serial_enabled && ! mavlink_hostname_str_.empty()) { + if (! mavlink_hostname_str_.empty()) { // Start hostname resolver thread hostname_resolver_thread_ = std::thread([this] () { ResolveWorker(); @@ -218,42 +195,6 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, mavlink_interface_->SetMavlinkTcpPort(mavlink_tcp_port); } - if (_sdf->HasElement("qgc_addr")) { - std::string qgc_addr = _sdf->Get("qgc_addr"); - if (qgc_addr != "INADDR_ANY") { - mavlink_interface_->SetQgcAddr(qgc_addr); - } - } - if (_sdf->HasElement("qgc_udp_port")) { - int qgc_udp_port = _sdf->Get("qgc_udp_port"); - mavlink_interface_->SetQgcUdpPort(qgc_udp_port); - } - - if (_sdf->HasElement("sdk_addr")) { - std::string sdk_addr = _sdf->Get("sdk_addr"); - if (sdk_addr != "INADDR_ANY") { - mavlink_interface_->SetSdkAddr(sdk_addr); - } - } - if (_sdf->HasElement("sdk_udp_port")) { - int sdk_udp_port = _sdf->Get("sdk_udp_port"); - mavlink_interface_->SetSdkUdpPort(sdk_udp_port); - } - - if (serial_enabled_) { - // Set up serial interface - if(_sdf->HasElement("serialDevice")) - { - std::string device = _sdf->Get("serialDevice"); - mavlink_interface_->SetDevice(device); - } - - if (_sdf->HasElement("baudRate")) { - int baudrate = _sdf->Get("baudRate"); - mavlink_interface_->SetBaudrate(baudrate); - } - } - mavlink_status_t* chan_state = mavlink_get_channel_status(MAVLINK_COMM_0); // set the Mavlink protocol version to use on the link @@ -269,13 +210,9 @@ void GazeboMavlinkInterface::Configure(const gz::sim::Entity &_entity, gzerr << "Unkown protocol version! Using v" << protocol_version_ << "by default " << std::endl; } - entity_ = _entity; - model_ = gz::sim::Model(_entity); - model_name_ = model_.Name(_ecm); - std::default_random_engine rnd_gen_; - if (hostptr_ || mavlink_hostname_str_.empty() || mavlink_interface_->SerialEnabled()) { + if (hostptr_ || mavlink_hostname_str_.empty()) { gzmsg << "--> load mavlink_interface_" << std::endl; mavlink_interface_->Load(); mavlink_loaded_ = true; @@ -289,7 +226,7 @@ void GazeboMavlinkInterface::PreUpdate(const gz::sim::UpdateInfo &_info, if (!(previous_imu_seq_++ % update_skip_factor_ == 0)) { return; } - + if (!mavlink_loaded_) { // mavlink not loaded, exit return; @@ -299,20 +236,9 @@ void GazeboMavlinkInterface::PreUpdate(const gz::sim::UpdateInfo &_info, mavlink_interface_->ReadMAVLinkMessages(); - // We need to send out heartbeats at a high rate until the connection is established, - // otherwise PX4 on USB doesn't enable mavlink and the buffer fills up. - std::chrono::steady_clock::duration current_time = _info.simTime; - if ((current_time - last_heartbeat_sent_time_).count() > 1.0 || !mavlink_interface_->ReceivedHeartbeats()) { - mavlink_interface_->SendHeartbeat(); - last_heartbeat_sent_time_ = current_time; - } - // Always send Gyro and Accel data at full rate (= sim update rate) SendSensorMessages(_info); - // Send groundtruth at full rate - //SendGroundTruth(); - handle_actuator_controls(_info); handle_control(dt); @@ -326,6 +252,49 @@ void GazeboMavlinkInterface::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) { } +void GazeboMavlinkInterface::PoseCallback(const gz::msgs::Pose_V &_msg){ + for (int p = 0; p < _msg.pose_size(); p++) { + if (_msg.pose(p).name() == model_name_) { + gz::msgs::Vector3d pose_position = _msg.pose(p).position(); + gz::msgs::Quaternion pose_orientation = _msg.pose(p).orientation(); + + // orientation transform + gz::math::Quaterniond q_gr = gz::math::Quaterniond( + pose_orientation.w(), + pose_orientation.x(), + pose_orientation.y(), + pose_orientation.z()); + + gz::math::Quaterniond q_nb; + RotateQuaternion(q_nb, q_gr); + + // send pose info + mavlink_hil_state_quaternion_t hil_state_quat; + + hil_state_quat.attitude_quaternion[0] = q_nb.W(); + hil_state_quat.attitude_quaternion[1] = q_nb.X(); + hil_state_quat.attitude_quaternion[2] = q_nb.Y(); + hil_state_quat.attitude_quaternion[3] = q_nb.Z(); + + hil_state_quat.lat = pose_position.y() * 1e3; + hil_state_quat.lon = pose_position.x() * 1e3; + hil_state_quat.alt = pose_position.z() * 1e3; + + mavlink_message_t msg; + mavlink_msg_hil_state_quaternion_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &hil_state_quat); + // Override default global mavlink channel status with instance specific status + mavlink_interface_->FinalizeOutgoingMessage(&msg, 1, 200, + MAVLINK_MSG_ID_HIL_STATE_QUATERNION_MIN_LEN, + MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, + MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); + mavlink_interface_->PushSendMessage(&msg); + } + } + + + +} + void GazeboMavlinkInterface::ImuCallback(const gz::msgs::IMU &_msg) { const std::lock_guard lock(last_imu_message_mutex_); last_imu_message_ = _msg; @@ -399,16 +368,14 @@ void GazeboMavlinkInterface::GpsCallback(const gz::msgs::NavSat &_msg) { //gzmsg << "[GpsCallback] alt: " << _msg.altitude() << std::endl; // send HIL_GPS Mavlink msg - if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { - mavlink_message_t msg; - mavlink_msg_hil_gps_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &hil_gps_msg); - // Override default global mavlink channel status with instance specific status - mavlink_interface_->FinalizeOutgoingMessage(&msg, 1, 200, - MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, - MAVLINK_MSG_ID_HIL_GPS_LEN, - MAVLINK_MSG_ID_HIL_GPS_CRC); - mavlink_interface_->PushSendMessage(&msg); - } + mavlink_message_t msg; + mavlink_msg_hil_gps_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &hil_gps_msg); + // Override default global mavlink channel status with instance specific status + mavlink_interface_->FinalizeOutgoingMessage(&msg, 1, 200, + MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, + MAVLINK_MSG_ID_HIL_GPS_LEN, + MAVLINK_MSG_ID_HIL_GPS_CRC); + mavlink_interface_->PushSendMessage(&msg); } void GazeboMavlinkInterface::SendSensorMessages(const gz::sim::UpdateInfo &_info) { @@ -448,7 +415,7 @@ void GazeboMavlinkInterface::SendSensorMessages(const gz::sim::UpdateInfo &_info AddSimpleNoise(last_imu_message.angular_velocity().x(), 0, 0.001), AddSimpleNoise(last_imu_message.angular_velocity().y(), 0, 0.001), AddSimpleNoise(last_imu_message.angular_velocity().z(), 0, 0.001))); - + uint64_t time_usec = std::chrono::duration_cast>(_info.simTime * 1e6).count(); SensorData::Imu imu_data; imu_data.accel_b = Eigen::Vector3d(accel_b.X(), accel_b.Y(), accel_b.Z()); @@ -457,77 +424,6 @@ void GazeboMavlinkInterface::SendSensorMessages(const gz::sim::UpdateInfo &_info mavlink_interface_->SendSensorMessages(time_usec); } -void GazeboMavlinkInterface::SendGroundTruth() -{ - // ground truth - gz::math::Quaterniond q_gr = gz::math::Quaterniond( - last_imu_message_.orientation().w(), - last_imu_message_.orientation().x(), - last_imu_message_.orientation().y(), - last_imu_message_.orientation().z()); - - gz::math::Quaterniond q_gb = q_gr*q_FLU_to_FRD.Inverse(); - gz::math::Quaterniond q_nb = q_ENU_to_NED*q_gb; - - // gz::math::Vector3d vel_b = q_FLU_to_FRD.RotateVector(model_->RelativeLinearVel()); - // gz::math::Vector3d vel_n = q_ENU_to_NED.RotateVector(model_->WorldLinearVel()); - // gz::math::Vector3d omega_nb_b = q_br.RotateVector(model_->RelativeAngularVel()); - gz::math::Vector3d vel_b; - gz::math::Vector3d vel_n; - gz::math::Vector3d omega_nb_b; - - // gz::math::Vector3d accel_true_b = q_FLU_to_FRD.RotateVector(model_->RelativeLinearAccel()); - gz::math::Vector3d accel_true_b; //TODO: Get model pointer - - // send ground truth - mavlink_hil_state_quaternion_t hil_state_quat; -// #if GAZEBO_MAJOR_VERSION >= 9 -// hil_state_quat.time_usec = world_->SimTime().Double() * 1e6; -// #else -// hil_state_quat.time_usec = world_->GetSimTime().Double() * 1e6; -// #endif - hil_state_quat.attitude_quaternion[0] = q_nb.W(); - hil_state_quat.attitude_quaternion[1] = q_nb.X(); - hil_state_quat.attitude_quaternion[2] = q_nb.Y(); - hil_state_quat.attitude_quaternion[3] = q_nb.Z(); - - hil_state_quat.rollspeed = omega_nb_b.X(); - hil_state_quat.pitchspeed = omega_nb_b.Y(); - hil_state_quat.yawspeed = omega_nb_b.Z(); - - hil_state_quat.lat = groundtruth_lat_rad * 180 / M_PI * 1e7; - hil_state_quat.lon = groundtruth_lon_rad * 180 / M_PI * 1e7; - hil_state_quat.alt = groundtruth_altitude * 1000; - - hil_state_quat.vx = vel_n.X() * 100; - hil_state_quat.vy = vel_n.Y() * 100; - hil_state_quat.vz = vel_n.Z() * 100; - - // assumed indicated airspeed due to flow aligned with pitot (body x) - hil_state_quat.ind_airspeed = vel_b.X(); - -// #if GAZEBO_MAJOR_VERSION >= 9 -// hil_state_quat.true_airspeed = model_->WorldLinearVel().Length() * 100; //no wind simulated -// #else -// hil_state_quat.true_airspeed = model_->GetWorldLinearVel().GetLength() * 100; //no wind simulated -// #endif - - hil_state_quat.xacc = accel_true_b.X() * 1000; - hil_state_quat.yacc = accel_true_b.Y() * 1000; - hil_state_quat.zacc = accel_true_b.Z() * 1000; - - if (!hil_mode_ || (hil_mode_ && hil_state_level_)) { - mavlink_message_t msg; - mavlink_msg_hil_state_quaternion_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &hil_state_quat); - // Override default global mavlink channel status with instance specific status - mavlink_interface_->FinalizeOutgoingMessage(&msg, 1, 200, - MAVLINK_MSG_ID_HIL_STATE_QUATERNION_MIN_LEN, - MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, - MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); - mavlink_interface_->PushSendMessage(&msg); - } -} - void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo &_info) { bool armed = mavlink_interface_->GetArmedState(); @@ -645,3 +541,22 @@ float GazeboMavlinkInterface::AddSimpleNoise(float value, float mean, float stdd std::normal_distribution dist(mean, stddev); return value + dist(rnd_gen_); } + +void GazeboMavlinkInterface::RotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, + const gz::math::Quaterniond q_FLU_to_ENU) +{ + // FLU (ROS) to FRD (PX4) static rotation + static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0); + + /** + * @brief Quaternion for rotation between ENU and NED frames + * + * NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East) + * ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North) + * This rotation is symmetric, so q_ENU_to_NED == q_NED_to_ENU. + */ + static const auto q_ENU_to_NED = gz::math::Quaterniond(0, 0.70711, 0.70711, 0); + + // final rotation composition + q_FRD_to_NED = q_ENU_to_NED * q_FLU_to_ENU * q_FLU_to_FRD.Inverse(); +} \ No newline at end of file diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index 88bd638..db4e207 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -1,10 +1,5 @@ #include "mavlink_interface.h" -MavlinkInterface::MavlinkInterface() : - serial_dev_(io_service_){ - -} - MavlinkInterface::~MavlinkInterface() { close(); } @@ -19,187 +14,119 @@ void MavlinkInterface::Load() abort(); } } - local_qgc_addr_.sin_addr.s_addr = htonl(INADDR_ANY); - if (qgc_addr_ != "INADDR_ANY") { - local_qgc_addr_.sin_addr.s_addr = inet_addr(qgc_addr_.c_str()); - if (local_qgc_addr_.sin_addr.s_addr == INADDR_NONE) { - std::cerr << "Invalid qgc_addr: " << qgc_addr_ << ", aborting" << std::endl; - abort(); - } - } - local_sdk_addr_.sin_addr.s_addr = htonl(INADDR_ANY); - if (sdk_addr_ != "INADDR_ANY") { - local_sdk_addr_.sin_addr.s_addr = inet_addr(sdk_addr_.c_str()); - if (local_sdk_addr_.sin_addr.s_addr == INADDR_NONE) { - std::cerr << "Invalid sdk_addr: " << sdk_addr_ << ", aborting" << std::endl; - abort(); - } - } // initialize sender status to zero memset((char *)&sender_m_status_, 0, sizeof(sender_m_status_)); - if (hil_mode_) { - - local_qgc_addr_.sin_family = AF_INET; - local_qgc_addr_.sin_port = htons(0); - local_qgc_addr_len_ = sizeof(local_qgc_addr_); - - remote_qgc_addr_.sin_family = AF_INET; - remote_qgc_addr_.sin_port = htons(qgc_udp_port_); - remote_qgc_addr_len_ = sizeof(remote_qgc_addr_); - - local_sdk_addr_.sin_family = AF_INET; - local_sdk_addr_.sin_port = htons(0); - local_sdk_addr_len_ = sizeof(local_sdk_addr_); - - remote_sdk_addr_.sin_family = AF_INET; - remote_sdk_addr_.sin_port = htons(sdk_udp_port_); - remote_sdk_addr_len_ = sizeof(remote_sdk_addr_); - - if ((qgc_socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - std::cerr << "Creating QGC UDP socket failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } - - if (bind(qgc_socket_fd_, (struct sockaddr *)&local_qgc_addr_, local_qgc_addr_len_) < 0) { - std::cerr << "QGC UDP bind failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } - - if ((sdk_socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - std::cerr << "Creating SDK UDP socket failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } - - if (bind(sdk_socket_fd_, (struct sockaddr *)&local_sdk_addr_, local_sdk_addr_len_) < 0) { - std::cerr << "SDK UDP bind failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } - - } - - if (serial_enabled_) { - // Set up serial interface - io_service_.post(std::bind(&MavlinkInterface::do_serial_read, this)); - - // run io_service for async io - io_thread_ = std::thread([this] () { - io_service_.run(); - }); - open_serial(); + memset((char *)&remote_simulator_addr_, 0, sizeof(remote_simulator_addr_)); + remote_simulator_addr_.sin_family = AF_INET; + remote_simulator_addr_len_ = sizeof(remote_simulator_addr_); - } else { - memset((char *)&remote_simulator_addr_, 0, sizeof(remote_simulator_addr_)); - remote_simulator_addr_.sin_family = AF_INET; - remote_simulator_addr_len_ = sizeof(remote_simulator_addr_); - - memset((char *)&local_simulator_addr_, 0, sizeof(local_simulator_addr_)); - local_simulator_addr_.sin_family = AF_INET; - local_simulator_addr_len_ = sizeof(local_simulator_addr_); - - if (use_tcp_) { - - if ((simulator_socket_fd_ = socket(AF_INET, SOCK_STREAM, 0)) < 0) { - std::cerr << "Creating TCP socket failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } - - int yes = 1; - int result = setsockopt(simulator_socket_fd_, IPPROTO_TCP, TCP_NODELAY, &yes, sizeof(yes)); - if (result != 0) { - std::cerr << "setsockopt failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } + memset((char *)&local_simulator_addr_, 0, sizeof(local_simulator_addr_)); + local_simulator_addr_.sin_family = AF_INET; + local_simulator_addr_len_ = sizeof(local_simulator_addr_); - struct linger nolinger {}; - nolinger.l_onoff = 1; - nolinger.l_linger = 0; - - result = setsockopt(simulator_socket_fd_, SOL_SOCKET, SO_LINGER, &nolinger, sizeof(nolinger)); - if (result != 0) { - std::cerr << "setsockopt failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } + if (use_tcp_) { - // The socket reuse is necessary for reconnecting to the same address - // if the socket does not close but gets stuck in TIME_WAIT. This can happen - // if the server is suddenly closed, for example, if the robot is deleted in gazebo. - int socket_reuse = 1; - result = setsockopt(simulator_socket_fd_, SOL_SOCKET, SO_REUSEADDR, &socket_reuse, sizeof(socket_reuse)); - if (result != 0) { - std::cerr << "setsockopt failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } + if ((simulator_socket_fd_ = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + std::cerr << "Creating TCP socket failed: " << strerror(errno) << ", aborting" << std::endl; + abort(); + } - // Same as above but for a given port - result = setsockopt(simulator_socket_fd_, SOL_SOCKET, SO_REUSEPORT, &socket_reuse, sizeof(socket_reuse)); - if (result != 0) { - std::cerr << "setsockopt failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } + int yes = 1; + int result = setsockopt(simulator_socket_fd_, IPPROTO_TCP, TCP_NODELAY, &yes, sizeof(yes)); + if (result != 0) { + std::cerr << "setsockopt failed: " << strerror(errno) << ", aborting" << std::endl; + abort(); + } - if (tcp_client_mode_) { - // TCP client mode - local_simulator_addr_.sin_addr.s_addr = htonl(INADDR_ANY); - local_simulator_addr_.sin_port = htons(0); - remote_simulator_addr_.sin_addr.s_addr = mavlink_addr_; - remote_simulator_addr_.sin_port = htons(mavlink_tcp_port_); - memset(fds_, 0, sizeof(fds_)); - } else { - // TCP server mode - local_simulator_addr_.sin_addr.s_addr = mavlink_addr_; - local_simulator_addr_.sin_port = htons(mavlink_tcp_port_); - - if (bind(simulator_socket_fd_, (struct sockaddr *)&local_simulator_addr_, local_simulator_addr_len_) < 0) { - std::cerr << "bind failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } + struct linger nolinger {}; + nolinger.l_onoff = 1; + nolinger.l_linger = 0; - errno = 0; - if (listen(simulator_socket_fd_, 0) < 0) { - std::cerr << "listen failed: " << strerror(errno) << ", aborting" << std::endl; - abort(); - } + result = setsockopt(simulator_socket_fd_, SOL_SOCKET, SO_LINGER, &nolinger, sizeof(nolinger)); + if (result != 0) { + std::cerr << "setsockopt failed: " << strerror(errno) << ", aborting" << std::endl; + abort(); + } - memset(fds_, 0, sizeof(fds_)); - fds_[LISTEN_FD].fd = simulator_socket_fd_; - fds_[LISTEN_FD].events = POLLIN; // only listens for new connections on tcp + // The socket reuse is necessary for reconnecting to the same address + // if the socket does not close but gets stuck in TIME_WAIT. This can happen + // if the server is suddenly closed, for example, if the robot is deleted in gazebo. + int socket_reuse = 1; + result = setsockopt(simulator_socket_fd_, SOL_SOCKET, SO_REUSEADDR, &socket_reuse, sizeof(socket_reuse)); + if (result != 0) { + std::cerr << "setsockopt failed: " << strerror(errno) << ", aborting" << std::endl; + abort(); } - } else { - // When connecting to SITL, we specify the port where the mavlink traffic originates from. - remote_simulator_addr_.sin_addr.s_addr = mavlink_addr_; - remote_simulator_addr_.sin_port = htons(mavlink_udp_remote_port_); - local_simulator_addr_.sin_addr.s_addr = htonl(INADDR_ANY); - local_simulator_addr_.sin_port = htons(mavlink_udp_local_port_); - if ((simulator_socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { - std::cerr << "Creating UDP socket failed: " << strerror(errno) << ", aborting" << std::endl; + // Same as above but for a given port + result = setsockopt(simulator_socket_fd_, SOL_SOCKET, SO_REUSEPORT, &socket_reuse, sizeof(socket_reuse)); + if (result != 0) { + std::cerr << "setsockopt failed: " << strerror(errno) << ", aborting" << std::endl; abort(); } + if (tcp_client_mode_) { + // TCP client mode + local_simulator_addr_.sin_addr.s_addr = htonl(INADDR_ANY); + local_simulator_addr_.sin_port = htons(0); + remote_simulator_addr_.sin_addr.s_addr = mavlink_addr_; + remote_simulator_addr_.sin_port = htons(mavlink_tcp_port_); + memset(fds_, 0, sizeof(fds_)); + } else { + // TCP server mode + local_simulator_addr_.sin_addr.s_addr = mavlink_addr_; + local_simulator_addr_.sin_port = htons(mavlink_tcp_port_); + if (bind(simulator_socket_fd_, (struct sockaddr *)&local_simulator_addr_, local_simulator_addr_len_) < 0) { std::cerr << "bind failed: " << strerror(errno) << ", aborting" << std::endl; abort(); } + errno = 0; + if (listen(simulator_socket_fd_, 0) < 0) { + std::cerr << "listen failed: " << strerror(errno) << ", aborting" << std::endl; + abort(); + } + memset(fds_, 0, sizeof(fds_)); - fds_[CONNECTION_FD].fd = simulator_socket_fd_; - fds_[CONNECTION_FD].events = POLLIN | POLLOUT; // read/write + fds_[LISTEN_FD].fd = simulator_socket_fd_; + fds_[LISTEN_FD].events = POLLIN; // only listens for new connections on tcp + } + } else { + // When connecting to SITL, we specify the port where the mavlink traffic originates from. + remote_simulator_addr_.sin_addr.s_addr = mavlink_addr_; + remote_simulator_addr_.sin_port = htons(mavlink_udp_remote_port_); + local_simulator_addr_.sin_addr.s_addr = htonl(INADDR_ANY); + local_simulator_addr_.sin_port = htons(mavlink_udp_local_port_); + + std::cout << "Creating UDP socket for SITL input on local port : " << mavlink_udp_local_port_ << " and remote port " << mavlink_udp_remote_port_ << std::endl; + if ((simulator_socket_fd_ = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + std::cerr << "Creating UDP socket failed: " << strerror(errno) << ", aborting" << std::endl; + abort(); } - // Start mavlink message receiver thread - receiver_thread_ = std::thread([this] () { - ReceiveWorker(); - }); - // Start mavlink message sender thread - sender_thread_ = std::thread([this] () { - SendWorker(); - }); + if (bind(simulator_socket_fd_, (struct sockaddr *)&local_simulator_addr_, local_simulator_addr_len_) < 0) { + std::cerr << "bind failed: " << strerror(errno) << ", aborting" << std::endl; + abort(); + } + + memset(fds_, 0, sizeof(fds_)); + fds_[CONNECTION_FD].fd = simulator_socket_fd_; + fds_[CONNECTION_FD].events = POLLIN | POLLOUT; // read/write } - // hil_data_.resize(1); + + // Start mavlink message receiver thread + receiver_thread_ = std::thread([this] () { + ReceiveWorker(); + }); + // Start mavlink message sender thread + sender_thread_ = std::thread([this] () { + SendWorker(); + }); } @@ -296,19 +223,15 @@ void MavlinkInterface::ReceiveWorker() { */ void MavlinkInterface::PushSendMessage(std::shared_ptr msg) { - if (serial_enabled_) { - send_mavlink_message(msg.get()); - } else { - const std::lock_guard guard(sender_buff_mtx_); - sender_buffer_.push(msg); - sender_cv_.notify_one(); - - if (sender_buffer_.size() > kMaxSendBufferSize) { - sender_buffer_.pop(); - // Starts reporting buffer overflows only after the connection is established to FC - if (received_first_actuator_) { - std::cerr << "PushSendMessage - Messages buffer overflow!" << std::endl; - } + const std::lock_guard guard(sender_buff_mtx_); + sender_buffer_.push(msg); + sender_cv_.notify_one(); + + if (sender_buffer_.size() > kMaxSendBufferSize) { + sender_buffer_.pop(); + // Starts reporting buffer overflows only after the connection is established to FC + if (received_first_actuator_) { + std::cerr << "PushSendMessage - Messages buffer overflow!" << std::endl; } } } @@ -353,27 +276,6 @@ void MavlinkInterface::SendWorker() { std::cout << "[" << thrd_name << "] Shutdown.." << std::endl; } -void MavlinkInterface::SendHeartbeat() { - // In order to start the mavlink instance on Pixhawk over USB, we need to send heartbeats. - if (hil_mode_) { - mavlink_message_t msg; - mavlink_msg_heartbeat_pack_chan( - 1, 200, - MAVLINK_COMM_0, - &msg, - MAV_TYPE_GENERIC, - MAV_AUTOPILOT_INVALID, - 0, 0, 0); - // Override default global mavlink channel status with instance specific status - FinalizeOutgoingMessage(&msg, 1, 200, - MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, - MAVLINK_MSG_ID_HEARTBEAT_LEN, - MAVLINK_MSG_ID_HEARTBEAT_CRC); - auto msg_shared = std::make_shared(msg); - PushSendMessage(msg_shared); - } -} - void MavlinkInterface::SendSensorMessages(uint64_t time_usec) { mavlink_hil_sensor_t sensor_msg; sensor_msg.fields_updated = 0; @@ -430,17 +332,15 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec) { } sensor_msg_mutex_.unlock(); - if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { - mavlink_message_t msg; - mavlink_msg_hil_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg); - // Override default global mavlink channel status with instance specific status - FinalizeOutgoingMessage(&msg, 1, 200, - MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, - MAVLINK_MSG_ID_HIL_SENSOR_LEN, - MAVLINK_MSG_ID_HIL_SENSOR_CRC); - auto msg_shared = std::make_shared(msg); - PushSendMessage(msg_shared); - } + mavlink_message_t msg; + mavlink_msg_hil_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg); + // Override default global mavlink channel status with instance specific status + FinalizeOutgoingMessage(&msg, 1, 200, + MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, + MAVLINK_MSG_ID_HIL_SENSOR_LEN, + MAVLINK_MSG_ID_HIL_SENSOR_CRC); + auto msg_shared = std::make_shared(msg); + PushSendMessage(msg_shared); } void MavlinkInterface::UpdateBarometer(const SensorData::Barometer &data) { @@ -480,9 +380,6 @@ void MavlinkInterface::ReadMAVLinkMessages() if (gotSigInt_) { return; } - if (serial_enabled_) { - return; - } received_actuator_ = false; @@ -589,33 +486,6 @@ void MavlinkInterface::handle_actuator_controls(mavlink_message_t *msg) received_first_actuator_ = true; } -void MavlinkInterface::forward_mavlink_message(const mavlink_message_t *message) -{ - if (gotSigInt_) { - return; - } - - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - int packetlen = mavlink_msg_to_send_buffer(buffer, message); - ssize_t len; - if (qgc_socket_fd_ > 0) { - len = sendto(qgc_socket_fd_, buffer, packetlen, 0, (struct sockaddr *)&remote_qgc_addr_, remote_qgc_addr_len_); - - if (len <= 0) - { - std::cerr << "Failed sending mavlink message to QGC: " << strerror(errno) << std::endl; - } - } - - if (sdk_socket_fd_ > 0) { - len = sendto(sdk_socket_fd_, buffer, packetlen, 0, (struct sockaddr *)&remote_sdk_addr_, remote_sdk_addr_len_); - if (len <= 0) - { - std::cerr << "Failed sending mavlink message to SDK: " << strerror(errno) << std::endl; - } - } -} - void MavlinkInterface::send_mavlink_message(const mavlink_message_t *message) { assert(message != nullptr); @@ -624,42 +494,23 @@ void MavlinkInterface::send_mavlink_message(const mavlink_message_t *message) return; } - if (serial_enabled_) { - - if (!is_serial_open()) { - std::cerr << "Serial port closed! " << std::endl; - return; - } - - { - std::lock_guard lock(mutex_); + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + int packetlen = mavlink_msg_to_send_buffer(buffer, message); - if (tx_q_.size() >= MAX_TXQ_SIZE) { - std::cout << "Tx queue overflow" << std::endl; - } - tx_q_.emplace_back(message); + if (fds_[CONNECTION_FD].fd > 0) { + ssize_t len; + if (use_tcp_) { + len = send(fds_[CONNECTION_FD].fd, buffer, packetlen, 0); + } else { + len = sendto(fds_[CONNECTION_FD].fd, buffer, packetlen, 0, (struct sockaddr *)&remote_simulator_addr_, remote_simulator_addr_len_); } - io_service_.post(std::bind(&MavlinkInterface::do_serial_write, this, true)); - - } else { - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - int packetlen = mavlink_msg_to_send_buffer(buffer, message); - - if (fds_[CONNECTION_FD].fd > 0) { - ssize_t len; - if (use_tcp_) { - len = send(fds_[CONNECTION_FD].fd, buffer, packetlen, 0); - } else { - len = sendto(fds_[CONNECTION_FD].fd, buffer, packetlen, 0, (struct sockaddr *)&remote_simulator_addr_, remote_simulator_addr_len_); - } - if (len < 0) { - if (received_first_actuator_) { - std::cerr << "Failed sending mavlink message: " << strerror(errno) << std::endl; - if (errno == ECONNRESET || errno == EPIPE) { - if (use_tcp_) { // udp socket remains alive - std::cerr << "Closing connection." << std::endl; - close_conn_ = true; - } + if (len < 0) { + if (received_first_actuator_) { + std::cerr << "Failed sending mavlink message: " << strerror(errno) << std::endl; + if (errno == ECONNRESET || errno == EPIPE) { + if (use_tcp_) { // udp socket remains alive + std::cerr << "Closing connection." << std::endl; + close_conn_ = true; } } } @@ -667,141 +518,27 @@ void MavlinkInterface::send_mavlink_message(const mavlink_message_t *message) } } -void MavlinkInterface::open_serial() { - try{ - serial_dev_.open(device_); - serial_dev_.set_option(boost::asio::serial_port_base::baud_rate(baudrate_)); - serial_dev_.set_option(boost::asio::serial_port_base::character_size(8)); - serial_dev_.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); - serial_dev_.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); - serial_dev_.set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none)); - std::cout << "Opened serial device " << device_ << std::endl; - } - catch (boost::system::system_error &err) { - std::cerr <<"Error opening serial device: " << err.what() << std::endl; - } -} - void MavlinkInterface::close() { - if(serial_enabled_) { - ::close(qgc_socket_fd_); - ::close(sdk_socket_fd_); - - std::lock_guard lock(mutex_); - if (!is_serial_open()) - return; - - io_service_.stop(); - serial_dev_.close(); - - if (io_thread_.joinable()) - io_thread_.join(); - - } else { - - // Shutdown receiver side - shutdown(fds_[CONNECTION_FD].fd, SHUT_RD); - - if (receiver_thread_.joinable()) - receiver_thread_.join(); + // Shutdown receiver side + shutdown(fds_[CONNECTION_FD].fd, SHUT_RD); - if (sender_thread_.joinable()) { - sender_cv_.notify_one(); - sender_thread_.join(); - } - - ::close(fds_[CONNECTION_FD].fd); - fds_[CONNECTION_FD] = { 0, 0, 0 }; - fds_[CONNECTION_FD].fd = -1; - - received_first_actuator_ = false; + if (receiver_thread_.joinable()) + receiver_thread_.join(); + if (sender_thread_.joinable()) { + sender_cv_.notify_one(); + sender_thread_.join(); } -} -void MavlinkInterface::do_serial_write(bool check_tx_state){ - if (check_tx_state && tx_in_progress_) - return; - - std::lock_guard lock(mutex_); - if (tx_q_.empty()) - return; + ::close(fds_[CONNECTION_FD].fd); + fds_[CONNECTION_FD] = { 0, 0, 0 }; + fds_[CONNECTION_FD].fd = -1; - tx_in_progress_ = true; - auto &buf_ref = tx_q_.front(); - - serial_dev_.async_write_some( - boost::asio::buffer(buf_ref.dpos(), buf_ref.nbytes()), [this, &buf_ref] (boost::system::error_code error, size_t bytes_transferred) - { - assert(bytes_transferred <= buf_ref.len); - if(error) { - std::cerr << "Serial error: " << error.message() << std::endl; - return; - } - - std::lock_guard lock(mutex_); - - if (tx_q_.empty()) { - tx_in_progress_ = false; - return; - } - - buf_ref.pos += bytes_transferred; - if (buf_ref.nbytes() == 0) { - tx_q_.pop_front(); - } - - if (!tx_q_.empty()) { - do_serial_write(false); - } - else { - tx_in_progress_ = false; - } - }); + received_first_actuator_ = false; } -void MavlinkInterface::do_serial_read(void) -{ - serial_dev_.async_read_some(boost::asio::buffer(rx_buf_), boost::bind( - &MavlinkInterface::parse_serial_buffer, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred - ) - ); -} - -// Based on MAVConnInterface::parse_buffer in MAVROS -void MavlinkInterface::parse_serial_buffer(const boost::system::error_code& err, std::size_t bytes_t){ - mavlink_status_t status; - mavlink_message_t message; - uint8_t *buf = this->rx_buf_.data(); - - assert(rx_buf_.size() >= bytes_t); - - for(; bytes_t > 0; bytes_t--) - { - auto c = *buf++; - - auto msg_received = static_cast(mavlink_frame_char_buffer(&m_buffer_, &m_status_, c, &message, &status)); - if (msg_received == Framing::bad_crc || msg_received == Framing::bad_signature) { - _mav_parse_error(&m_status_); - m_status_.msg_received = MAVLINK_FRAMING_INCOMPLETE; - m_status_.parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) { - m_status_.parse_state = MAVLINK_PARSE_STATE_GOT_STX; - m_buffer_.len = 0; - mavlink_start_checksum(&m_buffer_); - } - } - if (msg_received != Framing::incomplete) { - if (hil_mode_) { - forward_mavlink_message(&message); - } - handle_message(&message); - } - } - do_serial_read(); -} void MavlinkInterface::onSigInt() { gotSigInt_ = true; diff --git a/worlds/iris.world b/worlds/iris.world deleted file mode 100644 index 7b3bee1..0000000 --- a/worlds/iris.world +++ /dev/null @@ -1,341 +0,0 @@ - - - - - 0.004 - 1.0 - - - - - - - - - ogre2 - - - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/quadcopter/control - /world/quadcopter/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/quadcopter/stats - - - - - - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - - - - true - - - - - 0 0 1 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - model://x3 - - X3 - X3/rotor_0_joint - X3/rotor_0 - ccw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 0 - 8.06428e-05 - 1e-06 - motor_speed/0 - 10 - velocity - - - X3 - X3/rotor_1_joint - X3/rotor_1 - ccw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 1 - 8.06428e-05 - 1e-06 - motor_speed/1 - 10 - velocity - - - X3 - X3/rotor_2_joint - X3/rotor_2 - cw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 2 - 8.06428e-05 - 1e-06 - motor_speed/2 - 10 - velocity - - - X3 - X3/rotor_3_joint - X3/rotor_3 - cw - 0.0125 - 0.025 - 800.0 - 8.54858e-06 - 0.016 - gazebo/command/motor_speed - 3 - 8.06428e-05 - 1e-06 - motor_speed/3 - 10 - velocity - - - - /imu - /mag - /baro - INADDR_ANY - 14560 - 4560 - 0 - /dev/ttyACM0 - 921600 - INADDR_ANY - 14550 - INADDR_ANY - 14540 - 0 - 0 - 0 - 0 - 1 - 1 - 1 - /gazebo/command/motor_speed - - - 0 - 0 - 1000 - 0 - 100 - velocity - - - 1 - 0 - 1000 - 0 - 100 - velocity - - - 2 - 0 - 1000 - 0 - 100 - velocity - - - 3 - 0 - 1000 - 0 - 100 - velocity - - - 4 - 1 - 324.6 - 0 - 0 - velocity - -

0.1

- 0 - 0 - 0.0 - 0.0 - 2 - -2 - - zephyr_delta_wing::propeller_joint - - - 5 - 0 - 0.524 - 0 - 0 - position - zephyr_delta_wing::flap_left_joint - -

10.0

- 0 - 0 - 0 - 0 - 20 - -20 - - - - 6 - 0 - 0.524 - 0 - 0 - position - zephyr_delta_wing::flap_right_joint - -

10.0

- 0 - 0 - 0 - 0 - 20 - -20 - - - - 7 - 0 - 0.524 - 0 - 0 - position - - - - - -