Compare commits

...

8 Commits

Author SHA1 Message Date
Jaeyoung Lim 69d6fc2a7f Do not use a differential input 2022-08-22 11:07:03 +02:00
Jaeyoung Lim 02f0e35e3f Increase acceptance radius to 5m 2022-08-21 16:22:09 +02:00
Jaeyoung Lim feda568e20 Define boat as a groundrover vehicle type 2022-08-21 15:37:41 +02:00
Jaeyoung Lim fe5f2b501b Run rate control loop only once 2022-08-21 14:48:21 +02:00
Jaeyoung Lim fa1ecc60d3 Add minimum ground speed parameter 2022-08-20 18:35:31 +02:00
Jaeyoung Lim 40eafd458b Delete unused wheel base parameter 2022-08-20 18:04:11 +02:00
Jaeyoung Lim fe8dd05624 Use common rate control library
Increase ground speed trim
Use rate control for pos control
Controls
2022-08-20 17:35:53 +02:00
Jaeyoung Lim 535dabe00d Add Rover Rates Control
This commit adds a angular rates controller controlling the yaw rate of the rover. This is also used for the previous attitude controller in order to stabilize the vehicles.

This is important for vehicles operating in low friction surfaces or boats
2022-08-19 22:02:37 +02:00
10 changed files with 245 additions and 42 deletions
@@ -26,7 +26,6 @@ param set-default NAV_LOITER_RAD 2
param set-default CBRK_AIRSPD_CHK 162128
param set-default GND_MAX_ANG 0.6
param set-default GND_WHEEL_BASE 2.0
param set-default CA_AIRFRAME 5
@@ -26,7 +26,6 @@ param set-default NAV_LOITER_RAD 2
param set-default CBRK_AIRSPD_CHK 162128
param set-default GND_MAX_ANG 0.6
param set-default GND_WHEEL_BASE 2.0
param set-default CA_AIRFRAME 6
@@ -33,7 +33,6 @@ param set-default NAV_LOITER_RAD 2
param set-default CBRK_AIRSPD_CHK 162128
param set-default GND_MAX_ANG 0.6
param set-default GND_WHEEL_BASE 3.0
param set-default CA_AIRFRAME 5
@@ -13,20 +13,19 @@ param set-default GND_SPEED_I 8
param set-default GND_SPEED_IMAX 0.125
param set-default GND_SPEED_P 2
param set-default GND_SPEED_THR_SC 1
param set-default GND_SPEED_TRIM 1
param set-default GND_SPEED_TRIM 3
param set-default GND_THR_CRUISE 0.85
param set-default GND_THR_MAX 1
param set-default GND_THR_MIN 0
param set-default MIS_LTRMIN_ALT 0.01
param set-default MIS_TAKEOFF_ALT 0.01
param set-default NAV_ACC_RAD 0.5
param set-default NAV_ACC_RAD 5
param set-default NAV_LOITER_RAD 2
param set-default CBRK_AIRSPD_CHK 162128
param set-default GND_MAX_ANG 0.6
param set-default GND_WHEEL_BASE 2.0
param set-default CA_AIRFRAME 9
@@ -34,7 +34,6 @@ param set-default GND_THR_MAX 0.5
# Differential drive acts like ackermann steering with a maximum turn angle of 60 degrees, or pi/3 radians
param set-default GND_MAX_ANG 1.042
param set-default GND_WHEEL_BASE 0.17
# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
# to support negative throttle.
+2 -1
View File
@@ -120,7 +120,8 @@ bool is_fixed_wing(const vehicle_status_s &current_status)
bool is_ground_rover(const vehicle_status_s &current_status)
{
return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER;
return (current_status.system_type == VEHICLE_TYPE_GROUND_ROVER ||
current_status.system_type == VEHICLE_TYPE_BOAT);
}
// End time for currently blinking LED message, 0 if no blink message
@@ -30,6 +30,7 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE modules__rover_pos_control
MAIN rover_pos_control
@@ -37,6 +38,7 @@ px4_add_module(
RoverPositionControl.cpp
RoverPositionControl.hpp
DEPENDS
RateControl
l1
pid
)
@@ -102,6 +102,10 @@ void RoverPositionControl::parameters_update(bool force)
_param_speed_d.get(),
_param_speed_imax.get(),
_param_gndspeed_max.get());
_rate_control.setGains(matrix::Vector3f(0.0, 0.0, _param_rate_p.get()), matrix::Vector3f(0.0, 0.0, _param_rate_i.get()),
matrix::Vector3f(0.0, 0.0, _param_rate_d.get()));
_rate_control.setFeedForwardGain(matrix::Vector3f(0.0, 0.0, _param_rate_ff.get()));
_rate_control.setIntegratorLimit(matrix::Vector3f(0.0, 0.0, _param_rate_imax.get()));
}
}
@@ -151,6 +155,17 @@ RoverPositionControl::manual_control_setpoint_poll()
_attitude_sp_pub.publish(_att_sp);
} else if (_control_mode.flag_control_rates_enabled) {
// STABILIZED mode generate the attitude setpoint from manual user inputs
_rates_sp.roll = 0.0;
_rates_sp.pitch = 0.0;
_rates_sp.yaw = _manual_control_setpoint.y;
_rates_sp.thrust_body[0] = _manual_control_setpoint.z;
_rates_sp.timestamp = hrt_absolute_time();
_rates_sp_pub.publish(_rates_sp);
} else {
_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;
@@ -187,6 +202,14 @@ RoverPositionControl::attitude_setpoint_poll()
}
}
void
RoverPositionControl::rates_setpoint_poll()
{
if (_rates_sp_sub.updated()) {
_rates_sp_sub.copy(&_rates_sp);
}
}
void
RoverPositionControl::vehicle_attitude_poll()
{
@@ -195,6 +218,14 @@ RoverPositionControl::vehicle_attitude_poll()
}
}
void
RoverPositionControl::vehicle_angular_acceleration_poll()
{
if (_vehicle_angular_acceleration_sub.updated()) {
_vehicle_angular_acceleration_sub.copy(&_vehicle_angular_acceleration);
}
}
bool
RoverPositionControl::control_position(const matrix::Vector2d &current_position,
const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet)
@@ -287,21 +318,30 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
prev_wp(1));
_gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d);
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
///WIP: Convert Lateral acceleration demand to rate control reference
float min_speed = _param_gndspeed_min.get();
matrix::Vector2f saturated_speed_2d = (ground_speed_2d.norm() < min_speed) ? ground_speed_2d :
ground_speed_2d.normalized() * min_speed;
float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());
float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
_gnd_control.nav_lateral_acceleration_demand());
control_effort = math::constrain(control_effort, -1.0f, 1.0f);
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
float desired_yaw_rate = _gnd_control.nav_lateral_acceleration_demand() / saturated_speed_2d.norm();
_rates_sp.roll = 0.0;
_rates_sp.pitch = 0.0;
_rates_sp.yaw = desired_yaw_rate;
_rates_sp.thrust_body[0] = math::constrain(mission_throttle, 0.0f, 1.0f);
_rates_sp.timestamp = hrt_absolute_time();
_rates_sp_pub.publish(_rates_sp);
}
}
break;
case STOPPING: {
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
_rates_sp.roll = 0.0;
_rates_sp.pitch = 0.0;
_rates_sp.yaw = 0.0;
_rates_sp.thrust_body[0] = 0.0;
_rates_sp.timestamp = hrt_absolute_time();
_rates_sp_pub.publish(_rates_sp);
// Note _prev_wp is different to the local prev_wp which is related to a mission waypoint.
float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1),
(double)curr_wp(0), (double)curr_wp(1));
@@ -380,31 +420,63 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi
// quaternion attitude control law, qe is rotation from q to qd
const Quatf qe = Quatf(att.q).inversed() * Quatf(att_sp.q_d);
const Eulerf euler_sp = qe;
// PX4_INFO("Yaw error: %f", double(euler_sp(2)));
float control_effort = euler_sp(2) / _param_max_turn_angle.get();
control_effort = math::constrain(control_effort, -1.0f, 1.0f);
_rates_sp.roll = 0.0;
_rates_sp.pitch = 0.0;
_rates_sp.yaw = euler_sp(2) / _param_max_turn_angle.get();
_rates_sp.thrust_body[0] = math::constrain(att_sp.thrust_body[0], 0.0f, 1.0f);
_rates_sp.timestamp = hrt_absolute_time();
_rates_sp_pub.publish(_rates_sp);
}
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
void
RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, const vehicle_angular_acceleration_s &acc,
const vehicle_local_position_s &local_pos,
const vehicle_rates_setpoint_s &rates_sp)
{
float dt = (_control_rates_last_called > 0) ? hrt_elapsed_time(&_control_rates_last_called) * 1e-6f : 0.01f;
_control_rates_last_called = hrt_absolute_time();
const float control_throttle = att_sp.thrust_body[0];
const matrix::Vector3f vehicle_rates(rates.xyz[0], rates.xyz[1], rates.xyz[2]);
const matrix::Vector3f rates_setpoint(rates_sp.roll, rates_sp.pitch, rates_sp.yaw);
const matrix::Vector3f current_velocity(local_pos.vx, local_pos.vy, local_pos.vz);
bool lock_integrator = false;
if (current_velocity.norm() < _param_rate_i_minspeed.get()) {
_rate_control.resetIntegral();
}
const matrix::Vector3f angular_acceleration{acc.xyz};
const matrix::Vector3f desired_angular_acceleration = _rate_control.update(vehicle_rates, rates_setpoint,
angular_acceleration, dt,
lock_integrator);
_steering_input = math::constrain(desired_angular_acceleration(2), -1.0f, 1.0f);
_act_controls.control[actuator_controls_s::INDEX_YAW] = _steering_input;
const float control_throttle = rates_sp.thrust_body[0];
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, 1.0f);
}
void
RoverPositionControl::Run()
{
parameters_update(true);
/* run controller on gyro changes */
vehicle_angular_velocity_s angular_velocity;
if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
if (_vehicle_angular_velocity_sub.update(&_vehicle_rates)) {
// grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy
vehicle_angular_acceleration_poll();
/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
attitude_setpoint_poll();
rates_setpoint_poll();
vehicle_attitude_poll();
manual_control_setpoint_poll();
@@ -413,6 +485,10 @@ RoverPositionControl::Run()
/* update parameters from storage */
parameters_update();
if (!PX4_ISFINITE(_steering_input)) {
_steering_input = 0.0;
}
/* only run controller if position changed */
if (_local_pos_sub.update(&_local_pos)) {
@@ -492,6 +568,11 @@ RoverPositionControl::Run()
}
//Body Rate control
if (_control_mode.flag_control_rates_enabled) {
control_rates(_vehicle_rates, _vehicle_angular_acceleration, _local_pos, _rates_sp);
}
/* Only publish if any of the proper modes are enabled */
if (_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_attitude_enabled ||
@@ -41,14 +41,15 @@
* @author Marco Zorzi <mzorzi@student.ethz.ch>
*/
#include <float.h>
#include <float.h>
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/l1/ECL_L1_Pos_Controller.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <lib/pid/pid.h>
#include <lib/rate_control/rate_control.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
@@ -67,6 +68,9 @@
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_angular_acceleration.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -106,6 +110,7 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */
uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */
@@ -118,15 +123,20 @@ private:
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)};
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */
vehicle_rates_setpoint_s _rates_sp{}; /**< rate setpoint > */
vehicle_control_mode_s _control_mode{}; /**< control mode */
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
vehicle_local_position_s _local_pos{}; /**< global vehicle position */
actuator_controls_s _act_controls{}; /**< direct control of actuators */
vehicle_attitude_s _vehicle_att{};
vehicle_angular_acceleration_s _vehicle_angular_acceleration{};
vehicle_angular_velocity_s _vehicle_rates{};
trajectory_setpoint_s _trajectory_setpoint{};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
@@ -136,6 +146,7 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
hrt_abstime _control_rates_last_called{0};
hrt_abstime _manual_setpoint_last_called{0};
MapProjection _global_local_proj_ref{};
@@ -149,6 +160,8 @@ private:
uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position
ECL_L1_Pos_Controller _gnd_control;
RateControl _rate_control;
float _steering_input{0.0};
enum UGV_POSCTRL_MODE {
UGV_POSCTRL_MODE_AUTO,
@@ -179,6 +192,7 @@ private:
(ParamFloat<px4::params::GND_SPEED_TRIM>) _param_gndspeed_trim,
(ParamFloat<px4::params::GND_SPEED_MAX>) _param_gndspeed_max,
(ParamFloat<px4::params::GND_SPEED_MIN>) _param_gndspeed_min,
(ParamInt<px4::params::GND_SP_CTRL_MODE>) _param_speed_control_mode,
(ParamFloat<px4::params::GND_SPEED_P>) _param_speed_p,
@@ -187,12 +201,20 @@ private:
(ParamFloat<px4::params::GND_SPEED_IMAX>) _param_speed_imax,
(ParamFloat<px4::params::GND_SPEED_THR_SC>) _param_throttle_speed_scaler,
(ParamFloat<px4::params::GND_RATE_P>) _param_rate_p,
(ParamFloat<px4::params::GND_RATE_I>) _param_rate_i,
(ParamFloat<px4::params::GND_RATE_D>) _param_rate_d,
(ParamFloat<px4::params::GND_RATE_FF>) _param_rate_ff,
(ParamFloat<px4::params::GND_RATE_IMAX>) _param_rate_imax,
(ParamFloat<px4::params::GND_RATE_MAX>) _param_rate_max,
(ParamFloat<px4::params::GND_RATE_IMINSPD>) _param_rate_i_minspeed,
(ParamFloat<px4::params::GND_THR_MIN>) _param_throttle_min,
(ParamFloat<px4::params::GND_THR_MAX>) _param_throttle_max,
(ParamFloat<px4::params::GND_THR_CRUISE>) _param_throttle_cruise,
(ParamFloat<px4::params::GND_WHEEL_BASE>) _param_wheel_base,
(ParamFloat<px4::params::GND_MAX_ANG>) _param_max_turn_angle,
(ParamFloat<px4::params::GND_ATT_P>) _param_att_p,
(ParamFloat<px4::params::GND_MAN_Y_MAX>) _param_gnd_man_y_max,
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad /**< loiter radius for Rover */
)
@@ -204,8 +226,10 @@ private:
void position_setpoint_triplet_poll();
void attitude_setpoint_poll();
void rates_setpoint_poll();
void vehicle_control_mode_poll();
void vehicle_attitude_poll();
void vehicle_angular_acceleration_poll();
void manual_control_setpoint_poll();
/**
@@ -215,5 +239,8 @@ private:
const position_setpoint_triplet_s &_pos_sp_triplet);
void control_velocity(const matrix::Vector3f &current_velocity);
void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp);
void control_rates(const vehicle_angular_velocity_s &rates, const vehicle_angular_acceleration_s &acc,
const vehicle_local_position_s &local_pos,
const vehicle_rates_setpoint_s &rates_sp);
};
@@ -48,19 +48,6 @@
* Controller parameters, accessible via MAVLink
*/
/**
* Distance from front axle to rear axle
*
* A value of 0.31 is typical for 1/10 RC cars.
*
* @unit m
* @min 0.0
* @decimal 3
* @increment 0.01
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f);
/**
* L1 distance
*
@@ -247,6 +234,19 @@ PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f);
*/
PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f);
/**
* Minimum ground speed
*
*
* @unit m/s
* @min 0.0
* @max 40
* @decimal 1
* @increment 0.5
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_SPEED_MIN, 1.0f);
/**
* Maximum ground speed
*
@@ -275,6 +275,19 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f);
*/
PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
/**
* Attitude control P gain
*
*
* @unit rad
* @min 0.0
* @max 5.0
* @decimal 3
* @increment 0.01
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_ATT_P, 1.0f);
/**
* Max manual yaw rate
*
@@ -285,3 +298,87 @@ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f);
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f);
/**
* Rover Rate Proportional Gain
*
* @unit rad/s
* @min 0.0
* @max 10.0
* @decimal 3
* @increment 0.01
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_RATE_P, 5.0f);
/**
* Rover Rate Integral Gain
*
* @unit rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_RATE_I, 0.5f);
/**
* Rover Rate Integral Gain
*
* @unit rad/s
* @min 0.0
* @max 1.0
* @decimal 3
* @increment 0.005
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_RATE_D, 0.0f);
/**
* Rover Rate Proportional Gain
*
* @unit rad/s
* @min 0.0
* @max 10.0
* @decimal 3
* @increment 0.01
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_RATE_FF, 1.6f);
/**
* Rover Rate Maximum Integral Gain
*
* @unit rad/s
* @min 0.0
* @max 50.0
* @decimal 3
* @increment 0.005
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_RATE_IMAX, 1.0f);
/**
* Rover Rate Maximum Rate
*
* @unit rad/s
* @min 0.0
* @max 50.0
* @decimal 3
* @increment 0.005
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_RATE_MAX, 5.0f);
/**
* Rover Rate Integral Minimum speed
*
* @unit m/s
* @min 0.0
* @max 50.0
* @decimal 3
* @increment 0.005
* @group Rover Position Control
*/
PARAM_DEFINE_FLOAT(GND_RATE_IMINSPD, 1.0f);