From 1cccfb0ddf6006fdb44a4518ac3a6db0009e94b0 Mon Sep 17 00:00:00 2001 From: Junwoo Hwang Date: Fri, 11 Nov 2022 01:37:47 +0100 Subject: [PATCH] RoverPosControl: Fix up rate / attitude / velocity /pos control - There were small points that were missed from the original PR: https://github.com/PX4/PX4-Autopilot/pull/20082 - This was tested in sync with the QGC PR, and was updated accordingly: https://github.com/mavlink/qgroundcontrol/pull/10483 - This modifies some of the parameters unit / range / name / definitions, as some didn't make sense (e.g. GND_SPEED_MAX being used as an output limiter for the velocity PID controller? the unit doens't match) - Also added clear unit documentation & function documentation to make sure we don't mess up the radians / degrees, etc. RoverPosControl: Multiply Roll command via Max Rate Parameter --- .../RoverPositionControl.cpp | 61 ++++++++++++++++--- .../RoverPositionControl.hpp | 54 ++++++++++++++-- .../rover_pos_control_params.c | 28 ++++++--- 3 files changed, 120 insertions(+), 23 deletions(-) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 02df877a84..25f5f1908d 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -101,7 +101,7 @@ void RoverPositionControl::parameters_update(bool force) _param_speed_i.get(), _param_speed_d.get(), _param_speed_imax.get(), - _param_gndspeed_max.get()); + _param_gndspeed_pid_out_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())); @@ -139,6 +139,8 @@ RoverPositionControl::manual_control_setpoint_poll() _reset_yaw_sp = false; } else { + // TODO: Clarify relationship between RATE_MAX and MAN_Y_MAX. + // Currently, in Acro mode we use RATE_MAX as max rate setpoint, and in manual (attitude mode == stabilized), MAN_Y_MAX. const float yaw_rate = math::radians(_param_gnd_man_y_max.get()); _att_sp.yaw_sp_move_rate = _manual_control_setpoint.roll * yaw_rate; _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); @@ -147,19 +149,19 @@ RoverPositionControl::manual_control_setpoint_poll() _att_sp.yaw_body = _manual_yaw_sp; _att_sp.thrust_body[0] = _manual_control_setpoint.throttle; + // Question: What's the point of having yaw_body, but then putting that into Quaternion again? Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.timestamp = hrt_absolute_time(); - _attitude_sp_pub.publish(_att_sp); } else if (_control_mode.flag_control_rates_enabled) { - // STABILIZED mode generate the attitude setpoint from manual user inputs + // ACRO mode generate the rate setpoint from manual user inputs _rates_sp.roll = 0.0; _rates_sp.pitch = 0.0; - _rates_sp.yaw = _manual_control_setpoint.y; + _rates_sp.yaw = _manual_control_setpoint.y * _param_rate_max.get(); _rates_sp.thrust_body[0] = _manual_control_setpoint.z; _rates_sp.timestamp = hrt_absolute_time(); @@ -167,8 +169,9 @@ RoverPositionControl::manual_control_setpoint_poll() _rates_sp_pub.publish(_rates_sp); } else { - _act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.roll; - _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.pitch; + // MANUAL mode, directly map actuator outputs from Manual control + _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.roll; // Nominally yaw: _manual_control_setpoint.yaw; @@ -276,6 +279,12 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, mission_target_speed = _pos_sp_triplet.current.cruising_speed; } + // Log target speed in global NED frame (since we don't always enforce the heading, it is in the direction we are headed) + const Vector3f target_vel_vec = mission_target_speed * ground_speed.unit(); + _local_pos_setpoint.vx = target_vel_vec(0); + _local_pos_setpoint.vy = target_vel_vec(1); + _local_pos_setpoint.vz = target_vel_vec(2); + // Velocity in body frame const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2)); @@ -419,15 +428,25 @@ 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); + + // Attitude error in Euler angles [rad] const Eulerf euler_sp = qe; - // PX4_INFO("Yaw error: %f", double(euler_sp(2))); + // PX4_INFO("Yaw error: %f [rad]", double(euler_sp(2))); _rates_sp.roll = 0.0; _rates_sp.pitch = 0.0; - _rates_sp.yaw = euler_sp(2) / _param_max_turn_angle.get(); + + // Apply Attitude P-gain for Yaw control. Euler angle in [rad], rate setpoint in [rad/s] + _rates_sp.yaw = euler_sp(2) * _param_att_p.get(); + + // PX4_INFO("Yaw rate setpoint in attitude control: %f [rad/s]", (double)_rates_sp.yaw); + _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); + + // Log yaw setpoint for PID tuning debug + _local_pos_setpoint.yaw = att_sp.yaw_body; } void @@ -441,6 +460,8 @@ RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, con 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); + // PX4_INFO("Yaw rate setpoint: %f [rad/s]", (double)_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()); @@ -455,6 +476,9 @@ RoverPositionControl::control_rates(const vehicle_angular_velocity_s &rates, con 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); + + // Log yawrate setpoint for PID tuning + _local_pos_setpoint.yawspeed = rates_sp.yaw; } @@ -489,6 +513,7 @@ RoverPositionControl::Run() /* load local copies */ _global_pos_sub.update(&_global_pos); + // Get the latest position setpoint position_setpoint_triplet_poll(); if (!_global_local_proj_ref.isInitialized() @@ -500,7 +525,7 @@ RoverPositionControl::Run() _global_local_alt0 = _local_pos.ref_alt; } - // Convert Local setpoints to global setpoints + // Handle Offboard control: override position setpoint triplet item with trajectory_setpoint data if (_control_mode.flag_control_offboard_enabled) { _trajectory_setpoint_sub.update(&_trajectory_setpoint); @@ -546,6 +571,10 @@ RoverPositionControl::Run() pos_ctrl_status.timestamp = hrt_absolute_time(); _pos_ctrl_status_pub.publish(pos_ctrl_status); + + // Project position setpoint triplet's global position (lat/lon) into local (x/y/z) position + _global_local_proj_ref.project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, _local_pos_setpoint.x, + _local_pos_setpoint.y); } } else if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_velocity_enabled) { @@ -589,6 +618,20 @@ RoverPositionControl::Run() v_torque_sp.xyz[1] = _act_controls.control[actuator_controls_s::INDEX_PITCH]; v_torque_sp.xyz[2] = _act_controls.control[actuator_controls_s::INDEX_YAW]; _vehicle_torque_setpoint_pub.publish(v_torque_sp); + + // Normalized (When max thrust, magnitude = 1) thrust in North - East frame + // const Vector2f normalized_thrust_vec_2d = _act_controls.control[actuator_controls_s::INDEX_THROTTLE] * Vector2f(_local_pos.vx, _local_pos.vy).unit(); + // normalized_thrust_vec_2d.copyTo(_local_pos_setpoint.thrust); + + // Hack: For now send the throttle setpoint in Acc X value for debugging + _local_pos_setpoint.acceleration[0] = _act_controls.control[actuator_controls_s::INDEX_THROTTLE]; + + // Note: Since we are populating the local position setpoint struct's data in + // rate, attitude and position control, it is not guaranteed that the data + // is actually valid / up-to-date. Hence, it should only be used for PID tuning + + // Publish local position setpoint topic (for PID tuning UI graph in QGC) + _local_pos_setpoint_pub.publish(_local_pos_setpoint); } } } diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 704bd4c0a0..d07e9d13a5 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -114,6 +115,7 @@ private: uORB::Publication _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */ uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */ + uORB::Publication _local_pos_setpoint_pub{ORB_ID(vehicle_local_position_setpoint)}; uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< control mode subscription */ uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; @@ -127,14 +129,25 @@ private: 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 > */ + + // [rad] Attitude setpoint in global frame (North - East - Down) + vehicle_attitude_setpoint_s _att_sp{}; + + // [rad/s] Angular rates setpoint in local frame (Front - Right - Down) + vehicle_rates_setpoint_s _rates_sp{}; + 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 */ + + // Debug values used for sending velocity & position setpoints to GCS + vehicle_local_position_setpoint_s _local_pos_setpoint{}; + actuator_controls_s _act_controls{}; /**< direct control of actuators */ vehicle_attitude_s _vehicle_att{}; vehicle_angular_acceleration_s _vehicle_angular_acceleration{}; + + // [rad/s] Vehicle's current angular rates in local frame (Front - Right - Down) vehicle_angular_velocity_s _vehicle_rates{}; trajectory_setpoint_s _trajectory_setpoint{}; @@ -191,7 +204,7 @@ private: (ParamFloat) _param_l1_distance, (ParamFloat) _param_gndspeed_trim, - (ParamFloat) _param_gndspeed_max, + (ParamFloat) _param_gndspeed_pid_out_max, (ParamFloat) _param_gndspeed_min, (ParamInt) _param_speed_control_mode, @@ -233,12 +246,45 @@ private: void manual_control_setpoint_poll(); /** - * Control position. + * @brief Generate rate setpoint based on desired position setpoint using L1 guidance + * + * If the speed control mode (`GND_SP_CTRL_MODE`) is set to 1, PID velocity controller is used for + * closed-loop control. Otherwise, throttle is fixed at position setpoint trpilet's cruising throttle + * setting (open-loop control). + * + * @param global_pos [deg] Lattitude & Longitude of the vehicle currently + * @param ground_speed [m/s] Global velocity in North - East - Down frame + * @param _pos_sp_triplet Modified version of position setpoint triplet generated by the Navigator, indicating desired position */ bool control_position(const matrix::Vector2d &global_pos, const matrix::Vector3f &ground_speed, const position_setpoint_triplet_s &_pos_sp_triplet); + + /** + * @brief Generate actuator control setpoint based on desired trajectory setpoint + * + * Trajectory setpoint is received through offboard control, or a flight task. + * This function is currently NOT used for position control. + * + * @param current_velocity [m/s] Current vehicle's velocity derived from local position uORB message in North - East - Down frame + */ void control_velocity(const matrix::Vector3f ¤t_velocity); + + /** + * @brief Generate rate setpoint based on desired attitude + * + * @param att Current vehicle's attitude (rotation from FRD body frame to NED earth frame) + * @param att_sp Attitude setpoint containing body angle in North - East - Down frame + */ void control_attitude(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp); + + /** + * @brief Generate actuator output for achieving desired rate + * + * @param rates [rad/s] Current vehicle's angular rates + * @param acc [rad/s^2] Current vehicle's angular accelerations + * @param local_pos Current vehicle's local position data (used for extracting vehicle velocity) + * @param rates_sp [rad/s] Angular rate setpoint + */ 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 df5a43067e..9bf152c54b 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -143,10 +143,11 @@ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f); * Control mode for speed * * This allows the user to choose between closed loop gps speed or open loop cruise throttle speed + * * @min 0 * @max 1 - * @value 0 open loop control - * @value 1 close the loop with gps speed + * @value 0 Open loop control using cruise throttle + * @value 1 Closed loop control using GPS Velocity * @group Rover Position Control */ PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1); @@ -210,11 +211,10 @@ PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f); /** * Speed to throttle scaler * - * This is a gain to map the speed control output to the throttle linearly. + * This is a gain to map the speed control output (in somewhat [m/s]) to the throttle * - * @unit %m/s * @min 0.005 - * @max 50.0 + * @max 2.0 * @decimal 3 * @increment 0.005 * @group Rover Position Control @@ -237,6 +237,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f); /** * Minimum ground speed * + * Used for calculating saturated speed lower bound for waypoint navigation (position mode) * * @unit m/s * @min 0.0 @@ -248,17 +249,26 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f); PARAM_DEFINE_FLOAT(GND_SPEED_MIN, 1.0f); /** - * Maximum ground speed + * Maximum output of velocity PID controller * + * Used for limiting output of the speed PID controller (which is somewhat in m/s unit) + * + * Note: We need to figure out what the unit dimension is for the output of the Velocity PID controller, + * since we are mapping it directly to throttle, and it will be then *sqrt of thrust (for RPM controlled ESCs), + * but we are calculating based off of velocity error. + * + * Note: What is the role of the I-term? E.g. in Multicopter, the I-term of the Velocity controller accounts for + * constant offset force (acceleration) from external source. E.g. Wind. + * + * What is it for the boat? * - * @unit m/s * @min 0.0 * @max 40 * @decimal 1 * @increment 0.5 * @group Rover Position Control */ -PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f); +PARAM_DEFINE_FLOAT(GND_SPEED_PID_MX, 10.0f); /** * Maximum turn angle for Ackerman steering. @@ -278,8 +288,6 @@ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); /** * Attitude control P gain * - * - * @unit rad * @min 0.0 * @max 5.0 * @decimal 3