diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index a19d9b1989..b6fc159692 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -357,7 +357,6 @@ RoverPositionControl::run() _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* rate limit control mode updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); @@ -369,19 +368,17 @@ RoverPositionControl::run() parameters_update(true); /* wakeup source(s) */ - px4_pollfd_struct_t fds[5]; + px4_pollfd_struct_t fds[4] {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; fds[1].fd = _manual_control_setpoint_sub; fds[1].events = POLLIN; - fds[2].fd = _sensor_combined_sub; + fds[2].fd = _vehicle_attitude_sub; // Poll attitude fds[2].events = POLLIN; - fds[3].fd = _vehicle_attitude_sub; // Poll attitude + fds[3].fd = _local_pos_sub; // Added local position as source of position fds[3].events = POLLIN; - fds[4].fd = _local_pos_sub; // Added local position as source of position - fds[4].events = POLLIN; while (!should_exit()) { @@ -407,7 +404,7 @@ RoverPositionControl::run() bool manual_mode = _control_mode.flag_control_manual_enabled; /* only run controller if position changed */ - if (fds[0].revents & POLLIN || fds[4].revents & POLLIN) { + if (fds[0].revents & POLLIN || fds[3].revents & POLLIN) { perf_begin(_loop_perf); /* load local copies */ @@ -476,7 +473,7 @@ RoverPositionControl::run() perf_end(_loop_perf); } - if (fds[3].revents & POLLIN) { + if (fds[2].revents & POLLIN) { vehicle_attitude_poll(); @@ -505,24 +502,6 @@ RoverPositionControl::run() _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; } } - - if (fds[2].revents & POLLIN) { - - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - - //orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att); - _act_controls.timestamp = hrt_absolute_time(); - - /* Only publish if any of the proper modes are enabled */ - if (_control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_attitude_enabled || - _control_mode.flag_control_position_enabled || - manual_mode) { - /* publish the actuator controls */ - _actuator_controls_pub.publish(_act_controls); - } - } - } orb_unsubscribe(_control_mode_sub); @@ -531,9 +510,6 @@ RoverPositionControl::run() orb_unsubscribe(_manual_control_setpoint_sub); orb_unsubscribe(_pos_sp_triplet_sub); orb_unsubscribe(_vehicle_attitude_sub); - orb_unsubscribe(_sensor_combined_sub); - - warnx("exiting.\n"); } int RoverPositionControl::task_spawn(int argc, char *argv[]) diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 09b128791d..ae65b21bf2 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -61,7 +61,6 @@ #include #include #include -#include #include #include #include @@ -110,7 +109,6 @@ private: int _pos_sp_triplet_sub{-1}; int _att_sp_sub{-1}; int _vehicle_attitude_sub{-1}; - int _sensor_combined_sub{-1}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; @@ -122,7 +120,6 @@ private: vehicle_local_position_s _local_pos{}; /**< global vehicle position */ actuator_controls_s _act_controls{}; /**< direct control of actuators */ vehicle_attitude_s _vehicle_att{}; - sensor_combined_s _sensor_combined{}; SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};