dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

rover: publish actuator controls with rate of gyro 

Some ESC's with DShot don't initialize otherwise (as the 200Hz of the vehicle attitude rate is too low).
This commit is contained in:
benjinne 2021-04-26 02:53:20 -04:00 committed by GitHub
parent cc81c7d49e
commit 3664d9e4ce
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -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 &current_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:

4
src/modules/rover_pos_control/RoverPositionControl.hpp
View File

@ -67,6 +67,7 @@
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
@ -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)};