From 840d76ef5fe765ff9b1cba9660e00f2554c21f2c Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Mon, 12 Dec 2022 17:26:37 +0100 Subject: [PATCH] Add rate controller for rover This commit adds the rover rate controller for rover --- .../airframes/1060_gazebo-classic_rover | 1 - .../airframes/1061_gazebo-classic_r1_rover | 1 - .../airframes/1062_flightgear_tf-r1 | 1 - .../airframes/1070_gazebo-classic_boat | 10 +- .../airframes/50004_nxpcup_car_dfrobot_gpx | 1 - src/modules/rover_pos_control/CMakeLists.txt | 2 + .../RoverPositionControl.cpp | 121 +++++++++++++---- .../RoverPositionControl.hpp | 33 ++++- .../rover_pos_control_params.c | 123 ++++++++++++++++-- 9 files changed, 247 insertions(+), 46 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover index d58f5aebae..79757ad8fb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover @@ -22,7 +22,6 @@ param set-default NAV_ACC_RAD 0.5 param set-default NAV_LOITER_RAD 2 param set-default GND_MAX_ANG 0.6 -param set-default GND_WHEEL_BASE 2.0 param set-default CA_AIRFRAME 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover index ced7bb26cf..1ce042ae51 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover @@ -22,7 +22,6 @@ param set-default NAV_ACC_RAD 0.5 param set-default NAV_LOITER_RAD 2 param set-default GND_MAX_ANG 0.6 -param set-default GND_WHEEL_BASE 2.0 param set-default CA_AIRFRAME 6 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 index 40c5cb8d04..a879483923 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 @@ -29,7 +29,6 @@ param set-default NAV_ACC_RAD 0.5 param set-default NAV_LOITER_RAD 2 param set-default GND_MAX_ANG 0.6 -param set-default GND_WHEEL_BASE 3.0 param set-default CA_AIRFRAME 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat index c173598078..a778f48f48 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat @@ -13,16 +13,20 @@ 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 GND_RATE_P 0.08 +param set-default GND_RATE_D 0.02 +param set-default GND_RATE_FF 0.0 +param set-default GND_RATE_I 0.0 +param set-default GND_RATE_MAX 0.5 -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 GND_MAX_ANG 0.6 -param set-default GND_WHEEL_BASE 2.0 param set-default CA_AIRFRAME 9 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx b/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx index 6d4bd2f4f5..dc736b08f7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx +++ b/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx @@ -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. diff --git a/src/modules/rover_pos_control/CMakeLists.txt b/src/modules/rover_pos_control/CMakeLists.txt index 5e093862e7..2c42cce5fd 100644 --- a/src/modules/rover_pos_control/CMakeLists.txt +++ b/src/modules/rover_pos_control/CMakeLists.txt @@ -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 ) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 24909280e9..39946dd9c7 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -99,6 +99,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())); } } @@ -148,6 +152,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 { // Set heading from the manual roll input channel _yaw_control = _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw; @@ -181,6 +196,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() { @@ -189,6 +212,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 ¤t_position, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &pos_sp_triplet) @@ -223,6 +254,7 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, matrix::Vector2f ground_speed_2d(ground_speed); float mission_throttle = _param_throttle_cruise.get(); + float max_yaw_rate = _param_rate_max.get(); /* Just control the throttle */ if (_param_speed_control_mode.get() == 1) { @@ -273,21 +305,29 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, prev_wp(1)); _gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d); - _throttle_control = mission_throttle; - - 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); - _yaw_control = control_effort; + ///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; + ///TODO: Max yaw rate + 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 = math::constrain(desired_yaw_rate, -max_yaw_rate, max_yaw_rate); + _rates_sp.thrust_body[0] = math::constrain(mission_throttle, -1.0f, 1.0f); + _rates_sp.timestamp = hrt_absolute_time(); + _rates_sp_pub.publish(_rates_sp); } } break; case STOPPING: { - _yaw_control = 0.0f; - _throttle_control = 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)); @@ -364,31 +404,57 @@ 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); - - _yaw_control = control_effort; - - const float control_throttle = att_sp.thrust_body[0]; - - _throttle_control = math::constrain(control_throttle, 0.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); } +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 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 = bool(current_velocity.norm() < _param_rate_i_minspeed.get()); + + const matrix::Vector3f angular_acceleration{acc.xyz}; + const matrix::Vector3f torque = _rate_control.update(vehicle_rates, rates_setpoint, angular_acceleration, dt, + lock_integrator); + + _steering_input = math::constrain(_steering_input + torque(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(); @@ -397,6 +463,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)) { @@ -473,6 +543,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 || diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index ee1c65efa0..b0767ce77a 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -41,14 +41,15 @@ * @author Marco Zorzi */ -#include +#include #include #include #include #include #include #include +#include #include #include #include @@ -67,6 +68,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -105,6 +109,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::Publication _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */ @@ -116,14 +121,19 @@ 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 */ 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_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; @@ -133,6 +143,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ hrt_abstime _control_position_last_called{0}; /**) _param_gndspeed_trim, (ParamFloat) _param_gndspeed_max, + (ParamFloat) _param_gndspeed_min, (ParamInt) _param_speed_control_mode, (ParamFloat) _param_speed_p, @@ -185,12 +199,20 @@ private: (ParamFloat) _param_speed_imax, (ParamFloat) _param_throttle_speed_scaler, + (ParamFloat) _param_rate_p, + (ParamFloat) _param_rate_i, + (ParamFloat) _param_rate_d, + (ParamFloat) _param_rate_ff, + (ParamFloat) _param_rate_imax, + (ParamFloat) _param_rate_max, + (ParamFloat) _param_rate_i_minspeed, + (ParamFloat) _param_throttle_min, (ParamFloat) _param_throttle_max, (ParamFloat) _param_throttle_cruise, - (ParamFloat) _param_wheel_base, (ParamFloat) _param_max_turn_angle, + (ParamFloat) _param_att_p, (ParamFloat) _param_gnd_man_y_max, (ParamFloat) _param_nav_loiter_rad /**< loiter radius for Rover */ ) @@ -202,8 +224,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(); /** @@ -213,5 +237,8 @@ private: const position_setpoint_triplet_s &_pos_sp_triplet); void control_velocity(const matrix::Vector3f ¤t_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); }; 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 6458fb1fea..df5a43067e 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -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 1.0 + * @decimal 3 + * @increment 0.005 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_RATE_MAX, 0.5f); + +/** + * 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);