[Bug] actuator testing not work in SITL with gazebo classic

[hewei-io]

Describe the bug

When PX4 runs on Pixhawk hardware, individual actuators (motors, servos) can be tested in the following two ways:

• Using actuator_test in nsh
• Selecting Tool -> Vehicle Setup -> Actuators -> Actuator Testing in QGC

However, when running simulations in SITL with Gazebo Classic, neither method works.

This may not be a bug, as it seems to be intended behavior. However, being able to conduct actuator testing in SITL is especially useful for debugging airframe scripts and .sdf files. Additionally, addressing this would help maintain consistency between simulation and real hardware.

---

Why This Occurs

In SITL with Gazebo Classic, PX4 sends actuator outputs to the Gazebo Classic plugin using the MAVLink message:

• HIL_ACTUATORS_CONTROL (93), which is linked to
• the uORB topic actuator_outputs_sim (transferred from actuator_outputs in src/modules/simulation/pwm_out_sim).

In SITL with Gazebo Classic, the publisher and subscriber for HIL_ACTUATORS_CONTROL are:

• Publisher: simulator_mavlink module
• Subscriber: libgazebo_mavlink_interface.so Gazebo Classic plugin

On both sides, actuator_outputs_sim is processed only when the system is armed.
@src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

void SimulatorMavlink::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg)
{
        memset(msg, 0, sizeof(mavlink_hil_actuator_controls_t));

        msg->time_usec = hrt_absolute_time();

        bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);

        if (armed) {
                for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
                        msg->controls[i] = _actuator_outputs.output[i];
                }
        }
  
        msg->mode = mode_flag_custom;
        msg->mode |= (armed) ? mode_flag_armed : 0;
        msg->flags = 0;

#if defined(ENABLE_LOCKSTEP_SCHEDULER)
        msg->flags |= 1;
#endif
}

@Tools/simulation/gazebo-classic/sitl_gazebo-classic/src/gazebo_mavlink_interface.cpp

void GazeboMavlinkInterface::handle_actuator_controls() {
  bool armed = mavlink_interface_->GetArmedState();
  
  #if GAZEBO_MAJOR_VERSION >= 9
      last_actuator_time_ = world_->SimTime();
  #else
      last_actuator_time_ = world_->GetSimTime();
  #endif

  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++) {
    if (armed) {
      input_reference_[i] = (actuator_controls[input_index_[i]] + input_offset_[i])
          * input_scaling_[i] + zero_position_armed_[i];
      // std::cout << input_reference_ << ", ";
    } else {
      input_reference_[i] = zero_position_disarmed_[i];
      // std::cout << input_reference_ << ", ";
    }
  }
  // std::cout << "Input Reference: " << input_reference_.transpose() << std::endl;
  received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator();
}

---

Possible Solution

TL;DR:
Simply change if (armed) { to if (true) { in both pasted codes above.

D;R:
However, this may cause other issues, especially for servos when disarmed.

The logic for mapping actuator_otputs to actuator_outputs_sim in src/modules/simulation/pwm_out_sim is as follows:

• 900U -> 0.0f
• [1000U,2000U] -> [0.0f,1.0f] for non-reversible motors
• [1000U,2000U] -> [-1.0f,1.0f] for others (i.e. reversible motors, servos, etc.)

In the third case, both 900U and 1500U map to 0.0f, which may be appropriate for reversible motors, as 900U indicates initiation but not activation for ESC. However, for servos, manufacturers design a continuous interval of PWM signals to correspond to a continuous interval of positions, with 1500U typically representing the neutral position. This creates inconsistency between real hardware and SITL when disarmed:

	actuator_outputs/PWM	actuator_outputs_SIM	servo position
real hardware	900U	-	not NEUTRAL
SITL with gazebo classic	900U	0	NEUTRAL

This inconsistency is addressed in Tools/simulation/gazebo-classic/sitl_gazebo-classic/src/gazebo_mavlink_interface.cpp by applying different zero positions for armed and disarmed states.

void GazeboMavlinkInterface::handle_actuator_controls() {
// ...
    if (armed) {
      input_reference_[i] = (actuator_controls[input_index_[i]] + input_offset_[i])
          * input_scaling_[i] + zero_position_armed_[i];
      // std::cout << input_reference_ << ", ";
    } else {
      input_reference_[i] = zero_position_disarmed_[i];
      // std::cout << input_reference_ << ", ";
    }
// ...
}

If you revise if (armed) { to if (true) {, this inconsistency will manifest when running make px4_sitl gazebo-classic_tiltrotor: the tilt rotor will tilt about 45 degrees when disarmed, instead of remaining in the upward position as intended.

---

Addressing the Inconsistency Issue

The key point is not whether the system is armed, but rather in which case 900U (i.e., PWM_SIM_DISARMED_MAGIC) is mapped to 0.0f. For servos, mapping 900U to -1.0f may be more appropriate, as follows:
@src/modules/simulation/pwm_out_sim/PWMSim.cpp

bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
                           unsigned num_control_groups_updated)
{
        // Only publish once we receive actuator_controls (important for lock-step to work correctly)
        if (num_control_groups_updated > 0) {
                actuator_outputs_s actuator_outputs{};
                actuator_outputs.noutputs = num_outputs;

                const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();

                for (int i = 0; i < (int)num_outputs; i++) {
                        if (outputs[i] != PWM_SIM_DISARMED_MAGIC) {

                                OutputFunction function = _mixing_output.outputFunction(i);
                                bool is_reversible = reversible_outputs & (1u << i);
                                float output = outputs[i];

                                if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)
                                    && !is_reversible) {
                                        // Scale non-reversible motors to [0, 1]
                                        actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC);

                                } else {
                                        // Scale everything else to [-1, 1]
                                        const float pwm_center = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2.f;
                                        const float pwm_delta = (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC) / 2.f;
                                        actuator_outputs.output[i] = (output - pwm_center) / pwm_delta;
                                }
                        }
                        else{ // REVISED BEGIN
                                OutputFunction function = _mixing_output.outputFunction(i);
                                if (((int)function >= (int)OutputFunction::Servo1 && (int)function <= (int)OutputFunction::ServoMax)) { // servos with PWM_SIM_DISARMED_MAGIC
                                        actuator_outputs.output[i] = -1.0f;
                                }
                        } // REVISED END
                }

                actuator_outputs.timestamp = hrt_absolute_time();
                _actuator_outputs_sim_pub.publish(actuator_outputs);
                return true;
        }

        return false;
}

Software Version

PX4 v1.14.3