Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

Rover position controller: Modernize implementation

Browse Source 
This commit moves the steering output from yaw to the roll channel to better reflect the lateral control input / reaction mapping. It also removes old unused parameters and modernizes the mainloop to remove unnecessary polling.
This commit is contained in:
Lorenz Meier
2021-02-13 20:39:40 +01:00
parent bb0b4db028
commit ec2cf70276
5 changed files with 59 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix
+7 -4
View File
@@ -13,25 +13,28 @@ Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (ro
See the README for more information on the scaler format.
Output 0
Output 1 - Empty
-----------------------------------------
Z:
Steering mixer using roll on output 1
Output 2 - Steering mixer using yaw
------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000 5000
S: 0 2 10000 10000 0 -10000 10000
Output 2
Output 3 - Left row of wheels using yaw and throttle (1s rise time)
------------------------------------------
M: 2
O: 10000 10000 0 -10000 10000 10000
S: 0 2 -500 -500 0 0 10000
S: 0 3 10000 10000 0 -10000 10000
Output 3
Output 4 - Right row of wheels using yaw and throttle (1s rise time)
------------------------------------------
M: 2
O: 10000 10000 0 -10000 10000 10000
S: 0 2 500 500 0 0 10000
S: 0 3 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/rover_generic.main.mix
+6 -4
View File
@@ -13,23 +13,25 @@ Inputs to the mixer come from channel group 0 (vehicle attitude), channels 2 (ya
See the README for more information on the scaler format.
Output 0
Output 1: Empty
---------------------------------------
Z:
Steering mixer using roll on output 1
Output 2: Steering mixer using yaw, with 0.5s rise time
---------------------------------------
M: 1
O: 10000 10000 0 -10000 10000 5000
S: 0 2 10000 10000 0 -10000 10000
Output 2
Output 3: Empty
---------------------------------------
This mixer is empty.
Z:
Output 3
Output 4: Throttle with 2s rise time
---------------------------------------
M: 1
O: 10000 10000 0 -10000 10000 20000
S: 0 3 10000 10000 0 -10000 10000
src/modules/rover_pos_control/RoverPositionControl.cpp
+24 -50
View File
@@ -104,17 +104,6 @@ RoverPositionControl::vehicle_control_mode_poll()
}
}
void
RoverPositionControl::manual_control_setpoint_poll()
{
bool manual_updated;
orb_check(_manual_control_setpoint_sub, &manual_updated);
if (manual_updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
}
}
void
RoverPositionControl::position_setpoint_triplet_poll()
{
@@ -357,7 +346,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 +357,15 @@ RoverPositionControl::run()
parameters_update(true);
/* wakeup source(s) */
px4_pollfd_struct_t fds[5];
px4_pollfd_struct_t fds[3];
/* Setup of loop */
fds[0].fd = _global_pos_sub;
fds[0].events = POLLIN;
fds[1].fd = _manual_control_setpoint_sub;
fds[1].fd = _local_pos_sub; // Added local position as source of position
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].events = POLLIN;
fds[4].fd = _local_pos_sub; // Added local position as source of position
fds[4].events = POLLIN;
while (!should_exit()) {
@@ -397,7 +381,6 @@ RoverPositionControl::run()
/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
attitude_setpoint_poll();
//manual_control_setpoint_poll();
_vehicle_acceleration_sub.update();
@@ -406,8 +389,8 @@ 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) {
// Respond to a position update
if (fds[0].revents & POLLIN || fds[1].revents & POLLIN) {
perf_begin(_loop_perf);
/* load local copies */
@@ -476,7 +459,8 @@ RoverPositionControl::run()
perf_end(_loop_perf);
}
if (fds[3].revents & POLLIN) {
// Respond to an attitude update and run the attitude controller if enabled
if (fds[2].revents & POLLIN) {
vehicle_attitude_poll();
@@ -490,37 +474,28 @@ RoverPositionControl::run()
}
if (fds[1].revents & POLLIN) {
// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
// returning immediately and this loop will eat up resources.
// Manual pass-through if no other control mode is enabled
if (manual_mode) {
/* manual/direct control */
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint);
if (manual_mode) {
/* manual/direct control */
//PX4_INFO("Manual mode!");
_act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual_control_setpoint.y;
_act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual_control_setpoint.x;
_act_controls.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.r; //TODO: Readd yaw scale param
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
}
_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y;
_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x;
// Set heading from the manual roll input channel
_act_controls.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.y; // Nominally yaw: _manual_control_setpoint.r;
// Set throttle from the manual throttle channel
_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);
// publish the controller output
if (_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_attitude_enabled ||
_control_mode.flag_control_position_enabled ||
manual_mode) {
// timestamp and publish controls
_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);
}
_actuator_controls_pub.publish(_act_controls);
}
}
@@ -531,7 +506,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");
}
src/modules/rover_pos_control/RoverPositionControl.hpp
-4
View File
@@ -62,7 +62,6 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -109,7 +108,6 @@ private:
int _pos_sp_triplet_sub{-1};
int _att_sp_sub{-1};
int _vehicle_attitude_sub{-1};
int _sensor_combined_sub{-1};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -121,7 +119,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{};
uORB::SubscriptionData<vehicle_acceleration_s> _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
@@ -181,7 +178,6 @@ private:
*/
void parameters_update(bool force = false);
void manual_control_setpoint_poll();
void position_setpoint_triplet_poll();
void attitude_setpoint_poll();
void vehicle_control_mode_poll();
src/modules/rover_pos_control/rover_pos_control_params.c
+22 -20
View File
@@ -49,36 +49,50 @@
*/
/**
* L1 distance
*
* This is the waypoint radius
* Distance from front axle to rear axle
*
* A value of 0.31 is typical for 1/10 RC cars.
*
* @unit m
* @min 0.0
* @max 100.0
* @decimal 3
* @increment 0.01
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f);
/**
* L1 distance
*
* This is the distance at which the next waypoint is activated. This should be set
* to about 2-4x of GND_WHEEL_BASE and not smaller than one meter (due to GPS accuracy).
*
*
* @unit m
* @min 1.0
* @max 50.0
* @decimal 1
* @increment 0.1
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_L1_DIST, 5.0f);
PARAM_DEFINE_FLOAT(GND_L1_DIST, 1.0f);
/**
* L1 period
*
* This is the L1 distance and defines the tracking
* point ahead of the rover it's following.
* Using values around 2-5 for a traxxas stampede. Shorten
* Use values around 2-5m for a 0.3m wheel base. Tuning instructions: Shorten
* slowly during tuning until response is sharp without oscillation.
*
* @unit m
* @min 0.0
* @min 0.5
* @max 50.0
* @decimal 1
* @increment 0.5
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 10.0f);
PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 5.0f);
/**
* L1 damping
@@ -246,18 +260,6 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
*/
PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
/**
* Distance from front axle to rear axle
*
*
* @unit m
* @min 0.0
* @decimal 3
* @increment 0.01
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 2.0f);
/**
* Maximum turn angle for Ackerman steering.
* At a control output of 0, the steering wheels are at 0 radians.