From 7ad74287e49306faad7dbbff3ed5c1c6e6cf1fad Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 10 May 2023 15:31:37 +0200 Subject: [PATCH] add changing control mode of heartbeat through service --- src/drone_services/srv/SetVehicleControl.srv | 8 ++++ src/px4_connection/src/heartbeat.cpp | 50 +++++++++++++++----- src/px4_msgs | 2 +- 3 files changed, 48 insertions(+), 12 deletions(-) diff --git a/src/drone_services/srv/SetVehicleControl.srv b/src/drone_services/srv/SetVehicleControl.srv index 0be842e9..c0dd2411 100644 --- a/src/drone_services/srv/SetVehicleControl.srv +++ b/src/drone_services/srv/SetVehicleControl.srv @@ -1,3 +1,11 @@ +# bitmask for control: +# bit 0: actuator +# bit 1: body rate +# bit 2: attitude +# bit 3: acceleration +# bit 4: velocity +# bit 5: position + int32 control # control bitmask --- int8 status # status of operation \ No newline at end of file diff --git a/src/px4_connection/src/heartbeat.cpp b/src/px4_connection/src/heartbeat.cpp index 0376e8dd..3f1c5119 100644 --- a/src/px4_connection/src/heartbeat.cpp +++ b/src/px4_connection/src/heartbeat.cpp @@ -1,13 +1,20 @@ /** * @file heartbeat.cpp * @author Sem van der Hoeven (sem.hoeven@gmail.com) - * @brief Heartbeat node that keeps the connection between the flight computer + * @brief Heartbeat node that keeps the connection between the flight computer * and PX4 flight controller alive by sending OffboardControl messages */ #include #include "rclcpp/rclcpp.hpp" #include +#define CONTROL_ACTUATOR_POS 0 +#define CONTROL_BODY_RATE_POS 1 +#define CONTROL_ATTITUDE_POS 2 +#define CONTROL_ACCELERATION_POS 3 +#define CONTROL_VELOCITY_POS 4 +#define CONTROL_POSITION_POS 5 + using namespace std::chrono_literals; class HeartBeat : public rclcpp::Node @@ -15,8 +22,10 @@ class HeartBeat : public rclcpp::Node public: HeartBeat() : Node("heartbeat") { - // create a publisher on the offboard control mode topic - offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + // disarm_service_ = this->create_service("drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + vehicle_control_mode_service_ = this->create_service("drone/set_vehicle_control", std::bind()) + // create a publisher on the offboard control mode topic + offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); // create timer to send heartbeat messages (offboard control) every 100ms timer_ = this->create_wall_timer(100ms, std::bind(&HeartBeat::send_heartbeat, this)); start_time = this->get_clock()->now().seconds(); @@ -30,7 +39,10 @@ private: rclcpp::TimerBase::SharedPtr timer_; // start time of node in seconds double start_time; - + // which level of control is needed, only attitude control by default + int control_mode = 1 << CONTROL_ATTITUDE_POS; + // service to change vehicle_mode + rclcpp::Service::SharedPtr vehicle_control_mode_service_; /** * @brief Publish offboard control mode messages as a heartbeat. * Only the attitude is enabled, because that is how the drone will be controlled. @@ -38,19 +50,35 @@ private: */ void send_heartbeat() { - // set message to enable attitude + // set message to enable attitude based on control mode variable auto msg = px4_msgs::msg::OffboardControlMode(); - msg.position = false; - msg.velocity = false; - msg.acceleration = false; - msg.attitude = true; - msg.body_rate = false; - msg.actuator = false; + msg.position = (this->control_mode >> CONTROL_POSITION_POS) & 1 ? true : false; + msg.velocity = (this->control_mode >> CONTROL_VELOCITY_POS) & 1 ? true : false; + msg.acceleration = (this->control_mode >> CONTROL_ACCELERATION_POS) & 1 ? true : false; + msg.attitude = (this->control_mode >> CONTROL_ATTITUDE_POS) & 1 ? true : false; + msg.body_rate = (this->control_mode >> CONTROL_BODY_RATE_POS) & 1 ? true : false; + msg.actuator = (this->control_mode >> CONTROL_ACTUATOR_POS) & 1 ? true : false; // get timestamp and publish message msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; offboard_control_mode_publisher_->publish(msg); } + + void handle_vehicle_control_change( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + if (request->control < 0 || request->control > CONTROL_POSITION_POS) + { + response->status = 1; + } else { + this->control_mode = request->control + RCLCPP_INFO(this->get_logger(), "set control mode to %d", this->control_mode) + response->status = 0; + } + + } }; int main(int argc, char *argv[]) diff --git a/src/px4_msgs b/src/px4_msgs index ffc3a4cd..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4