diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 95bc589664..cf1a150dea 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -181,10 +181,10 @@ set(msg_files RoverPositionSetpoint.msg RoverRateSetpoint.msg RoverRateStatus.msg + RoverSpeedSetpoint.msg + RoverSpeedStatus.msg RoverSteeringSetpoint.msg RoverThrottleSetpoint.msg - RoverVelocitySetpoint.msg - RoverVelocityStatus.msg Rpm.msg RtlStatus.msg RtlTimeEstimate.msg diff --git a/msg/RoverSpeedSetpoint.msg b/msg/RoverSpeedSetpoint.msg new file mode 100644 index 0000000000..ac7787f6c9 --- /dev/null +++ b/msg/RoverSpeedSetpoint.msg @@ -0,0 +1,5 @@ +# Rover Speed Setpoint + +uint64 timestamp # [us] Time since system start +float32 speed_body_x # [m/s] [@range -inf (Backwards), inf (Forwards)] [@frame Body] Speed setpoint in body x direction +float32 speed_body_y # [m/s] [@range -inf (Left), inf (Right)] [@frame Body] [@invalid NaN If not mecanum] Mecanum only: Speed setpoint in body y direction diff --git a/msg/RoverVelocityStatus.msg b/msg/RoverSpeedStatus.msg similarity index 100% rename from msg/RoverVelocityStatus.msg rename to msg/RoverSpeedStatus.msg diff --git a/msg/RoverVelocitySetpoint.msg b/msg/RoverVelocitySetpoint.msg deleted file mode 100644 index 71387355d0..0000000000 --- a/msg/RoverVelocitySetpoint.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Rover Velocity Setpoint - -uint64 timestamp # [us] Time since system start -float32 speed # [m/s] [@range -inf (Backwards), inf (Forwards)] Speed setpoint -float32 bearing # [rad] [@range -pi,pi] [@frame NED] [@invalid: NaN, speed is defined in body x direction] Bearing setpoint -float32 yaw # [rad] [@range -pi, pi] [@frame NED] [@invalid NaN, Defaults to vehicle yaw] Mecanum only: Yaw setpoint diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 1d9362bf5c..38d6074d53 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -111,10 +111,10 @@ void LoggedTopics::add_default_topics() add_optional_topic("rover_position_setpoint", 100); add_optional_topic("rover_rate_setpoint", 100); add_optional_topic("rover_rate_status", 100); + add_optional_topic("rover_speed_setpoint", 100); + add_optional_topic("rover_speed_status", 100); add_optional_topic("rover_steering_setpoint", 100); add_optional_topic("rover_throttle_setpoint", 100); - add_optional_topic("rover_velocity_setpoint", 100); - add_optional_topic("rover_velocity_status", 100); add_topic("rtl_time_estimate", 1000); add_topic("rtl_status", 2000); add_optional_topic("sensor_airflow", 100); diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp index 38be9532c5..f3eea3f910 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp @@ -42,7 +42,7 @@ AckermannManualMode::AckermannManualMode(ModuleParams *parent) : ModuleParams(pa _rover_steering_setpoint_pub.advertise(); _rover_rate_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_setpoint_pub.advertise(); + _rover_speed_setpoint_pub.advertise(); _rover_position_setpoint_pub.advertise(); } @@ -164,12 +164,10 @@ void AckermannManualMode::position() _pos_ctl_course_direction = Vector2f(NAN, NAN); // Speed control - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = hrt_absolute_time(); - rover_velocity_setpoint.speed = speed_setpoint; - rover_velocity_setpoint.bearing = NAN; - rover_velocity_setpoint.yaw = NAN; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = hrt_absolute_time(); + rover_speed_setpoint.speed_body_x = speed_setpoint; + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); // Rate control rover_rate_setpoint_s rover_rate_setpoint{}; diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp index 3d5eb1974a..6216ed17ff 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include /** @@ -83,7 +83,7 @@ public: void stab(); /** - * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint. + * @brief Generate and publish roverSpeedSetpoint/roverRateSetpoint or roverPositionSetpoint from manualControlSetpoint. */ void position(); @@ -109,7 +109,7 @@ private: uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; // Variables diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.cpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.cpp index 84289a8d2e..ccf7f7ba36 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.cpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.cpp @@ -38,8 +38,9 @@ using namespace time_literals; AckermannOffboardMode::AckermannOffboardMode(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _rover_velocity_setpoint_pub.advertise(); + _rover_speed_setpoint_pub.advertise(); _rover_position_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); } void AckermannOffboardMode::updateParams() @@ -69,11 +70,13 @@ void AckermannOffboardMode::offboardControl() } else if (offboard_control_mode.velocity) { const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = hrt_absolute_time(); - rover_velocity_setpoint.speed = velocity_ned.norm(); - rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0)); - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); - + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = hrt_absolute_time(); + rover_speed_setpoint.speed_body_x = velocity_ned.norm(); + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = hrt_absolute_time(); + rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } } diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.hpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.hpp index c3c95edd31..573289e300 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.hpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannOffboardMode/AckermannOffboardMode.hpp @@ -43,7 +43,8 @@ // uORB includes #include #include -#include +#include +#include #include #include #include @@ -80,6 +81,7 @@ private: uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; // uORB publications - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; }; diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index 08abaa7d6c..d59a87c232 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -38,7 +38,8 @@ using namespace time_literals; AckermannPosControl::AckermannPosControl(ModuleParams *parent) : ModuleParams(parent) { _pure_pursuit_status_pub.advertise(); - _rover_velocity_setpoint_pub.advertise(); + _rover_speed_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); updateParams(); } @@ -47,6 +48,7 @@ void AckermannPosControl::updateParams() { ModuleParams::updateParams(); _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get()); } @@ -82,20 +84,36 @@ void AckermannPosControl::updatePosControl() const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned, _curr_pos_ned, fabsf(speed_setpoint)); + + if (_param_ro_speed_red.get() > FLT_EPSILON) { + const float course_error = fabsf(matrix::wrap_pi(bearing_setpoint - _vehicle_yaw)); + const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(course_error, + 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); + const float max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), _min_speed, + _param_ro_max_thr_speed.get()); + speed_setpoint = math::constrain(speed_setpoint, -max_speed, max_speed); + } + _pure_pursuit_status_pub.publish(pure_pursuit_status); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = timestamp; - rover_velocity_setpoint.speed = speed_setpoint; - rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = timestamp; + rover_speed_setpoint.speed_body_x = speed_setpoint; + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = timestamp; + rover_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( bearing_setpoint + M_PI_F); - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } else { - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = timestamp; - rover_velocity_setpoint.speed = 0.f; - rover_velocity_setpoint.bearing = _vehicle_yaw; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = timestamp; + rover_speed_setpoint.speed_body_x = 0.f; + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = timestamp; + rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } } } diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp index f742471c75..f43cc5b172 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp @@ -47,7 +47,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -69,7 +70,7 @@ public: ~AckermannPosControl() = default; /** - * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint. + * @brief Generate and publish roverSpeedSetpoint and roverAttitudeSetpoint from roverPositionSetpoint. */ void updatePosControl(); @@ -99,7 +100,8 @@ private: rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; // Variables @@ -110,17 +112,22 @@ private: float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; float _acceptance_radius{0.f}; // Acceptance radius for the waypoint. + float _min_speed{NAN}; // Class Instances MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_red, (ParamFloat) _param_ro_decel_limit, (ParamFloat) _param_ro_jerk_limit, (ParamFloat) _param_ro_speed_limit, (ParamFloat) _param_pp_lookahd_gain, (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_pp_lookahd_min, - (ParamFloat) _param_ro_yaw_rate_limit + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_str_ang ) }; diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp similarity index 62% rename from src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp rename to src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp index 695b9fd79a..12532c1d76 100644 --- a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp +++ b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp @@ -31,23 +31,20 @@ * ****************************************************************************/ -#include "AckermannVelControl.hpp" +#include "AckermannSpeedControl.hpp" using namespace time_literals; -AckermannVelControl::AckermannVelControl(ModuleParams *parent) : ModuleParams(parent) +AckermannSpeedControl::AckermannSpeedControl(ModuleParams *parent) : ModuleParams(parent) { _rover_throttle_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_status_pub.advertise(); + _rover_speed_status_pub.advertise(); updateParams(); } -void AckermannVelControl::updateParams() +void AckermannSpeedControl::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; - _min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get()); // Set up PID controller _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); @@ -58,37 +55,20 @@ void AckermannVelControl::updateParams() if (_param_ro_accel_limit.get() > FLT_EPSILON) { _adjusted_speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); } - - } -void AckermannVelControl::updateVelControl() +void AckermannSpeedControl::updateSpeedControl() { updateSubscriptions(); const hrt_abstime timestamp_prev = _timestamp; _timestamp = hrt_absolute_time(); const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - float max_speed = _param_ro_speed_limit.get(); - - // Attitude Setpoint - if (PX4_ISFINITE(_bearing_setpoint)) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = _bearing_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - - if (_param_ro_speed_red.get() > FLT_EPSILON) { - const float course_error = fabsf(matrix::wrap_pi(_bearing_setpoint - _vehicle_yaw)); - const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(course_error, - 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); - max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), _min_speed, max_speed); - } - } // Throttle Setpoint if (PX4_ISFINITE(_speed_setpoint)) { - const float speed_setpoint = math::constrain(_speed_setpoint, -max_speed, max_speed); + const float speed_setpoint = math::constrain(_speed_setpoint, -_param_ro_speed_limit.get(), + _param_ro_speed_limit.get()); rover_throttle_setpoint_s rover_throttle_setpoint{}; rover_throttle_setpoint.timestamp = _timestamp; rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed, @@ -96,28 +76,26 @@ void AckermannVelControl::updateVelControl() _param_ro_max_thr_speed.get(), dt); rover_throttle_setpoint.throttle_body_y = NAN; _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - } - // Publish position controller status (logging only) - rover_velocity_status_s rover_velocity_status; - rover_velocity_status.timestamp = _timestamp; - rover_velocity_status.measured_speed_body_x = _vehicle_speed; - rover_velocity_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState(); - rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); - rover_velocity_status.measured_speed_body_y = NAN; - rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; - rover_velocity_status.pid_throttle_body_y_integral = NAN; - _rover_velocity_status_pub.publish(rover_velocity_status); + // Publish speed controller status (logging only) + rover_speed_status_s rover_speed_status; + rover_speed_status.timestamp = _timestamp; + rover_speed_status.measured_speed_body_x = _vehicle_speed; + rover_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState(); + rover_speed_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + rover_speed_status.measured_speed_body_y = NAN; + rover_speed_status.adjusted_speed_body_y_setpoint = NAN; + rover_speed_status.pid_throttle_body_y_integral = NAN; + _rover_speed_status_pub.publish(rover_speed_status); } -void AckermannVelControl::updateSubscriptions() +void AckermannSpeedControl::updateSubscriptions() { if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude{}; _vehicle_attitude_sub.copy(&vehicle_attitude); _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); } if (_vehicle_local_position_sub.updated()) { @@ -129,15 +107,14 @@ void AckermannVelControl::updateSubscriptions() _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } - if (_rover_velocity_setpoint_sub.updated()) { - rover_velocity_setpoint_s rover_velocity_setpoint; - _rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint); - _speed_setpoint = rover_velocity_setpoint.speed; - _bearing_setpoint = rover_velocity_setpoint.bearing; + if (_rover_speed_setpoint_sub.updated()) { + rover_speed_setpoint_s rover_speed_setpoint; + _rover_speed_setpoint_sub.copy(&rover_speed_setpoint); + _speed_setpoint = rover_speed_setpoint.speed_body_x; } } -bool AckermannVelControl::runSanityChecks() +bool AckermannSpeedControl::runSanityChecks() { bool ret = true; @@ -147,7 +124,7 @@ bool AckermannVelControl::runSanityChecks() if (_param_ro_speed_limit.get() < FLT_EPSILON) { ret = false; - events::send(events::ID("ackermann_vel_control_conf_invalid_speed_lim"), events::Log::Error, + events::send(events::ID("ackermann_speed_control_conf_invalid_speed_lim"), events::Log::Error, "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); } diff --git a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp similarity index 71% rename from src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp rename to src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp index 11ae3cdc4d..bf4205411b 100644 --- a/src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp +++ b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp @@ -48,31 +48,30 @@ #include #include #include -#include -#include -#include +#include +#include #include #include using namespace matrix; /** - * @brief Class for ackermann position control. + * @brief Class for ackermann speed control. */ -class AckermannVelControl : public ModuleParams +class AckermannSpeedControl : public ModuleParams { public: /** - * @brief Constructor for AckermannVelControl. + * @brief Constructor for AckermannSpeedControl. * @param parent The parent ModuleParams object. */ - AckermannVelControl(ModuleParams *parent); - ~AckermannVelControl() = default; + AckermannSpeedControl(ModuleParams *parent); + ~AckermannSpeedControl() = default; /** - * @brief Generate and publish roverAttitudeSetpoint and RoverThrottleSetpoint from roverVelocitySetpoint. + * @brief Generate and publish RoverThrottleSetpoint from roverSpeedSetpoint. */ - void updateVelControl(); + void updateSpeedControl(); /** * @brief Check if the necessary parameters are set. @@ -81,9 +80,9 @@ public: bool runSanityChecks(); /** - * @brief Reset velocity controller. + * @brief Reset speed controller. */ - void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _bearing_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);}; + void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);}; protected: /** @@ -93,30 +92,24 @@ protected: private: /** - * @brief Update uORB subscriptions used in position controller. + * @brief Update uORB subscriptions used in speed controller. */ void updateSubscriptions(); // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)}; + uORB::Subscription _rover_speed_setpoint_sub{ORB_ID(rover_speed_setpoint)}; // uORB publications uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_speed_status_pub{ORB_ID(rover_speed_status)}; // Variables hrt_abstime _timestamp{0}; Quatf _vehicle_attitude_quaternion{}; float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards - float _vehicle_yaw{0.f}; // [rad] Yaw angle of the vehicle float _speed_setpoint{NAN}; - float _bearing_setpoint{NAN}; - float _min_speed{NAN}; - float _max_yaw_rate{NAN}; // Controllers PID _pid_speed; @@ -130,10 +123,6 @@ private: (ParamFloat) _param_ro_decel_limit, (ParamFloat) _param_ro_jerk_limit, (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ro_speed_th, - (ParamFloat) _param_ro_speed_red, - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ra_wheel_base, - (ParamFloat) _param_ra_max_str_ang + (ParamFloat) _param_ro_speed_th ) }; diff --git a/src/modules/rover_ackermann/AckermannVelControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannSpeedControl/CMakeLists.txt similarity index 89% rename from src/modules/rover_ackermann/AckermannVelControl/CMakeLists.txt rename to src/modules/rover_ackermann/AckermannSpeedControl/CMakeLists.txt index bcf45619c3..eba08edfb1 100644 --- a/src/modules/rover_ackermann/AckermannVelControl/CMakeLists.txt +++ b/src/modules/rover_ackermann/AckermannSpeedControl/CMakeLists.txt @@ -31,9 +31,9 @@ # ############################################################################ -px4_add_library(AckermannVelControl - AckermannVelControl.cpp +px4_add_library(AckermannSpeedControl + AckermannSpeedControl.cpp ) -target_link_libraries(AckermannVelControl PUBLIC PID) -target_include_directories(AckermannVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(AckermannSpeedControl PUBLIC PID) +target_include_directories(AckermannSpeedControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 2605428012..79dce06543 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -34,7 +34,7 @@ add_subdirectory(AckermannActControl) add_subdirectory(AckermannRateControl) add_subdirectory(AckermannAttControl) -add_subdirectory(AckermannVelControl) +add_subdirectory(AckermannSpeedControl) add_subdirectory(AckermannPosControl) add_subdirectory(AckermannDriveModes) @@ -48,7 +48,7 @@ px4_add_module( AckermannActControl AckermannRateControl AckermannAttControl - AckermannVelControl + AckermannSpeedControl AckermannPosControl AckermannAutoMode AckermannManualMode diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index a40a3abb84..543a48dcc0 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -139,7 +139,7 @@ void RoverAckermann::updateControllers() } if (_vehicle_control_mode.flag_control_velocity_enabled) { - _ackermann_vel_control.updateVelControl(); + _ackermann_speed_control.updateSpeedControl(); } if (_vehicle_control_mode.flag_control_attitude_enabled) { @@ -167,7 +167,7 @@ void RoverAckermann::runSanityChecks() return; } - if (_vehicle_control_mode.flag_control_velocity_enabled && !_ackermann_vel_control.runSanityChecks()) { + if (_vehicle_control_mode.flag_control_velocity_enabled && !_ackermann_speed_control.runSanityChecks()) { _sanity_checks_passed = false; return; } @@ -182,7 +182,7 @@ void RoverAckermann::runSanityChecks() void RoverAckermann::reset() { - _ackermann_vel_control.reset(); + _ackermann_speed_control.reset(); _ackermann_att_control.reset(); _ackermann_rate_control.reset(); _manual_mode.reset(); diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index fa1453e76f..d5a4175d0f 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -54,7 +54,7 @@ #include "AckermannActControl/AckermannActControl.hpp" #include "AckermannRateControl/AckermannRateControl.hpp" #include "AckermannAttControl/AckermannAttControl.hpp" -#include "AckermannVelControl/AckermannVelControl.hpp" +#include "AckermannSpeedControl/AckermannSpeedControl.hpp" #include "AckermannPosControl/AckermannPosControl.hpp" #include "AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp" #include "AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp" @@ -124,7 +124,7 @@ private: AckermannActControl _ackermann_act_control{this}; AckermannRateControl _ackermann_rate_control{this}; AckermannAttControl _ackermann_att_control{this}; - AckermannVelControl _ackermann_vel_control{this}; + AckermannSpeedControl _ackermann_speed_control{this}; AckermannPosControl _ackermann_pos_control{this}; AckermannAutoMode _auto_mode{this}; AckermannManualMode _manual_mode{this}; diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index 05aef49cfe..10d7d551a1 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 PX4 DeSpeedopment Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -34,7 +34,7 @@ add_subdirectory(DifferentialActControl) add_subdirectory(DifferentialRateControl) add_subdirectory(DifferentialAttControl) -add_subdirectory(DifferentialVelControl) +add_subdirectory(DifferentialSpeedControl) add_subdirectory(DifferentialPosControl) add_subdirectory(DifferentialDriveModes) @@ -48,7 +48,7 @@ px4_add_module( DifferentialActControl DifferentialRateControl DifferentialAttControl - DifferentialVelControl + DifferentialSpeedControl DifferentialPosControl DifferentialAutoMode DifferentialManualMode diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp index 0b116708a9..f44ae3a703 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp @@ -42,7 +42,7 @@ DifferentialManualMode::DifferentialManualMode(ModuleParams *parent) : ModulePar _rover_steering_setpoint_pub.advertise(); _rover_rate_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_setpoint_pub.advertise(); + _rover_speed_setpoint_pub.advertise(); _rover_position_setpoint_pub.advertise(); } @@ -156,12 +156,10 @@ void DifferentialManualMode::position() _pos_ctl_course_direction = Vector2f(NAN, NAN); // Speed control - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = hrt_absolute_time(); - rover_velocity_setpoint.speed = speed_setpoint; - rover_velocity_setpoint.bearing = NAN; - rover_velocity_setpoint.yaw = NAN; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = hrt_absolute_time(); + rover_speed_setpoint.speed_body_x = speed_setpoint; + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); // Rate control rover_rate_setpoint_s rover_rate_setpoint{}; diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp index 1642afe1d7..cd297b4ce6 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include using namespace matrix; @@ -110,7 +110,7 @@ private: uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; // Variables diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp index eca17e18ff..c00bcf5b1e 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp @@ -40,7 +40,7 @@ DifferentialOffboardMode::DifferentialOffboardMode(ModuleParams *parent) : Modul updateParams(); _rover_rate_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_setpoint_pub.advertise(); + _rover_speed_setpoint_pub.advertise(); _rover_position_setpoint_pub.advertise(); } @@ -71,11 +71,14 @@ void DifferentialOffboardMode::offboardControl() } else if (offboard_control_mode.velocity) { const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = hrt_absolute_time(); - rover_velocity_setpoint.speed = velocity_ned.norm(); - rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0)); - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = hrt_absolute_time(); + rover_speed_setpoint.speed_body_x = velocity_ned.norm(); + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = hrt_absolute_time(); + rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } else if (offboard_control_mode.attitude) { rover_attitude_setpoint_s rover_attitude_setpoint{}; diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp index ebdeef0112..7317862213 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include #include @@ -84,6 +84,6 @@ private: // uORB publications uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; }; diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp index 520dd116fb..e9b7a9ae44 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp @@ -38,7 +38,8 @@ using namespace time_literals; DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModuleParams(parent) { _pure_pursuit_status_pub.advertise(); - _rover_velocity_setpoint_pub.advertise(); + _rover_speed_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); updateParams(); } @@ -81,19 +82,46 @@ void DifferentialPosControl::updatePosControl() _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned, _curr_pos_ned, fabsf(speed_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = timestamp; - rover_velocity_setpoint.speed = speed_setpoint; - rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi( + + const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw); + + if (_current_state == DrivingState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { + _current_state = DrivingState::SPOT_TURNING; + + } else if (_current_state == DrivingState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + _current_state = DrivingState::DRIVING; + } + + if (_current_state == DrivingState::SPOT_TURNING) { + speed_setpoint = 0.f; // stop during spot turning + + } else if (_param_ro_speed_red.get() > FLT_EPSILON) { + const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(fabsf(heading_error), + 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); + const float max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), 0.f, + _param_ro_max_thr_speed.get()); + speed_setpoint = math::constrain(speed_setpoint, -max_speed, max_speed); + } + + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = timestamp; + rover_speed_setpoint.speed_body_x = speed_setpoint; + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = timestamp; + rover_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi( yaw_setpoint + M_PI_F); - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } else { - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = timestamp; - rover_velocity_setpoint.speed = 0.f; - rover_velocity_setpoint.bearing = _vehicle_yaw; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = timestamp; + rover_speed_setpoint.speed_body_x = 0.f; + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = timestamp; + rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } } diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp index 935ba97af5..5ebe7d5a12 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp @@ -46,7 +46,8 @@ // uORB includes #include #include -#include +#include +#include #include #include #include @@ -54,6 +55,14 @@ using namespace matrix; +/** + * @brief Enum class for the different states of driving. + */ +enum class DrivingState { + SPOT_TURNING, // The vehicle is currently turning on the spot. + DRIVING // The vehicle is currently driving. +}; + /** * @brief Class for differential position control. */ @@ -68,7 +77,7 @@ public: ~DifferentialPosControl() = default; /** - * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint. + * @brief Generate and publish roverSpeedSetpoint and roverAttitudeSetpoint from roverPositionSetpoint. */ void updatePosControl(); @@ -97,22 +106,28 @@ private: rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; // Variables Vector2f _curr_pos_ned{}; Vector2f _start_ned{}; float _arrival_speed{0.f}; float _vehicle_yaw{0.f}; + DrivingState _current_state{DrivingState::DRIVING}; DEFINE_PARAMETERS( + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn, + (ParamFloat) _param_ro_max_thr_speed, (ParamFloat) _param_ro_decel_limit, (ParamFloat) _param_ro_jerk_limit, (ParamFloat) _param_ro_speed_limit, (ParamFloat) _param_pp_lookahd_gain, (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_pp_lookahd_min, - (ParamFloat) _param_nav_acc_rad + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_ro_speed_red ) }; diff --git a/src/modules/rover_differential/DifferentialVelControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialSpeedControl/CMakeLists.txt similarity index 88% rename from src/modules/rover_differential/DifferentialVelControl/CMakeLists.txt rename to src/modules/rover_differential/DifferentialSpeedControl/CMakeLists.txt index ba725559b8..217be5e18b 100644 --- a/src/modules/rover_differential/DifferentialVelControl/CMakeLists.txt +++ b/src/modules/rover_differential/DifferentialSpeedControl/CMakeLists.txt @@ -31,9 +31,9 @@ # ############################################################################ -px4_add_library(DifferentialVelControl - DifferentialVelControl.cpp +px4_add_library(DifferentialSpeedControl + DifferentialSpeedControl.cpp ) -target_link_libraries(DifferentialVelControl PUBLIC PID) -target_include_directories(DifferentialVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(DifferentialSpeedControl PUBLIC PID) +target_include_directories(DifferentialSpeedControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp similarity index 52% rename from src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp rename to src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp index 32a419bea5..6476cce630 100644 --- a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp +++ b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp @@ -31,19 +31,18 @@ * ****************************************************************************/ -#include "DifferentialVelControl.hpp" +#include "DifferentialSpeedControl.hpp" using namespace time_literals; -DifferentialVelControl::DifferentialVelControl(ModuleParams *parent) : ModuleParams(parent) +DifferentialSpeedControl::DifferentialSpeedControl(ModuleParams *parent) : ModuleParams(parent) { _rover_throttle_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_status_pub.advertise(); + _rover_speed_status_pub.advertise(); updateParams(); } -void DifferentialVelControl::updateParams() +void DifferentialSpeedControl::updateParams() { ModuleParams::updateParams(); @@ -58,34 +57,17 @@ void DifferentialVelControl::updateParams() } } -void DifferentialVelControl::updateVelControl() +void DifferentialSpeedControl::updateSpeedControl() { updateSubscriptions(); const hrt_abstime timestamp_prev = _timestamp; _timestamp = hrt_absolute_time(); const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - float max_speed = _param_ro_speed_limit.get(); - - - // Attitude Setpoint - if (PX4_ISFINITE(_bearing_setpoint)) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = _bearing_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - - if (_param_ro_speed_red.get() > FLT_EPSILON) { - const float course_error = fabsf(matrix::wrap_pi(_bearing_setpoint - _vehicle_yaw)); - const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(course_error, - 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); - max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), 0.f, max_speed); - } - } // Throttle Setpoint if (PX4_ISFINITE(_speed_setpoint)) { - const float speed_setpoint = calcSpeedSetpoint(max_speed); + const float speed_setpoint = calcSpeedSetpoint(); rover_throttle_setpoint_s rover_throttle_setpoint{}; rover_throttle_setpoint.timestamp = _timestamp; rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed, @@ -93,28 +75,26 @@ void DifferentialVelControl::updateVelControl() _param_ro_max_thr_speed.get(), dt); rover_throttle_setpoint.throttle_body_y = 0.f; _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - } - // Publish velocity controller status (logging only) - rover_velocity_status_s rover_velocity_status; - rover_velocity_status.timestamp = _timestamp; - rover_velocity_status.measured_speed_body_x = _vehicle_speed; - rover_velocity_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState(); - rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); - rover_velocity_status.measured_speed_body_y = NAN; - rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; - rover_velocity_status.pid_throttle_body_y_integral = NAN; - _rover_velocity_status_pub.publish(rover_velocity_status); + // Publish speed controller status (logging only) + rover_speed_status_s rover_speed_status; + rover_speed_status.timestamp = _timestamp; + rover_speed_status.measured_speed_body_x = _vehicle_speed; + rover_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState(); + rover_speed_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + rover_speed_status.measured_speed_body_y = NAN; + rover_speed_status.adjusted_speed_body_y_setpoint = NAN; + rover_speed_status.pid_throttle_body_y_integral = NAN; + _rover_speed_status_pub.publish(rover_speed_status); } -void DifferentialVelControl::updateSubscriptions() +void DifferentialSpeedControl::updateSubscriptions() { if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude{}; _vehicle_attitude_sub.copy(&vehicle_attitude); _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); } if (_vehicle_local_position_sub.updated()) { @@ -126,64 +106,49 @@ void DifferentialVelControl::updateSubscriptions() _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } - if (_rover_velocity_setpoint_sub.updated()) { - rover_velocity_setpoint_s rover_velocity_setpoint; - _rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint); - _speed_setpoint = rover_velocity_setpoint.speed; - _bearing_setpoint = rover_velocity_setpoint.bearing; + if (_rover_speed_setpoint_sub.updated()) { + rover_speed_setpoint_s rover_speed_setpoint; + _rover_speed_setpoint_sub.copy(&rover_speed_setpoint); + _speed_setpoint = rover_speed_setpoint.speed_body_x; } } -float DifferentialVelControl::calcSpeedSetpoint(const float max_speed) +float DifferentialSpeedControl::calcSpeedSetpoint() { - const float heading_error = matrix::wrap_pi(_bearing_setpoint - _vehicle_yaw); + float speed_setpoint = math::constrain(_speed_setpoint, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); - if (_current_state == DrivingState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { - _current_state = DrivingState::SPOT_TURNING; + const float speed_setpoint_normalized = math::interpolate(speed_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); - } else if (_current_state == DrivingState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { - _current_state = DrivingState::DRIVING; + if (_rover_steering_setpoint_sub.updated()) { + rover_steering_setpoint_s rover_steering_setpoint{}; + _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); + _normalized_speed_diff = rover_steering_setpoint.normalized_steering_setpoint; } - float speed_setpoint = 0.f; - - if (_current_state == DrivingState::DRIVING) { - speed_setpoint = math::constrain(_speed_setpoint, -max_speed, max_speed); - - const float speed_setpoint_normalized = math::interpolate(speed_setpoint, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); - - if (_rover_steering_setpoint_sub.updated()) { - rover_steering_setpoint_s rover_steering_setpoint{}; - _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); - _normalized_speed_diff = rover_steering_setpoint.normalized_steering_setpoint; - } - - if (fabsf(speed_setpoint_normalized) > 1.f - fabsf( - _normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels - speed_setpoint = math::interpolate(sign(speed_setpoint_normalized) * (1.f - fabsf(_normalized_speed_diff)), -1.f, - 1.f, - - _param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); - } + if (fabsf(speed_setpoint_normalized) > 1.f - fabsf( + _normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels + speed_setpoint = math::interpolate(sign(speed_setpoint_normalized) * (1.f - fabsf(_normalized_speed_diff)), -1.f, + 1.f, -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); } return speed_setpoint; } -bool DifferentialVelControl::runSanityChecks() +bool DifferentialSpeedControl::runSanityChecks() { bool ret = true; if (_param_ro_speed_limit.get() < FLT_EPSILON) { ret = false; - events::send(events::ID("differential_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + events::send(events::ID("differential_speed_control_conf_invalid_speed_lim"), events::Log::Error, "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); } if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { ret = false; - events::send(events::ID("differential_posVel_control_conf_invalid_speed_control"), events::Log::Error, + events::send(events::ID("differential_speed_control_conf_invalid_speed_control"), events::Log::Error, "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup", _param_ro_max_thr_speed.get(), _param_ro_speed_p.get()); } diff --git a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp similarity index 71% rename from src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp rename to src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp index 12f09719c8..3a4d13f076 100644 --- a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp +++ b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp @@ -48,9 +48,8 @@ #include #include #include -#include -#include -#include +#include +#include #include #include #include @@ -58,30 +57,22 @@ using namespace matrix; /** - * @brief Enum class for the different states of driving. + * @brief Class for differential speed control. */ -enum class DrivingState { - SPOT_TURNING, // The vehicle is currently turning on the spot. - DRIVING // The vehicle is currently driving. -}; - -/** - * @brief Class for differential velocity control. - */ -class DifferentialVelControl : public ModuleParams +class DifferentialSpeedControl : public ModuleParams { public: /** - * @brief Constructor for DifferentialVelControl. + * @brief Constructor for DifferentialSpeedControl. * @param parent The parent ModuleParams object. */ - DifferentialVelControl(ModuleParams *parent); - ~DifferentialVelControl() = default; + DifferentialSpeedControl(ModuleParams *parent); + ~DifferentialSpeedControl() = default; /** - * @brief Generate and publish roverAttitudeSetpoint/RoverThrottleSetpoint from roverVelocitySetpoint. + * @brief Generate and publish RoverThrottleSetpoint from roverSpeedSetpoint. */ - void updateVelControl(); + void updateSpeedControl(); /** * @brief Check if the necessary parameters are set. @@ -90,9 +81,9 @@ public: bool runSanityChecks(); /** - * @brief Reset velocity controller. + * @brief Reset speed controller. */ - void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _bearing_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);}; + void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);}; protected: /** @@ -102,45 +93,38 @@ protected: private: /** - * @brief Update uORB subscriptions used in velocity controller. + * @brief Update uORB subscriptions used in speed controller. */ void updateSubscriptions(); /** * @brief Calculate the speed setpoint based on the current state. - * @param max_speed Maximum speed limit [m/s]. * @return Speed setpoint. */ - float calcSpeedSetpoint(float max_speed); + float calcSpeedSetpoint(); // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)}; + uORB::Subscription _rover_speed_setpoint_sub{ORB_ID(rover_speed_setpoint)}; uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; // uORB publications uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _rover_speed_status_pub{ORB_ID(rover_speed_status)}; // Variables hrt_abstime _timestamp{0}; Quatf _vehicle_attitude_quaternion{}; float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards - float _vehicle_yaw{0.f}; float _speed_setpoint{NAN}; - float _bearing_setpoint{NAN}; float _normalized_speed_diff{NAN}; - DrivingState _current_state{DrivingState::DRIVING}; // Controllers PID _pid_speed; SlewRate _adjusted_speed_setpoint; DEFINE_PARAMETERS( - (ParamFloat) _param_rd_trans_trn_drv, - (ParamFloat) _param_rd_trans_drv_trn, (ParamFloat) _param_ro_max_thr_speed, (ParamFloat) _param_ro_speed_p, (ParamFloat) _param_ro_speed_i, @@ -148,7 +132,6 @@ private: (ParamFloat) _param_ro_decel_limit, (ParamFloat) _param_ro_jerk_limit, (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ro_speed_th, - (ParamFloat) _param_ro_speed_red + (ParamFloat) _param_ro_speed_th ) }; diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index 1ef591d180..f911363ac5 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -139,10 +139,6 @@ void RoverDifferential::updateControllers() _differential_pos_control.updatePosControl(); } - if (_vehicle_control_mode.flag_control_velocity_enabled) { - _differential_vel_control.updateVelControl(); - } - if (_vehicle_control_mode.flag_control_attitude_enabled) { _differential_att_control.updateAttControl(); } @@ -151,6 +147,10 @@ void RoverDifferential::updateControllers() _differential_rate_control.updateRateControl(); } + if (_vehicle_control_mode.flag_control_velocity_enabled) { + _differential_speed_control.updateSpeedControl(); + } + if (_vehicle_control_mode.flag_control_allocation_enabled) { _differential_act_control.updateActControl(); } @@ -168,7 +168,7 @@ void RoverDifferential::runSanityChecks() return; } - if (_vehicle_control_mode.flag_control_velocity_enabled && !_differential_vel_control.runSanityChecks()) { + if (_vehicle_control_mode.flag_control_velocity_enabled && !_differential_speed_control.runSanityChecks()) { _sanity_checks_passed = false; return; } @@ -183,7 +183,7 @@ void RoverDifferential::runSanityChecks() void RoverDifferential::reset() { - _differential_vel_control.reset(); + _differential_speed_control.reset(); _differential_att_control.reset(); _differential_rate_control.reset(); _manual_mode.reset(); diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 06ed7f7be9..19b2fd1d0c 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -53,7 +53,7 @@ #include "DifferentialActControl/DifferentialActControl.hpp" #include "DifferentialRateControl/DifferentialRateControl.hpp" #include "DifferentialAttControl/DifferentialAttControl.hpp" -#include "DifferentialVelControl/DifferentialVelControl.hpp" +#include "DifferentialSpeedControl/DifferentialSpeedControl.hpp" #include "DifferentialPosControl/DifferentialPosControl.hpp" #include "DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp" #include "DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp" @@ -123,7 +123,7 @@ private: DifferentialActControl _differential_act_control{this}; DifferentialRateControl _differential_rate_control{this}; DifferentialAttControl _differential_att_control{this}; - DifferentialVelControl _differential_vel_control{this}; + DifferentialSpeedControl _differential_speed_control{this}; DifferentialPosControl _differential_pos_control{this}; DifferentialAutoMode _auto_mode{this}; DifferentialManualMode _manual_mode{this}; diff --git a/src/modules/rover_mecanum/CMakeLists.txt b/src/modules/rover_mecanum/CMakeLists.txt index 3b76841ad2..295910c7d4 100644 --- a/src/modules/rover_mecanum/CMakeLists.txt +++ b/src/modules/rover_mecanum/CMakeLists.txt @@ -34,7 +34,7 @@ add_subdirectory(MecanumActControl) add_subdirectory(MecanumRateControl) add_subdirectory(MecanumAttControl) -add_subdirectory(MecanumVelControl) +add_subdirectory(MecanumSpeedControl) add_subdirectory(MecanumPosControl) add_subdirectory(MecanumDriveModes) @@ -48,7 +48,7 @@ px4_add_module( MecanumActControl MecanumRateControl MecanumAttControl - MecanumVelControl + MecanumSpeedControl MecanumPosControl MecanumAutoMode MecanumManualMode diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp index aba3d23f9f..0bacf61f19 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp @@ -42,7 +42,7 @@ MecanumManualMode::MecanumManualMode(ModuleParams *parent) : ModuleParams(parent _rover_steering_setpoint_pub.advertise(); _rover_rate_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_setpoint_pub.advertise(); + _rover_speed_setpoint_pub.advertise(); _rover_position_setpoint_pub.advertise(); } @@ -153,19 +153,16 @@ void MecanumManualMode::position() velocity_setpoint_body(1) = math::interpolate(manual_control_setpoint.roll, -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); velocity_setpoint_body(2) = 0.f; - const Vector3f velocity_setpoint_ned = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body); - - if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON || velocity_setpoint_ned.norm() < FLT_EPSILON) { + if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON || velocity_setpoint_body.norm() < FLT_EPSILON) { _pos_ctl_yaw_setpoint = NAN; // Speed control - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = hrt_absolute_time(); - rover_velocity_setpoint.speed = velocity_setpoint_ned.norm(); - rover_velocity_setpoint.bearing = atan2f(velocity_setpoint_ned(1), velocity_setpoint_ned(0)); - rover_velocity_setpoint.yaw = NAN; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = hrt_absolute_time(); + rover_speed_setpoint.speed_body_x = velocity_setpoint_body(0); + rover_speed_setpoint.speed_body_y = velocity_setpoint_body(1); + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); // Rate control rover_rate_setpoint_s rover_rate_setpoint{}; @@ -192,6 +189,7 @@ void MecanumManualMode::position() _rover_position_setpoint_pub.publish(rover_position_setpoint); } else { // Course control + const Vector3f velocity_setpoint_ned = _vehicle_attitude_quaternion.rotateVector(velocity_setpoint_body); const Vector3f pos_ctl_course_direction_local = velocity_setpoint_ned.normalized(); const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0), pos_ctl_course_direction_local(1)); diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp index 1c03956be4..9f270fbd46 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include using namespace matrix; @@ -110,7 +110,7 @@ private: uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; // Variables diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp index 3da9ddb6f0..f217ada200 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp @@ -40,7 +40,7 @@ MecanumOffboardMode::MecanumOffboardMode(ModuleParams *parent) : ModuleParams(pa updateParams(); _rover_rate_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_setpoint_pub.advertise(); + _rover_speed_setpoint_pub.advertise(); _rover_position_setpoint_pub.advertise(); } @@ -70,12 +70,23 @@ void MecanumOffboardMode::offboardControl() _rover_position_setpoint_pub.publish(rover_position_setpoint); } else if (offboard_control_mode.velocity) { - const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = hrt_absolute_time(); - rover_velocity_setpoint.speed = velocity_ned.norm(); - rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0)); - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + } + + const Vector3f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1], 0.f); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = hrt_absolute_time(); + rover_speed_setpoint.speed_body_x = velocity_in_body_frame(0); + rover_speed_setpoint.speed_body_y = velocity_in_body_frame(1); + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = hrt_absolute_time(); + rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } else if (offboard_control_mode.attitude) { rover_attitude_setpoint_s rover_attitude_setpoint{}; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp index 11b25f4940..18cc930760 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp @@ -45,10 +45,11 @@ #include #include #include -#include +#include #include #include #include +#include using namespace matrix; @@ -80,10 +81,13 @@ private: // uORB subscriptions uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; // uORB publications uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + + Quatf _vehicle_attitude_quaternion{}; }; diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp index 135888a12b..b30403ba61 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp @@ -37,8 +37,9 @@ using namespace time_literals; MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_velocity_setpoint_pub.advertise(); + _rover_speed_setpoint_pub.advertise(); _pure_pursuit_status_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); updateParams(); } @@ -85,20 +86,31 @@ void MecanumPosControl::updatePosControl() _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned, _curr_pos_ned, fabsf(speed_setpoint)); _pure_pursuit_status_pub.publish(pure_pursuit_status); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = timestamp; - rover_velocity_setpoint.speed = speed_setpoint; - rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( - bearing_setpoint + M_PI_F); - rover_velocity_setpoint.yaw = _yaw_setpoint; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + + const Vector3f velocity_in_local_frame(speed_setpoint * cosf(bearing_setpoint), + speed_setpoint * sinf(bearing_setpoint), 0.f); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = timestamp; + rover_speed_setpoint.speed_body_x = velocity_in_body_frame(0); + rover_speed_setpoint.speed_body_y = velocity_in_body_frame(1); + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = timestamp; + rover_attitude_setpoint.yaw_setpoint = _yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } else { - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = timestamp; - rover_velocity_setpoint.speed = 0.f; - rover_velocity_setpoint.bearing = _vehicle_yaw; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + rover_speed_setpoint_s rover_speed_setpoint{}; + rover_speed_setpoint.timestamp = timestamp; + rover_speed_setpoint.speed_body_x = 0.f; + rover_speed_setpoint.speed_body_y = 0.f; + _rover_speed_setpoint_pub.publish(rover_speed_setpoint); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = timestamp; + rover_attitude_setpoint.yaw_setpoint = _vehicle_yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); } } } diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp index 7979b39ced..aab2d34386 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp @@ -47,7 +47,8 @@ // uORB includes #include #include -#include +#include +#include #include #include #include @@ -69,7 +70,7 @@ public: ~MecanumPosControl() = default; /** - * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint. + * @brief Generate and publish roverSpeedSetpoint and roverAttitudeSetpoint from roverPositionSetpoint. */ void updatePosControl(); @@ -98,8 +99,9 @@ private: rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; + uORB::Publication _rover_speed_setpoint_pub{ORB_ID(rover_speed_setpoint)}; uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; // Variables Quatf _vehicle_attitude_quaternion{}; diff --git a/src/modules/rover_mecanum/MecanumVelControl/CMakeLists.txt b/src/modules/rover_mecanum/MecanumSpeedControl/CMakeLists.txt similarity index 86% rename from src/modules/rover_mecanum/MecanumVelControl/CMakeLists.txt rename to src/modules/rover_mecanum/MecanumSpeedControl/CMakeLists.txt index 890ec41fa6..8719ce91ec 100644 --- a/src/modules/rover_mecanum/MecanumVelControl/CMakeLists.txt +++ b/src/modules/rover_mecanum/MecanumSpeedControl/CMakeLists.txt @@ -31,10 +31,10 @@ # ############################################################################ -px4_add_library(MecanumVelControl - MecanumVelControl.cpp +px4_add_library(MecanumSpeedControl + MecanumSpeedControl.cpp ) -target_link_libraries(MecanumVelControl PUBLIC PID) -target_link_libraries(MecanumVelControl PUBLIC pure_pursuit) -target_include_directories(MecanumVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(MecanumSpeedControl PUBLIC PID) +target_link_libraries(MecanumSpeedControl PUBLIC pure_pursuit) +target_include_directories(MecanumSpeedControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp similarity index 74% rename from src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp rename to src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp index 76c02f7219..1033982438 100644 --- a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp +++ b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp @@ -31,19 +31,18 @@ * ****************************************************************************/ -#include "MecanumVelControl.hpp" +#include "MecanumSpeedControl.hpp" using namespace time_literals; -MecanumVelControl::MecanumVelControl(ModuleParams *parent) : ModuleParams(parent) +MecanumSpeedControl::MecanumSpeedControl(ModuleParams *parent) : ModuleParams(parent) { _rover_throttle_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_status_pub.advertise(); + _rover_speed_status_pub.advertise(); updateParams(); } -void MecanumVelControl::updateParams() +void MecanumSpeedControl::updateParams() { ModuleParams::updateParams(); @@ -62,7 +61,7 @@ void MecanumVelControl::updateParams() } } -void MecanumVelControl::updateVelControl() +void MecanumSpeedControl::updateSpeedControl() { const hrt_abstime timestamp_prev = _timestamp; _timestamp = hrt_absolute_time(); @@ -70,15 +69,6 @@ void MecanumVelControl::updateVelControl() updateSubscriptions(); - // Attitude Setpoint - if (PX4_ISFINITE(_yaw_setpoint)) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = _yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - - } - // Throttle Setpoints if (PX4_ISFINITE(_speed_x_setpoint) && PX4_ISFINITE(_speed_y_setpoint)) { Vector2f speed_setpoint = calcSpeedSetpoint(); @@ -96,18 +86,18 @@ void MecanumVelControl::updateVelControl() } // Publish position controller status (logging only) - rover_velocity_status_s rover_velocity_status; - rover_velocity_status.timestamp = _timestamp; - rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; - rover_velocity_status.adjusted_speed_body_x_setpoint = _adjusted_speed_x_setpoint.getState(); - rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; - rover_velocity_status.adjusted_speed_body_y_setpoint = _adjusted_speed_y_setpoint.getState(); - rover_velocity_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral(); - rover_velocity_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral(); - _rover_velocity_status_pub.publish(rover_velocity_status); + rover_speed_status_s rover_speed_status; + rover_speed_status.timestamp = _timestamp; + rover_speed_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_x_setpoint.getState(); + rover_speed_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_speed_status.adjusted_speed_body_y_setpoint = _adjusted_speed_y_setpoint.getState(); + rover_speed_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral(); + rover_speed_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral(); + _rover_speed_status_pub.publish(rover_speed_status); } -void MecanumVelControl::updateSubscriptions() +void MecanumSpeedControl::updateSubscriptions() { if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude{}; @@ -125,34 +115,15 @@ void MecanumVelControl::updateSubscriptions() _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; } - if (_rover_velocity_setpoint_sub.updated()) { - rover_velocity_setpoint_s rover_velocity_setpoint; - _rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint); - - const float speed_setpoint = math::constrain(rover_velocity_setpoint.speed, -_param_ro_speed_limit.get(), - _param_ro_speed_limit.get()); - - if (PX4_ISFINITE(rover_velocity_setpoint.speed) && PX4_ISFINITE(rover_velocity_setpoint.bearing)) { - const Vector3f velocity_in_local_frame(speed_setpoint * cosf(rover_velocity_setpoint.bearing), - speed_setpoint * sinf(rover_velocity_setpoint.bearing), 0.f); - const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _speed_x_setpoint = velocity_in_body_frame(0); - _speed_y_setpoint = velocity_in_body_frame(1); - - } else if (PX4_ISFINITE(rover_velocity_setpoint.speed)) { - _speed_x_setpoint = speed_setpoint; - _speed_y_setpoint = 0.f; - - } else { - _speed_x_setpoint = NAN; - _speed_y_setpoint = NAN; - } - - _yaw_setpoint = rover_velocity_setpoint.yaw; + if (_rover_speed_setpoint_sub.updated()) { + rover_speed_setpoint_s rover_speed_setpoint; + _rover_speed_setpoint_sub.copy(&rover_speed_setpoint); + _speed_x_setpoint = rover_speed_setpoint.speed_body_x; + _speed_y_setpoint = rover_speed_setpoint.speed_body_y; } } -Vector2f MecanumVelControl::calcSpeedSetpoint() +Vector2f MecanumSpeedControl::calcSpeedSetpoint() { if (_rover_steering_setpoint_sub.updated()) { rover_steering_setpoint_s rover_steering_setpoint{}; @@ -190,7 +161,7 @@ Vector2f MecanumVelControl::calcSpeedSetpoint() return speed_setpoint; } -bool MecanumVelControl::runSanityChecks() +bool MecanumSpeedControl::runSanityChecks() { bool ret = true; diff --git a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp similarity index 81% rename from src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp rename to src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp index c8e37cc70e..88c443da69 100644 --- a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp +++ b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp @@ -49,31 +49,30 @@ #include #include #include -#include -#include -#include +#include +#include #include #include using namespace matrix; /** - * @brief Class for mecanum velocity control. + * @brief Class for mecanum speed control. */ -class MecanumVelControl : public ModuleParams +class MecanumSpeedControl : public ModuleParams { public: /** - * @brief Constructor for MecanumVelControl. + * @brief Constructor for MecanumSpeedControl. * @param parent The parent ModuleParams object. */ - MecanumVelControl(ModuleParams *parent); - ~MecanumVelControl() = default; + MecanumSpeedControl(ModuleParams *parent); + ~MecanumSpeedControl() = default; /** - * @brief Generate and publish roverAttitudeSetpoint and RoverThrottleSetpoint from roverVelocitySetpoint. + * @brief Generate and publish RoverThrottleSetpoint from roverSpeedSetpoint. */ - void updateVelControl(); + void updateSpeedControl(); /** * @brief Check if the necessary parameters are set. @@ -82,9 +81,9 @@ public: bool runSanityChecks(); /** - * @brief Reset velocity controller. + * @brief Reset speed controller. */ - void reset() {_pid_speed_x.resetIntegral(); _pid_speed_y.resetIntegral(); _speed_x_setpoint = NAN; _speed_y_setpoint = NAN; _adjusted_speed_x_setpoint.setForcedValue(0.f); _adjusted_speed_y_setpoint.setForcedValue(0.f); _yaw_setpoint = NAN;}; + void reset() {_pid_speed_x.resetIntegral(); _pid_speed_y.resetIntegral(); _speed_x_setpoint = NAN; _speed_y_setpoint = NAN; _adjusted_speed_x_setpoint.setForcedValue(0.f); _adjusted_speed_y_setpoint.setForcedValue(0.f);}; protected: /** @@ -94,7 +93,7 @@ protected: private: /** - * @brief Update uORB subscriptions used in velocity controller. + * @brief Update uORB subscriptions used in speed controller. */ void updateSubscriptions(); @@ -107,13 +106,12 @@ private: // uORB subscriptions uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)}; + uORB::Subscription _rover_speed_setpoint_sub{ORB_ID(rover_speed_setpoint)}; uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; // uORB publications uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; - uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _rover_speed_status_pub{ORB_ID(rover_speed_status)}; // Variables hrt_abstime _timestamp{0}; @@ -124,7 +122,6 @@ private: float _speed_x_setpoint{NAN}; float _speed_y_setpoint{NAN}; float _normalized_speed_diff{NAN}; - float _yaw_setpoint{NAN}; // Controllers PID _pid_speed_x; diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index d09be2e281..b99e70bc5b 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -139,10 +139,6 @@ void RoverMecanum::updateControllers() _mecanum_pos_control.updatePosControl(); } - if (_vehicle_control_mode.flag_control_velocity_enabled) { - _mecanum_vel_control.updateVelControl(); - } - if (_vehicle_control_mode.flag_control_attitude_enabled) { _mecanum_att_control.updateAttControl(); } @@ -151,6 +147,10 @@ void RoverMecanum::updateControllers() _mecanum_rate_control.updateRateControl(); } + if (_vehicle_control_mode.flag_control_velocity_enabled) { + _mecanum_speed_control.updateSpeedControl(); + } + if (_vehicle_control_mode.flag_control_allocation_enabled) { _mecanum_act_control.updateActControl(); } @@ -168,7 +168,7 @@ void RoverMecanum::runSanityChecks() return; } - if (_vehicle_control_mode.flag_control_velocity_enabled && !_mecanum_vel_control.runSanityChecks()) { + if (_vehicle_control_mode.flag_control_velocity_enabled && !_mecanum_speed_control.runSanityChecks()) { _sanity_checks_passed = false; return; } @@ -183,7 +183,7 @@ void RoverMecanum::runSanityChecks() void RoverMecanum::reset() { - _mecanum_vel_control.reset(); + _mecanum_speed_control.reset(); _mecanum_att_control.reset(); _mecanum_rate_control.reset(); _manual_mode.reset(); diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index 83f1cb6df2..7f3d7a8477 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -53,7 +53,7 @@ #include "MecanumActControl/MecanumActControl.hpp" #include "MecanumRateControl/MecanumRateControl.hpp" #include "MecanumAttControl/MecanumAttControl.hpp" -#include "MecanumVelControl/MecanumVelControl.hpp" +#include "MecanumSpeedControl/MecanumSpeedControl.hpp" #include "MecanumPosControl/MecanumPosControl.hpp" #include "MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp" #include "MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp" @@ -123,7 +123,7 @@ private: MecanumActControl _mecanum_act_control{this}; MecanumRateControl _mecanum_rate_control{this}; MecanumAttControl _mecanum_att_control{this}; - MecanumVelControl _mecanum_vel_control{this}; + MecanumSpeedControl _mecanum_speed_control{this}; MecanumPosControl _mecanum_pos_control{this}; MecanumAutoMode _auto_mode{this}; MecanumManualMode _manual_mode{this}; diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 41f44e63f6..5ed398d1c2 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -199,8 +199,8 @@ subscriptions: - topic: /fmu/in/rover_position_setpoint type: px4_msgs::msg::RoverPositionSetpoint - - topic: /fmu/in/rover_velocity_setpoint - type: px4_msgs::msg::RoverVelocitySetpoint + - topic: /fmu/in/rover_speed_setpoint + type: px4_msgs::msg::RoverSpeedSetpoint - topic: /fmu/in/rover_attitude_setpoint type: px4_msgs::msg::RoverAttitudeSetpoint