sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add test arming back after 20 setpoints

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-12 15:13:08 +02:00
parent eae3620848
commit 3b41f4f81d
1 changed files with 15 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/px4_connection/src/px4_controller.cpp
View File

@@ -82,7 +82,7 @@ private:
bool new_setpoint = false; // for printing new q_d when a new setpoint has been received bool new_setpoint = false; // for printing new q_d when a new setpoint has been received
float last_setpoint[3] = {0, 0, 0}; float last_setpoint[3] = {0, 0, 0};
float last_angle = 0; // angle in radians (-PI .. PI) float last_angle = 0; // angle in radians (-PI .. PI)
float last_thrust = 0; // default 10% thrust for when the drone gets armed float last_thrust = 0; // default 10% thrust for when the drone gets armed
float velocity[3] = {0, 0, 0}; float velocity[3] = {0, 0, 0};
@@ -255,6 +255,20 @@ private:
*/ */
void send_setpoint() void send_setpoint()
{ {
setpoint_count++;
if (setpoint_count == 20)
{
this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
RCLCPP_INFO(this->get_logger(), "Set to offboard mode");
// arm the drone
this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0);
RCLCPP_INFO(this->get_logger(), "Arm command sent");
this->last_thrust = -0.1; // spin motors at 10% thrust
armed = true;
}
RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust); RCLCPP_INFO(this->get_logger(), "current values: %f %f %f %f", last_setpoint[0], last_setpoint[1], last_setpoint[2], last_thrust);
if (!flying) if (!flying)
{ {