From ec2cf702760e812654c07701e0771e0d00401ce7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:39:40 +0100 Subject: [PATCH] Rover position controller: Modernize implementation 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. --- .../mixers/rover_diff_and_servo.main.mix | 11 ++- .../mixers/rover_generic.main.mix | 10 ++- .../RoverPositionControl.cpp | 74 ++++++------------- .../RoverPositionControl.hpp | 4 - .../rover_pos_control_params.c | 42 ++++++----- 5 files changed, 59 insertions(+), 82 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix b/ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix index c6c8dcbe92..3f707dd21a 100644 --- a/ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix +++ b/ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/rover_generic.main.mix b/ROMFS/px4fmu_common/mixers/rover_generic.main.mix index 9605ee3edd..fee98ac1de 100644 --- a/ROMFS/px4fmu_common/mixers/rover_generic.main.mix +++ b/ROMFS/px4fmu_common/mixers/rover_generic.main.mix @@ -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 diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 2b592da5fc..20592f15b8 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -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"); } diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index f503d367f2..241f272287 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -62,7 +62,6 @@ #include #include #include -#include #include #include #include @@ -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_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(); diff --git a/src/modules/rover_pos_control/rover_pos_control_params.c b/src/modules/rover_pos_control/rover_pos_control_params.c index e168732ef5..bdc82044d8 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -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.