Hi,
I am trying to write an application base on the px4_simple_app with which I can give thrust on the quadrocopter in the gazebo simulation started with
make posix_sitl_default gazebo
My simple app publishes to the actuator_control topic to the control[3] channel. Here is the code
/* subscribe to actuator_controls topic */
int act_sub_fd = orb_subscribe(ORB_ID(actuator_controls));
/* limit the update rate to 5 Hz */
orb_set_interval(act_sub_fd, 200);
/* advertise actuator topic to give thrust */
struct actuator_controls_s act;
memset(&act, 0, sizeof(act));
orb_advert_t act_pub = orb_advertise(ORB_ID(actuator_controls), &act);
/* one could wait for multiple topics with this technique, just using one here */
px4_pollfd_struct_t fds[] = {
{ .fd = act_sub_fd, .events = POLLIN },
};
int error_counter = 0;
for (int i = 0; i < 5; i++) {
/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
int poll_ret = px4_poll(fds, 1, 1000);
/* handle the poll result */
if (poll_ret == 0) {
/* this means none of our providers is giving us data */
PX4_ERR("Got no data within a second");
} else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0) {
/* use a counter to prevent flooding (and slowing us down) */
PX4_ERR("ERROR return value from poll(): %d", poll_ret);
}
error_counter++;
} else {
if (fds[0].revents & POLLIN) {
/* obtained data for the second file descriptor */
struct actuator_controls_s raw3;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(actuator_controls), act_sub_fd, &raw3);
PX4_INFO("Thrust Actuator:\t%8.4f",
(double)raw3.control[3]);
// Try to give the HippoC a thrust
act.control[3] = 0.5f;
orb_publish(ORB_ID(actuator_controls), act_pub, &act);
}
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
}
}
PX4_INFO("exiting");
return 0;
}
If I run this app in the terminal using
psh> px4_simple_app
the publisher publishs to the actuator_controls topic as I can see from the output of the subscriber. The problem is that in the simualtion nothing happens with the quadrocopter. Does anyone has an idea what I do wrong?
Best regards
Nils Rottmann