PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
8.52k stars 13.51k forks source link

[Bug] Actuator not controlled in Offboard Mode #23720

Closed serkanMzlm closed 1 month ago

serkanMzlm commented 1 month ago

Describe the bug

I broadcast "fmu/in/actuator_motors" for actuator control in offboard mode, but there was no change in the PWM signal at the IO or FMU outputs.

To Reproduce

  1. I connected the PX4 to the computer.

  2. I made the necessary settings in the PX4 section for MicroXRCEAgent and connected it to the computer with the FTDI cable.

  3. I published a message of type px4_msgs::msg::OffboardControlMode.

    
    offboardControlModeMsg msg{};
    msg.position = false;
    msg.velocity = false;
    msg.acceleration = false;
    msg.attitude = false;
    msg.body_rate = false;
    msg.direct_actuator = true;
    msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
    pub.offboard_control_mode->publish(msg);
  4. I published a message of type px4_msgs::msg::ActuatorMotors.

    px4_msgs::msg::ActuatorMotors sp_motors{};
    for (int i = 0; i < 12; ++i)
    {
        sp_motors.control[i] = 1;
    }
    sp_motors.reversible_flags = 0;
    sp_motors.timestamp = this->get_clock()->now().nanoseconds() / 1000;
    sp_motors.timestamp_sample = sp_motors.timestamp;
    pub.motor->publish(sp_motors);

Expected behavior

In this part, I waited for the PWM to change at the IO or FMU outputs.

Screenshot / Media

No response

Flight Log

No response

Software Version

1.15

Flight controller

No response

Vehicle type

Multicopter

How are the different components wired up (including port information)

There is no problem with the wiring

Additional context

No response

serkanMzlm commented 1 month ago

My goal is to get PWM output to run the 6 motors I have installed externally, in addition to the 4 motors required to fly the drone.

beniaminopozzan commented 1 month ago

Hello, did you put the drone into offboard mode? Where you able to arm?

serkanMzlm commented 1 month ago

@beniaminopozzan Yes, it goes into offboard mode and when I arm it, there is no change in the PWM output. The "ros2 topic echo /fmu/out/vehicle_status" command runs in the terminal on the right.

image

Note: I can control the engines in the Gazebo simulation environment with these codes, but I cannot do this in the real PX4.

There is also a problem in the simulation environment, the engines disarm after a certain period of time, but the engines I will add as extras are not engines related to drone flight dynamics, so they should not only depend on the arm disarm status.

What should I set the outputs on the IO port to?

image

AlexisTM commented 1 month ago

Did you follow: https://docs.px4.io/main/en/config/actuators.html? If you do not assign motor outputs, you do not get a motor output.

serkanMzlm commented 1 month ago

@AlexisTM The motors selection is set to "main 1 -> peripheral via actuator set 1", but is this correct, for example? How do I know which ports control which outputs between ActuatorMotors and ActuatorServos?

serkanMzlm commented 1 month ago

QGC only has motor 1 2 3 4, there is no servo option.

image

beniaminopozzan commented 1 month ago

@AlexisTM The motors selection is set to "main 1 -> peripheral via actuator set 1", but is this correct, for example? How do I know which ports control which outputs between ActuatorMotors and ActuatorServos?

If you want to use fmu/in/actuator_motors then you should set your motors (in the actuator tabs) as Motor 1, Motor 2, etc This allows you to switch between different modes (Offboard, Position, etc) because PX4 will map the internal thrust and torque setpoints to motor commands. This is done by the control allocator if the output is set as Motor x. Of course, in Offboard mode with direct command the control allocator is disabled.

If you want to configure your Motors as Periferal with actuator set z then you should use refer to https://docs.px4.io/main/en/payloads/generic_actuator_control.html#generic-actuator-control-with-mavlink In this scenario you have to send a vehicleCommand message that repliates MAV_CMD_DO_SET_ACTUATOR See here for the implementation: https://github.com/PX4/PX4-Autopilot/blob/main/src/lib/mixer_module/functions/FunctionActuatorSet.hpp tl,dr: send a VehicleCommand message with command VEHICLE_CMD_DO_SET_ACTUATOR = 187 and the desired output on the paramx fields.

I'd need to double check why it works on gazebo but not on reality. My guess is that Gazebo blindly listens to the actuatorMotor topic so the actual motor assignment is ignored

serkanMzlm commented 1 month ago

@beniaminopozzan Currently, as a solution, I send VEHICLE_CMD_ACTUATOR_TEST and send the value of whichever engine I want to control. This way it worked perfectly.

vehicleCommandMsg msg{};
    msg.param1 = param1;
    msg.param2 = 1.0f;
    msg.param5 = 1300.0f + param5;
    msg.command = vehicleCommandMsg::VEHICLE_CMD_ACTUATOR_TEST;
    msg.target_system = 1;
    msg.target_component = 1;
    msg.source_system = 255;
    msg.source_component = 1;
    msg.from_external = true;
    msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
    pub.vehicle_command->publish(msg);
mohdkhanBechamo commented 1 month ago

@beniaminopozzan Currently, as a solution, I send VEHICLE_CMD_ACTUATOR_TEST and send the value of whichever engine I want to control. This way it worked perfectly.

vehicleCommandMsg msg{};
    msg.param1 = param1;
    msg.param2 = 1.0f;
    msg.param5 = 1300.0f + param5;
    msg.command = vehicleCommandMsg::VEHICLE_CMD_ACTUATOR_TEST;
    msg.target_system = 1;
    msg.target_component = 1;
    msg.source_system = 255;
    msg.source_component = 1;
    msg.from_external = true;
    msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
    pub.vehicle_command->publish(msg);

@serkanMzlm Hi, I am trying to acheive the same solution. did you have to send the px4_msgs::msg::OffboardControlMode message in addition to sending the Vehicle command msg?

serkanMzlm commented 1 month ago

@mohdkhanBechamo All you have to do is find your engine numbers and simply broadcast the vehicleCommandMsg message.