diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 7d5fc91160..eac2e898ca 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -72,8 +72,8 @@ RoverPositionControl::~RoverPositionControl() bool RoverPositionControl::init() { - if (!_vehicle_attitude_sub.registerCallback()) { - PX4_ERR("vehicle attitude callback registration failed!"); + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle angular velocity callback registration failed!"); return false; } @@ -187,6 +187,14 @@ RoverPositionControl::attitude_setpoint_poll() } } +void +RoverPositionControl::vehicle_attitude_poll() +{ + if (_att_sub.updated()) { + _att_sub.copy(&_vehicle_att); + } +} + bool RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) @@ -385,11 +393,15 @@ RoverPositionControl::Run() { parameters_update(true); - if (_vehicle_attitude_sub.update(&_vehicle_att)) { + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); attitude_setpoint_poll(); + vehicle_attitude_poll(); manual_control_setpoint_poll(); _vehicle_acceleration_sub.update(); @@ -527,7 +539,7 @@ int RoverPositionControl::print_usage(const char *reason) ### Description Controls the position of a ground rover using an L1 controller. -Publishes `actuator_controls_0` messages at a constant 250Hz. +Publishes `actuator_controls_0` messages at IMU_GYRO_RATEMAX. ### Implementation Currently, this implementation supports only a few modes: diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 1104939987..4be4536773 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -99,7 +100,7 @@ public: private: void Run() override; - uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -112,6 +113,7 @@ private: uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};