SITL with gazebo doesn't receive the actuator control published by example code

Hi all,

I’m newbie in px4 and trying to make example code.

What I want to is to give reference signal (e.g. : 3211 roll angle signal) to the quadrotor.

As the first step of that, I make some example code as below.

The code subscribes vehicle_attitude.h, and publish actuator_controls.h.

The problem is that when I type px4_tutorial in px4shell, terminal prints attitude but the quadrotor doesn’t react to actuator_controls published by px4_tutorial.

What am I missed?

Also, there are any difference between actuator_controls_0 and actuator_controls_1 which are both name of the topic “actuator_controls”.

thanks and sorry for my poor english.

#include <px4_platform_common/log.h>
#include <uORB/topics/sensor_combined.h>
#include <px4_platform_common/posix.h>
#include <uORB/topics/vehicle_attitude.h>
#include <string.h>
#include <uORB/topics/actuator_controls.h>

__EXPORT int px4_tutorial_main(int argc, char *argv[]);

int px4_tutorial_main(int argc, char *argv[]) {
	PX4_INFO("Hello Sky!");

	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

	struct actuator_controls_s ctr;
	memset(&ctr, 0, sizeof(ctr));
	orb_advert_t ctr_pub = orb_advertise(ORB_ID(actuator_controls_0), &ctr);

	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

	orb_set_interval(sensor_sub_fd, 200);

	px4_pollfd_struct_t fds[] = {
		{ .fd = sensor_sub_fd,  .events = POLLIN},
	};

	int error_counter = 0;

	for (int i = 0; i < 5; i++) {
		int poll_ret = px4_poll(fds, 1, 1000);
		if (poll_ret == 0) {
			PX4_ERR("Got no data within a second");
		} else if (poll_ret <0 ) {
			if (error_counter < 10 || error_counter % 50 == 0) {
				PX4_ERR("ERROR return value from poll(): %d", poll_ret);
			}
			error_counter++;

		} else {
			if (fds[0].revents & POLLIN) {
				struct sensor_combined_s raw;
				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
				PX4_INFO("Attitude:\t%8.4f\t%8.4f\t%8.4f",
				       	(double)raw.accelerometer_m_s2[0],
					(double)raw.accelerometer_m_s2[1],
					(double)raw.accelerometer_m_s2[2]);
					
					att.q[0] = 1;
					att.q[1] = 2;
					att.q[2] = 3;
					orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
			}
		}
	}

	ctr.control[0] = 0;
	ctr.control[1] = 0;
	ctr.control[2] = 0;				
	ctr.control[3] = 1.0f;

	orb_publish(ORB_ID(actuator_controls_0), ctr_pub, &ctr);
	
	PX4_INFO("exiting");

	return 0;
}

You need to do at least two things to get this to work in SITL:

  1. Disable lockstep, otherwise it might get out of sync and everything will stop, see: https://dev.px4.io/master/en/simulation/index.html#disable-lockstep-simulation

  2. Disable other modules that are started and are doing controls. E.g. for fixedwing it should be these:
    https://github.com/PX4/Firmware/blob/b7b23a521aea3de10afad54e01b60a93ac1d94f5/ROMFS/px4fmu_common/init.d/rc.fw_apps#L16-L17
    Otherwise you will fight the existing controllers.