diff --git a/src/modules/rover_mecanum/CMakeLists.txt b/src/modules/rover_mecanum/CMakeLists.txt index 38dd9a2b63..3b76841ad2 100644 --- a/src/modules/rover_mecanum/CMakeLists.txt +++ b/src/modules/rover_mecanum/CMakeLists.txt @@ -36,6 +36,7 @@ add_subdirectory(MecanumRateControl) add_subdirectory(MecanumAttControl) add_subdirectory(MecanumVelControl) add_subdirectory(MecanumPosControl) +add_subdirectory(MecanumDriveModes) px4_add_module( MODULE modules__rover_mecanum @@ -49,6 +50,9 @@ px4_add_module( MecanumAttControl MecanumVelControl MecanumPosControl + MecanumAutoMode + MecanumManualMode + MecanumOffboardMode px4_work_queue rover_control pure_pursuit diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp index f01a8b6c57..ff65646970 100644 --- a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp +++ b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp @@ -88,15 +88,6 @@ void MecanumActControl::updateActControl() actuator_motors.timestamp = _timestamp; _actuator_motors_pub.publish(actuator_motors); - } else { - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - actuator_motors.control[0] = 0.f; - actuator_motors.control[1] = 0.f; - actuator_motors.control[2] = 0.f; - actuator_motors.control[3] = 0.f; - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); } } @@ -125,21 +116,6 @@ Vector4f MecanumActControl::computeInverseKinematics(float throttle_body_x, floa return motor_commands; } -void MecanumActControl::manualManualMode() -{ - manual_control_setpoint_s manual_control_setpoint{}; - _manual_control_setpoint_sub.copy(&manual_control_setpoint); - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = _timestamp; - rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.yaw; - _rover_steering_setpoint_pub.publish(rover_steering_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); -} - void MecanumActControl::stopVehicle() { actuator_motors_s actuator_motors{}; diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp index 40f6dfbd58..63aa155004 100644 --- a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp +++ b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp @@ -47,7 +47,6 @@ #include #include #include -#include /** * @brief Class for mecanum actuator control. @@ -67,11 +66,6 @@ public: */ void updateActControl(); - /** - * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. - */ - void manualManualMode(); - /** * @brief Stop the vehicle by sending 0 commands to motors and servos. */ @@ -97,12 +91,9 @@ private: uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; // uORB publications - uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; // Variables hrt_abstime _timestamp{0}; @@ -116,9 +107,9 @@ private: // Parameters DEFINE_PARAMETERS( - (ParamInt) _param_r_rev, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, + (ParamInt) _param_r_rev, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp index edddef8ce1..c9bacb846e 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp @@ -38,8 +38,6 @@ using namespace time_literals; MecanumAttControl::MecanumAttControl(ModuleParams *parent) : ModuleParams(parent) { _rover_rate_setpoint_pub.advertise(); - _rover_throttle_setpoint_pub.advertise(); - _rover_attitude_setpoint_pub.advertise(); _rover_attitude_status_pub.advertise(); updateParams(); } @@ -52,21 +50,20 @@ void MecanumAttControl::updateParams() _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; } + // Set up PID controller _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); _pid_yaw.setIntegralLimit(_max_yaw_rate); _pid_yaw.setOutputLimit(_max_yaw_rate); + + // Set up slew rate _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); } void MecanumAttControl::updateAttControl() { - const hrt_abstime timestamp_prev = _timestamp; + hrt_abstime timestamp_prev = _timestamp; _timestamp = hrt_absolute_time(); - _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - if (_vehicle_control_mode_sub.updated()) { - _vehicle_control_mode_sub.copy(&_vehicle_control_mode); - } + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude{}; @@ -75,17 +72,20 @@ void MecanumAttControl::updateAttControl() _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); } - if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + if (_rover_attitude_setpoint_sub.updated()) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + _rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint); + _yaw_setpoint = rover_attitude_setpoint.yaw_setpoint; + } - if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { - generateAttitudeAndThrottleSetpoint(); - } + if (PX4_ISFINITE(_yaw_setpoint)) { + const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, + _vehicle_yaw, _yaw_setpoint, dt); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); - generateRateSetpoint(); - - } else { // Reset pid and slew rate when attitude control is not active - _pid_yaw.resetIntegral(); - _adjusted_yaw_setpoint.setForcedValue(0.f); } // Publish attitude controller status (logging only) @@ -97,93 +97,6 @@ void MecanumAttControl::updateAttControl() } -void MecanumAttControl::generateAttitudeAndThrottleSetpoint() -{ - const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled - && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled; - - if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - - const float yaw_delta = math::interpolate(math::deadzone(manual_control_setpoint.yaw, - _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(), - _max_yaw_rate / _param_ro_yaw_p.get()); - - if (fabsf(yaw_delta) > FLT_EPSILON) { // Closed loop yaw rate control - _stab_yaw_setpoint = NAN; - const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta); - 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 { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) - if (!PX4_ISFINITE(_stab_yaw_setpoint)) { - _stab_yaw_setpoint = _vehicle_yaw; - } - - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - - } - - } - - } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control - trajectory_setpoint_s trajectory_setpoint{}; - _trajectory_setpoint_sub.copy(&trajectory_setpoint); - - if (_offboard_control_mode_sub.updated()) { - _offboard_control_mode_sub.copy(&_offboard_control_mode); - } - - const bool offboard_att_control = _offboard_control_mode.attitude; - - if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - } - } -} - -void MecanumAttControl::generateRateSetpoint() -{ - if (_rover_attitude_setpoint_sub.updated()) { - _rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint); - } - - if (_rover_rate_setpoint_sub.updated()) { - _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); - } - - // Check if a new rate setpoint was already published from somewhere else - if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update - && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) { - return; - } - - const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, - _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt); - - _last_rate_setpoint_update = _timestamp; - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); -} - bool MecanumAttControl::runSanityChecks() { bool ret = true; @@ -194,13 +107,9 @@ bool MecanumAttControl::runSanityChecks() if (_param_ro_yaw_p.get() < FLT_EPSILON) { ret = false; - - if (_prev_param_check_passed) { - events::send(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error, - "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); - } + events::send(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); } - _prev_param_check_passed = ret; return ret; } diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp index 9f037f8fc9..327b4652ad 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp @@ -48,15 +48,9 @@ #include #include #include -#include -#include -#include #include #include #include -#include -#include -#include /** * @brief Class for mecanum attitude control. @@ -72,10 +66,21 @@ public: ~MecanumAttControl() = default; /** - * @brief Update attitude controller. + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. */ void updateAttControl(); + /** + * @brief Reset attitude controller. + */ + void reset() {_pid_yaw.resetIntegral(); _yaw_setpoint = NAN;}; + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + protected: /** * @brief Update the parameters of the module. @@ -83,51 +88,20 @@ protected: void updateParams() override; private: - /** - * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode) - * or trajectorySetpoint (Offboard attitude control). - */ - void generateAttitudeAndThrottleSetpoint(); - - /** - * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. - */ - void generateRateSetpoint(); - - /** - * @brief Check if the necessary parameters are set. - * @return True if all checks pass. - */ - bool runSanityChecks(); // uORB subscriptions - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - 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::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; - uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; - vehicle_control_mode_s _vehicle_control_mode{}; - rover_attitude_setpoint_s _rover_attitude_setpoint{}; - rover_rate_setpoint_s _rover_rate_setpoint{}; - offboard_control_mode_s _offboard_control_mode{}; // uORB publications - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - 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_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; // Variables - hrt_abstime _timestamp{0}; - hrt_abstime _last_rate_setpoint_update{0}; float _vehicle_yaw{0.f}; - float _dt{0.f}; + hrt_abstime _timestamp{0}; float _max_yaw_rate{0.f}; - float _stab_yaw_setpoint{NAN}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad] - bool _prev_param_check_passed{true}; + float _yaw_setpoint{NAN}; // Controllers PID _pid_yaw; @@ -135,8 +109,8 @@ private: // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ro_yaw_p, - (ParamFloat) _param_ro_yaw_stick_dz + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz ) }; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/CMakeLists.txt b/src/modules/rover_mecanum/MecanumDriveModes/CMakeLists.txt new file mode 100644 index 0000000000..c519f0fdfd --- /dev/null +++ b/src/modules/rover_mecanum/MecanumDriveModes/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(MecanumAutoMode) +add_subdirectory(MecanumManualMode) +add_subdirectory(MecanumOffboardMode) diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/CMakeLists.txt b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/CMakeLists.txt new file mode 100644 index 0000000000..4b750c9b5d --- /dev/null +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(MecanumAutoMode +MecanumAutoMode.cpp +) + +target_include_directories(MecanumAutoMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp new file mode 100644 index 0000000000..33c9c2af77 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MecanumAutoMode.hpp" + +using namespace time_literals; + +MecanumAutoMode::MecanumAutoMode(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_position_setpoint_pub.advertise(); +} + +void MecanumAutoMode::updateParams() +{ + ModuleParams::updateParams(); +} + +void MecanumAutoMode::autoControl() +{ + if (_position_setpoint_triplet_sub.updated()) { + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + int curr_wp_type = position_setpoint_triplet.current.type; + + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + MapProjection global_ned_proj_ref{}; + + if (!global_ned_proj_ref.isInitialized() + || (global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + Vector2f curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + Vector2f curr_wp_ned{NAN, NAN}; + Vector2f prev_wp_ned{NAN, NAN}; + Vector2f next_wp_ned{NAN, NAN}; + + RoverControl::globalToLocalSetpointTriplet(curr_wp_ned, prev_wp_ned, next_wp_ned, position_setpoint_triplet, + curr_pos_ned, global_ned_proj_ref); + + float waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + + // Waypoint cruising speed + float cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + + rover_position_setpoint_s rover_position_setpoint{}; + rover_position_setpoint.timestamp = hrt_absolute_time(); + rover_position_setpoint.position_ned[0] = curr_wp_ned(0); + rover_position_setpoint.position_ned[1] = curr_wp_ned(1); + rover_position_setpoint.start_ned[0] = prev_wp_ned(0); + rover_position_setpoint.start_ned[1] = prev_wp_ned(1); + rover_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle, + _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(), curr_wp_type); + rover_position_setpoint.cruising_speed = cruising_speed; + rover_position_setpoint.yaw = PX4_ISFINITE(position_setpoint_triplet.current.yaw) ? + position_setpoint_triplet.current.yaw : NAN; + _rover_position_setpoint_pub.publish(rover_position_setpoint); + } +} + +float MecanumAutoMode::arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, + const float max_speed, const float miss_spd_gain, int curr_wp_type) +{ + // Upcoming stop + if (!PX4_ISFINITE(waypoint_transition_angle) || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + return 0.f; + } + + // Straight line speed + if (miss_spd_gain > FLT_EPSILON) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, + M_PI_F, 0.f, 1.f), 0.f, 1.f); + return max_speed * (1.f - speed_reduction); + } + + return cruising_speed; // Fallthrough + +} diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp new file mode 100644 index 0000000000..de8baccf21 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include + +// Libraries +#include +#include + +// uORB includes +#include +#include +#include +#include +#include + +/** + * @brief Class for Mecanum auto mode. + */ +class MecanumAutoMode : public ModuleParams +{ +public: + /** + * @brief Constructor for auto mode. + * @param parent The parent ModuleParams object. + */ + MecanumAutoMode(ModuleParams *parent); + ~MecanumAutoMode() = default; + + /** + * @brief Generate and publish roverPositionSetpoint from positionSetpointTriplet. + */ + void autoControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Calculate the speed at which the rover should arrive at the current waypoint. During waypoint transition the speed is restricted to + * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). + * @param cruising_speed Cruising speed [m/s]. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum speed setpoint [m/s] + * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. + * @param curr_wp_type Type of the current waypoint. + * @return Speed setpoint [m/s]. + */ + float arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, const float max_speed, + const float miss_spd_gain, int curr_wp_type); + + // uORB subscriptions + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + + // uORB publications + uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_rm_miss_spd_gain + ) +}; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/CMakeLists.txt b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/CMakeLists.txt new file mode 100644 index 0000000000..0710c29181 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(MecanumManualMode + MecanumManualMode.cpp +) + +target_include_directories(MecanumManualMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp new file mode 100644 index 0000000000..9864798c9c --- /dev/null +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MecanumManualMode.hpp" + +using namespace time_literals; + +MecanumManualMode::MecanumManualMode(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); + _rover_rate_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_setpoint_pub.advertise(); + _rover_position_setpoint_pub.advertise(); +} + +void MecanumManualMode::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; +} + +void MecanumManualMode::manual() +{ + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = hrt_absolute_time(); + rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.yaw; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = hrt_absolute_time(); + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); +} + +void MecanumManualMode::acro() +{ + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = hrt_absolute_time(); + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = hrt_absolute_time(); + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate (manual_control_setpoint.yaw, -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +void MecanumManualMode::stab() +{ + 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(); + } + + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = hrt_absolute_time(); + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON) { + _stab_yaw_setpoint = NAN; + + // Rate control + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = hrt_absolute_time(); + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + // Set uncontrolled setpoint invalid + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = hrt_absolute_time(); + rover_attitude_setpoint.yaw_setpoint = NAN; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { // Heading control + if (!PX4_ISFINITE(_stab_yaw_setpoint)) { + _stab_yaw_setpoint = _vehicle_yaw; + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = hrt_absolute_time(); + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } +} + +void MecanumManualMode::position() +{ + 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()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + } + + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + Vector3f velocity_setpoint_body{}; + velocity_setpoint_body(0) = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + 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) { + _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); + + // Rate control + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = hrt_absolute_time(); + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + // Set uncontrolled setpoints invalid + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = hrt_absolute_time(); + rover_attitude_setpoint.yaw_setpoint = NAN; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + rover_position_setpoint_s rover_position_setpoint{}; + rover_position_setpoint.timestamp = hrt_absolute_time(); + rover_position_setpoint.position_ned[0] = NAN; + rover_position_setpoint.position_ned[1] = NAN; + rover_position_setpoint.start_ned[0] = NAN; + rover_position_setpoint.start_ned[1] = NAN; + rover_position_setpoint.arrival_speed = NAN; + rover_position_setpoint.cruising_speed = NAN; + rover_position_setpoint.yaw = NAN; + _rover_position_setpoint_pub.publish(rover_position_setpoint); + + } else { // Course control + 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)); + + // Reset course control if course direction change is above threshold + if (fabsf(acosf(pos_ctl_course_direction_temp(0) * _pos_ctl_course_direction(0) + pos_ctl_course_direction_temp( + 1) * _pos_ctl_course_direction(1))) > _param_rm_course_ctl_th.get()) { + _pos_ctl_yaw_setpoint = NAN; + } + + if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) { + _pos_ctl_start_position_ned = _curr_pos_ned; + _pos_ctl_yaw_setpoint = _vehicle_yaw; + _pos_ctl_course_direction = pos_ctl_course_direction_temp; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned; + const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; + rover_position_setpoint_s rover_position_setpoint{}; + rover_position_setpoint.timestamp = hrt_absolute_time(); + rover_position_setpoint.position_ned[0] = target_waypoint_ned(0); + rover_position_setpoint.position_ned[1] = target_waypoint_ned(1); + rover_position_setpoint.start_ned[0] = NAN; + rover_position_setpoint.start_ned[1] = NAN; + rover_position_setpoint.arrival_speed = NAN; + rover_position_setpoint.cruising_speed = velocity_setpoint_ned.norm(); + rover_position_setpoint.yaw = _pos_ctl_yaw_setpoint; + _rover_position_setpoint_pub.publish(rover_position_setpoint); + } +} + +void MecanumManualMode::reset() +{ + _stab_yaw_setpoint = NAN; + _pos_ctl_course_direction = Vector2f(NAN, NAN); + _pos_ctl_start_position_ned = Vector2f(NAN, NAN); + _curr_pos_ned = Vector2f(NAN, NAN); +} diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp new file mode 100644 index 0000000000..52bbb27fb3 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include + +// Libraries +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for Mecanum manual mode. + */ +class MecanumManualMode : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumManualMode. + * @param parent The parent ModuleParams object. + */ + MecanumManualMode(ModuleParams *parent); + ~MecanumManualMode() = default; + + /** + * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. + */ + void manual(); + + /** + * @brief Generate and publish roverThrottleSetpoint/RoverRateSetpoint from manualControlSetpoint. + */ + void acro(); + + /** + * @brief Generate and publish roverSetpoints from manualControlSetpoint. + */ + void stab(); + + /** + * @brief Generate and publish roverSetpoints from manualControlSetpoint. + */ + void position(); + + /** + * @brief Reset manual mode variables. + */ + void reset(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + // uORB subscriptions + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + // uORB publications + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + 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_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + + // Variables + Vector2f _pos_ctl_course_direction{NAN, NAN}; + Vector2f _pos_ctl_start_position_ned{NAN, NAN}; + Vector2f _curr_pos_ned{NAN, NAN}; + Quatf _vehicle_attitude_quaternion; + float _pos_ctl_yaw_setpoint{NAN}; + float _stab_yaw_setpoint{NAN}; + float _vehicle_yaw{NAN}; + float _max_yaw_rate{NAN}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_rm_course_ctl_th + ) +}; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/CMakeLists.txt b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/CMakeLists.txt new file mode 100644 index 0000000000..b4c31e233f --- /dev/null +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(MecanumOffboardMode + MecanumOffboardMode.cpp +) + +target_include_directories(MecanumOffboardMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp new file mode 100644 index 0000000000..3da9ddb6f0 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MecanumOffboardMode.hpp" + +using namespace time_literals; + +MecanumOffboardMode::MecanumOffboardMode(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_rate_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_setpoint_pub.advertise(); + _rover_position_setpoint_pub.advertise(); +} + +void MecanumOffboardMode::updateParams() +{ + ModuleParams::updateParams(); +} + +void MecanumOffboardMode::offboardControl() +{ + offboard_control_mode_s offboard_control_mode{}; + _offboard_control_mode_sub.copy(&offboard_control_mode); + + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (offboard_control_mode.position) { + rover_position_setpoint_s rover_position_setpoint{}; + rover_position_setpoint.timestamp = hrt_absolute_time(); + rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0]; + rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1]; + rover_position_setpoint.start_ned[0] = NAN; + rover_position_setpoint.start_ned[1] = NAN; + rover_position_setpoint.cruising_speed = NAN; + rover_position_setpoint.arrival_speed = NAN; + rover_position_setpoint.yaw = NAN; + _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); + + } else if (offboard_control_mode.attitude) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = hrt_absolute_time(); + rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else if (offboard_control_mode.body_rate) { + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = hrt_absolute_time(); + rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } +} diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp new file mode 100644 index 0000000000..11b25f4940 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include + +// Libraries +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for Mecanum manual mode. + */ +class MecanumOffboardMode : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumOffboardMode. + * @param parent The parent ModuleParams object. + */ + MecanumOffboardMode(ModuleParams *parent); + ~MecanumOffboardMode() = default; + + /** + * @brief Generate and publish roverSetpoints from trajectorySetpoint. + */ + void offboardControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + // uORB subscriptions + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + + // 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_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; +}; diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp index b44756d15b..9f966823ad 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp @@ -38,7 +38,6 @@ using namespace time_literals; MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent) { _rover_velocity_setpoint_pub.advertise(); - _rover_position_setpoint_pub.advertise(); _pure_pursuit_status_pub.advertise(); updateParams(); @@ -55,29 +54,62 @@ void MecanumPosControl::updatePosControl() { updateSubscriptions(); - if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { - if (_vehicle_control_mode.flag_control_offboard_enabled) { - offboardPositionMode(); + hrt_abstime timestamp = hrt_absolute_time(); - } else if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled) { - manualPositionMode(); + if (_rover_position_setpoint_sub.updated()) { + _rover_position_setpoint_sub.copy(&_rover_position_setpoint); + _start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]); + _start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; + _yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw; + } - } else if (_vehicle_control_mode.flag_control_auto_enabled) { - autoPositionMode(); + const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]); + if (target_waypoint_ned.isAllFinite()) { + + float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (distance_to_target > _param_nav_acc_rad.get()) { + float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : + 0.f; + const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _param_nav_acc_rad.get() : + distance_to_target; + float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance, fabsf(arrival_speed)); + speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + + if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) { + speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint, + fabsf(_rover_position_setpoint.cruising_speed)); + } + + pure_pursuit_status_s pure_pursuit_status{}; + pure_pursuit_status.timestamp = timestamp; + + 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)); + _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); + + } 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); } - - generateVelocitySetpoint(); - } } void MecanumPosControl::updateSubscriptions() { - if (_vehicle_control_mode_sub.updated()) { - _vehicle_control_mode_sub.copy(&_vehicle_control_mode); - } - if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude{}; _vehicle_attitude_sub.copy(&vehicle_attitude); @@ -100,213 +132,6 @@ void MecanumPosControl::updateSubscriptions() } -void MecanumPosControl::offboardPositionMode() -{ - if (_offboard_control_mode_sub.updated()) { - _offboard_control_mode_sub.copy(&_offboard_control_mode); - } - - if (!_offboard_control_mode.position) { - return; - } - - trajectory_setpoint_s trajectory_setpoint{}; - _trajectory_setpoint_sub.copy(&trajectory_setpoint); - - // Translate trajectory setpoint to rover position setpoint - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0]; - rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1]; - rover_position_setpoint.cruising_speed = _param_ro_speed_limit.get(); - rover_position_setpoint.yaw = _vehicle_yaw; - _rover_position_setpoint_pub.publish(rover_position_setpoint); - -} - -void MecanumPosControl::generateVelocitySetpoint() -{ - hrt_abstime timestamp = hrt_absolute_time(); - - if (_rover_position_setpoint_sub.updated()) { - _rover_position_setpoint_sub.copy(&_rover_position_setpoint); - _start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]); - _start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; - _yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw; - } - - const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]); - float distance_to_target = target_waypoint_ned.isAllFinite() ? (target_waypoint_ned - _curr_pos_ned).norm() : NAN; - - if (PX4_ISFINITE(distance_to_target) && distance_to_target > _param_nav_acc_rad.get()) { - - float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : - 0.f; - const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _param_nav_acc_rad.get() : - distance_to_target; - float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance, fabsf(arrival_speed)); - speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); - - if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) { - speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint, - fabsf(_rover_position_setpoint.cruising_speed)); - } - - pure_pursuit_status_s pure_pursuit_status{}; - pure_pursuit_status.timestamp = timestamp; - - const float yaw_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)); - _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( - yaw_setpoint + M_PI_F); - rover_velocity_setpoint.yaw = _yaw_setpoint; - _rover_velocity_setpoint_pub.publish(rover_velocity_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.yaw = _vehicle_yaw; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); - } - -} - -void MecanumPosControl::manualPositionMode() -{ - manual_control_setpoint_s manual_control_setpoint{}; - _manual_control_setpoint_sub.copy(&manual_control_setpoint); - - Vector3f velocity_setpoint_body{}; - velocity_setpoint_body(0) = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); - velocity_setpoint_body(1) = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); - const float yaw_delta = math::interpolate(math::deadzone(manual_control_setpoint.yaw, - _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(), - _max_yaw_rate / _param_ro_yaw_p.get()); - - if (fabsf(yaw_delta) > FLT_EPSILON || velocity_setpoint_body.norm() < FLT_EPSILON) { // Closed loop yaw rate control - _pos_ctl_yaw_setpoint = NAN; - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const Vector3f velocity = Vector3f(velocity_setpoint_body(0), velocity_setpoint_body(1), 0.f); - const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized()); - const Vector2f pos_ctl_course_direction = Vector2f(pos_ctl_course_direction_local(0), - pos_ctl_course_direction_local(1)); - const Vector2f target_waypoint_ned = _curr_pos_ned + _param_pp_lookahd_max.get() * pos_ctl_course_direction; - - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = target_waypoint_ned(0); - rover_position_setpoint.position_ned[1] = target_waypoint_ned(1); - rover_position_setpoint.start_ned[0] = NAN; - rover_position_setpoint.start_ned[1] = NAN; - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.cruising_speed = velocity_setpoint_body.norm(); - rover_position_setpoint.yaw = matrix::wrap_pi(_vehicle_yaw + yaw_delta); - _rover_position_setpoint_pub.publish(rover_position_setpoint); - - } else { // Course control if the steering input is zero (keep driving on a straight line) - const Vector3f velocity = Vector3f(velocity_setpoint_body(0), velocity_setpoint_body(1), 0.f); - const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized()); - const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0), - pos_ctl_course_direction_local(1)); - - // Reset course control if course direction change is above threshold - if (fabsf(acosf(pos_ctl_course_direction_temp(0) * _pos_ctl_course_direction(0) + pos_ctl_course_direction_temp( - 1) * _pos_ctl_course_direction(1))) > _param_rm_course_ctl_th.get()) { - _pos_ctl_yaw_setpoint = NAN; - } - - if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) { - _pos_ctl_start_position_ned = _curr_pos_ned; - _pos_ctl_yaw_setpoint = _vehicle_yaw; - _pos_ctl_course_direction = pos_ctl_course_direction_temp; - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned; - const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = target_waypoint_ned(0); - rover_position_setpoint.position_ned[1] = target_waypoint_ned(1); - rover_position_setpoint.start_ned[0] = NAN; - rover_position_setpoint.start_ned[1] = NAN; - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.cruising_speed = velocity.norm(); - rover_position_setpoint.yaw = _pos_ctl_yaw_setpoint; - _rover_position_setpoint_pub.publish(rover_position_setpoint); - } -} - -void MecanumPosControl::autoPositionMode() -{ - if (_position_setpoint_triplet_sub.updated()) { - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - _curr_wp_type = position_setpoint_triplet.current.type; - - RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, - _curr_pos_ned, _global_ned_proj_ref); - - _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); - - // Waypoint cruising speed - _auto_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( - position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); - - // Waypoint yaw setpoint - if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) { - _auto_yaw = position_setpoint_triplet.current.yaw; - - } else { - _auto_yaw = _vehicle_yaw; - } - - rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = hrt_absolute_time(); - rover_position_setpoint.position_ned[0] = _curr_wp_ned(0); - rover_position_setpoint.position_ned[1] = _curr_wp_ned(1); - rover_position_setpoint.start_ned[0] = _prev_wp_ned(0); - rover_position_setpoint.start_ned[1] = _prev_wp_ned(1); - rover_position_setpoint.arrival_speed = autoArrivalSpeed(_auto_speed, _waypoint_transition_angle, - _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(), _curr_wp_type); - rover_position_setpoint.cruising_speed = _auto_speed; - rover_position_setpoint.yaw = _auto_yaw; - _rover_position_setpoint_pub.publish(rover_position_setpoint); - } - -} - - -float MecanumPosControl::autoArrivalSpeed(const float auto_speed, const float waypoint_transition_angle, - const float max_speed, const float miss_spd_gain, const int curr_wp_type) -{ - // Upcoming stop - if (!PX4_ISFINITE(waypoint_transition_angle) || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND - || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - return 0.f; - } - - // Straight line speed - if (miss_spd_gain > FLT_EPSILON) { - const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, - M_PI_F, 0.f, 1.f), 0.f, 1.f); - return max_speed * (1.f - speed_reduction); - } - - return auto_speed; // Fallthrough -} - bool MecanumPosControl::runSanityChecks() { bool ret = true; @@ -323,6 +148,5 @@ bool MecanumPosControl::runSanityChecks() ret = false; } - _prev_param_check_passed = ret; return ret; } diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp index a2fa1ebd0a..becb86b5ac 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp @@ -39,9 +39,7 @@ // Libraries #include -#include #include -#include #include #include #include @@ -51,13 +49,7 @@ #include #include #include -#include -#include -#include #include -#include -#include -#include #include #include @@ -77,10 +69,16 @@ public: ~MecanumPosControl() = default; /** - * @brief Update position controller. + * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint. */ void updatePosControl(); + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + protected: /** * @brief Update the parameters of the module. @@ -93,87 +91,23 @@ private: */ void updateSubscriptions(); - /** - * @brief Generate and publish roverPositionSetpoint from position of trajectorySetpoint. - */ - void offboardPositionMode(); - - /** - * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint (Position Mode) or - * positionSetpointTriplet (Auto Mode) or roverPositionSetpoint. - */ - void generateVelocitySetpoint(); - - /** - * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint. - */ - void manualPositionMode(); - - /** - * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet. - */ - void autoPositionMode(); - - /** - * @brief Calculate the speed at which the rover should arrive at the current waypoint. During waypoint transition the speed is restricted to - * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). - * @param auto_speed Default auto speed [m/s]. - * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param max_speed Maximum velocity magnitude setpoint [m/s] - * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. - * @param curr_wp_type Type of the current waypoint. - * @return Velocity magnitude setpoint [m/s]. - */ - float autoArrivalSpeed(float auto_speed, float waypoint_transition_angle, float max_speed, float miss_spd_gain, - int curr_wp_type); - - - /** - * @brief Check if the necessary parameters are set. - * @return True if all checks pass. - */ - bool runSanityChecks(); - // uORB subscriptions - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - 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::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _rover_position_setpoint_sub{ORB_ID(rover_position_setpoint)}; - vehicle_control_mode_s _vehicle_control_mode{}; - offboard_control_mode_s _offboard_control_mode{}; rover_position_setpoint_s _rover_position_setpoint{}; // uORB publications uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; - uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; - + uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; // Variables Quatf _vehicle_attitude_quaternion{}; Vector2f _curr_pos_ned{}; - Vector2f _pos_ctl_course_direction{}; - Vector2f _pos_ctl_start_position_ned{}; Vector2f _start_ned{}; float _vehicle_yaw{0.f}; float _max_yaw_rate{0.f}; - float _pos_ctl_yaw_setpoint{0.f}; // Yaw setpoint for manual position mode, NAN if yaw rate is manually controlled [rad] - float _auto_speed{0.f}; - float _auto_yaw{0.f}; float _yaw_setpoint{NAN}; - int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; - bool _prev_param_check_passed{true}; - - // Waypoint variables - Vector2f _curr_wp_ned{}; - Vector2f _prev_wp_ned{}; - Vector2f _next_wp_ned{}; - float _cruising_speed{0.f}; - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] // Class Instances MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp index ee25b15950..1440881dfd 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp @@ -37,8 +37,6 @@ using namespace time_literals; MecanumRateControl::MecanumRateControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_rate_setpoint_pub.advertise(); - _rover_throttle_setpoint_pub.advertise(); _rover_steering_setpoint_pub.advertise(); _rover_rate_status_pub.advertise(); updateParams(); @@ -47,24 +45,21 @@ MecanumRateControl::MecanumRateControl(ModuleParams *parent) : ModuleParams(pare void MecanumRateControl::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; - _max_yaw_accel = _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F; - _max_yaw_decel = _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F; + + // Set up PID controller _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); _pid_yaw_rate.setIntegralLimit(1.f); _pid_yaw_rate.setOutputLimit(1.f); - _adjusted_yaw_rate_setpoint.setSlewRate(_max_yaw_accel); + + // Set up slew rate + _adjusted_yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F); } void MecanumRateControl::updateRateControl() { - const hrt_abstime timestamp_prev = _timestamp; + hrt_abstime timestamp_prev = _timestamp; _timestamp = hrt_absolute_time(); - _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - if (_vehicle_control_mode_sub.updated()) { - _vehicle_control_mode_sub.copy(&_vehicle_control_mode); - } + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; if (_vehicle_angular_velocity_sub.updated()) { vehicle_angular_velocity_s vehicle_angular_velocity{}; @@ -73,16 +68,25 @@ void MecanumRateControl::updateRateControl() vehicle_angular_velocity.xyz[2] : 0.f; } - if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { - if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { - generateRateAndThrottleSetpoint(); - } + if (_rover_rate_setpoint_sub.updated()) { + rover_rate_setpoint_s rover_rate_setpoint{}; + _rover_rate_setpoint_sub.copy(&rover_rate_setpoint); + _yaw_rate_setpoint = rover_rate_setpoint.yaw_rate_setpoint; + } - generateSteeringSetpoint(); + if (PX4_ISFINITE(_yaw_rate_setpoint)) { + const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + _yaw_rate_setpoint : 0.f; + const float speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, + _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rm_wheel_track.get(), dt); + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); - } else { // Reset controller and slew rate when rate control is not active + } else { _pid_yaw_rate.resetIntegral(); - _adjusted_yaw_rate_setpoint.setForcedValue(0.f); } // Publish rate controller status (logging only) @@ -95,96 +99,25 @@ void MecanumRateControl::updateRateControl() } -void MecanumRateControl::generateRateAndThrottleSetpoint() -{ - const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled - && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; - - if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = math::interpolate (manual_control_setpoint.yaw, -1.f, 1.f, - -_max_yaw_rate, _max_yaw_rate); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); - } - - } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard rate control - trajectory_setpoint_s trajectory_setpoint{}; - _trajectory_setpoint_sub.copy(&trajectory_setpoint); - - if (_offboard_control_mode_sub.updated()) { - _offboard_control_mode_sub.copy(&_offboard_control_mode); - } - - const bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.attitude; - - if (offboard_rate_control && PX4_ISFINITE(trajectory_setpoint.yawspeed)) { - rover_rate_setpoint_s rover_rate_setpoint{}; - rover_rate_setpoint.timestamp = _timestamp; - rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); - } - } -} - -void MecanumRateControl::generateSteeringSetpoint() -{ - if (_rover_rate_setpoint_sub.updated()) { - _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); - - } - - float speed_diff_normalized{0.f}; - - if (PX4_ISFINITE(_rover_rate_setpoint.yaw_rate_setpoint) && PX4_ISFINITE(_vehicle_yaw_rate)) { - const float yaw_rate_setpoint = fabsf(_rover_rate_setpoint.yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * - M_DEG_TO_RAD_F ? - _rover_rate_setpoint.yaw_rate_setpoint : 0.f; - speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, - yaw_rate_setpoint, _vehicle_yaw_rate, _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, - _max_yaw_decel, _param_rm_wheel_track.get(), _dt); - } - - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = _timestamp; - rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized; - _rover_steering_setpoint_pub.publish(rover_steering_setpoint); -} - bool MecanumRateControl::runSanityChecks() { bool ret = true; if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { ret = false; - - if (_prev_param_check_passed) { - events::send(events::ID("mecanum_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, - "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); - } + events::send(events::ID("mecanum_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); } if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_rm_max_thr_yaw_r.get() < FLT_EPSILON) && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { ret = false; - - if (_prev_param_check_passed) { - events::send(events::ID("mecanum_rate_control_conf_invalid_rate_control"), events::Log::Error, - "Invalid configuration for rate control: Neither feed forward (RM_MAX_THR_YAW_R) nor feedback (RO_YAW_RATE_P) is setup", - _param_rm_wheel_track.get(), - _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); - } + events::send(events::ID("mecanum_rate_control_conf_invalid_rate_control"), events::Log::Error, + "Invalid configuration for rate control: Neither feed forward (RM_MAX_THR_YAW_R) nor feedback (RO_YAW_RATE_P) is setup", + _param_rm_wheel_track.get(), + _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); } - _prev_param_check_passed = ret; return ret; } diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp index 912a97ff63..ef93c90ad9 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp @@ -47,15 +47,9 @@ #include #include #include -#include -#include -#include #include #include #include -#include -#include -#include /** * @brief Class for mecanum rate control. @@ -71,10 +65,21 @@ public: ~MecanumRateControl() = default; /** - * @brief Update rate controller. + * @brief Generate and publish roverSteeringSetpoint from roverRateSetpoint. */ void updateRateControl(); + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + /** + * @brief Reset rate controller. + */ + void reset() {_pid_yaw_rate.resetIntegral(); _yaw_rate_setpoint = NAN;}; + protected: /** * @brief Update the parameters of the module. @@ -83,48 +88,18 @@ protected: private: - /** - * @brief Generate and publish roverRateSetpoint and roverThrottleSetpoint from manualControlSetpoint (Acro Mode). - */ - void generateRateAndThrottleSetpoint(); - - /** - * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. - */ - void generateSteeringSetpoint(); - - /** - * @brief Check if the necessary parameters are set. - * @return True if all checks pass. - */ - bool runSanityChecks(); - // uORB subscriptions - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; - uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; - vehicle_control_mode_s _vehicle_control_mode{}; - offboard_control_mode_s _offboard_control_mode{}; - rover_rate_setpoint_s _rover_rate_setpoint{}; // uORB publications - uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; // Variables hrt_abstime _timestamp{0}; - float _max_yaw_rate{0.f}; - float _max_yaw_accel{0.f}; - float _max_yaw_decel{0.f}; float _vehicle_yaw_rate{0.f}; - float _dt{0.f}; // Time since last update [s]. - bool _prev_param_check_passed{true}; + float _yaw_rate_setpoint{NAN}; // Controllers PID _pid_yaw_rate; diff --git a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp index 0f3140bc39..25810822e4 100644 --- a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp +++ b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp @@ -39,7 +39,6 @@ MecanumVelControl::MecanumVelControl(ModuleParams *parent) : ModuleParams(parent { _rover_throttle_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_setpoint_pub.advertise(); _rover_velocity_status_pub.advertise(); updateParams(); } @@ -47,6 +46,8 @@ MecanumVelControl::MecanumVelControl(ModuleParams *parent) : ModuleParams(parent void MecanumVelControl::updateParams() { ModuleParams::updateParams(); + + // Set up PID controllers _pid_speed_x.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); _pid_speed_x.setIntegralLimit(1.f); _pid_speed_x.setOutputLimit(1.f); @@ -54,9 +55,10 @@ void MecanumVelControl::updateParams() _pid_speed_y.setIntegralLimit(1.f); _pid_speed_y.setOutputLimit(1.f); + // Set up slew rates if (_param_ro_accel_limit.get() > FLT_EPSILON) { - _speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get()); - _speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get()); + _adjusted_speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get()); + _adjusted_speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get()); } } @@ -64,31 +66,42 @@ void MecanumVelControl::updateVelControl() { const hrt_abstime timestamp_prev = _timestamp; _timestamp = hrt_absolute_time(); - _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; updateSubscriptions(); - if ((_vehicle_control_mode.flag_control_velocity_enabled) && _vehicle_control_mode.flag_armed && runSanityChecks()) { - if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Velocity Control - generateVelocitySetpoint(); - } + // 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); - generateAttitudeAndThrottleSetpoint(); + } + + // Throttle Setpoints + if (PX4_ISFINITE(_speed_x_setpoint) && PX4_ISFINITE(_speed_y_setpoint)) { + Vector2f 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_x_setpoint, _pid_speed_x, + speed_setpoint(0), _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), dt); + rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_adjusted_speed_y_setpoint, _pid_speed_y, + speed_setpoint(1), _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), dt); + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - } else { // Reset controller and slew rate when velocity control is not active - _pid_speed_x.resetIntegral(); - _speed_x_setpoint.setForcedValue(0.f); - _pid_speed_y.resetIntegral(); - _speed_y_setpoint.setForcedValue(0.f); } // 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 = _speed_x_setpoint.getState(); + 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 = _speed_y_setpoint.getState(); + 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); @@ -96,10 +109,6 @@ void MecanumVelControl::updateVelControl() void MecanumVelControl::updateSubscriptions() { - if (_vehicle_control_mode_sub.updated()) { - _vehicle_control_mode_sub.copy(&_vehicle_control_mode); - } - if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude{}; _vehicle_attitude_sub.copy(&vehicle_attitude); @@ -116,110 +125,66 @@ 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; } -} -void MecanumVelControl::generateVelocitySetpoint() -{ - trajectory_setpoint_s trajectory_setpoint{}; - _trajectory_setpoint_sub.copy(&trajectory_setpoint); - - if (_offboard_control_mode_sub.updated()) { - _offboard_control_mode_sub.copy(&_offboard_control_mode); - } - - const bool offboard_vel_control = _offboard_control_mode.velocity && !_offboard_control_mode.position; - - const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); - - if (offboard_vel_control && velocity_in_local_frame.isAllFinite()) { - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = _timestamp; - rover_velocity_setpoint.speed = velocity_in_local_frame.norm(); - rover_velocity_setpoint.bearing = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); - rover_velocity_setpoint.yaw = _vehicle_yaw; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); - - } -} - -void MecanumVelControl::generateAttitudeAndThrottleSetpoint() -{ if (_rover_velocity_setpoint_sub.updated()) { - _rover_velocity_setpoint_sub.copy(&_rover_velocity_setpoint); - } + rover_velocity_setpoint_s rover_velocity_setpoint; + _rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint); - // Attitude Setpoint - if (PX4_ISFINITE(_rover_velocity_setpoint.yaw)) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.yaw; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - _last_attitude_setpoint_update = _timestamp; + if (PX4_ISFINITE(rover_velocity_setpoint.speed) && PX4_ISFINITE(rover_velocity_setpoint.bearing)) { + const Vector3f velocity_in_local_frame(rover_velocity_setpoint.speed * cosf(rover_velocity_setpoint.bearing), + rover_velocity_setpoint.speed * 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 { - 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); - _last_attitude_setpoint_update = _timestamp; - } + } else if (PX4_ISFINITE(rover_velocity_setpoint.speed)) { + _speed_x_setpoint = rover_velocity_setpoint.speed; + _speed_y_setpoint = 0.f; - // Throttle Setpoint - float speed_body_x_setpoint{0.f}; - float speed_body_y_setpoint{0.f}; - - if (fabsf(_rover_velocity_setpoint.speed) > FLT_EPSILON && PX4_ISFINITE(_rover_velocity_setpoint.bearing)) { - const Vector3f velocity_in_local_frame(_rover_velocity_setpoint.speed * cosf( - _rover_velocity_setpoint.bearing), - _rover_velocity_setpoint.speed * sinf(_rover_velocity_setpoint.bearing), 0.f); - const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - speed_body_x_setpoint = velocity_in_body_frame(0); - speed_body_y_setpoint = velocity_in_body_frame(1); - - } else { - speed_body_x_setpoint = _rover_velocity_setpoint.speed; - speed_body_y_setpoint = 0.f; - } - - if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { // Adjust speed setpoints if infeasible - if (_rover_steering_setpoint_sub.updated()) { - _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } else { + _speed_x_setpoint = NAN; + _speed_y_setpoint = NAN; } - float speed_body_x_setpoint_normalized = math::interpolate(speed_body_x_setpoint, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + _yaw_setpoint = rover_velocity_setpoint.yaw; + } +} - float speed_body_y_setpoint_normalized = math::interpolate(speed_body_y_setpoint, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); - - const float total_speed = fabsf(speed_body_x_setpoint_normalized) + fabsf(speed_body_y_setpoint_normalized) + fabsf( - _rover_steering_setpoint.normalized_speed_diff); - - if (total_speed > 1.f) { - const float theta = atan2f(fabsf(speed_body_y_setpoint_normalized), fabsf(speed_body_x_setpoint_normalized)); - const float magnitude = (1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff)) / (sinf(theta) + cosf(theta)); - const float normalization = 1.f / (sqrtf(powf(speed_body_x_setpoint_normalized, - 2.f) + powf(speed_body_y_setpoint_normalized, 2.f))); - speed_body_x_setpoint_normalized *= magnitude * normalization; - speed_body_y_setpoint_normalized *= magnitude * normalization; - speed_body_x_setpoint = math::interpolate(speed_body_x_setpoint_normalized, -1.f, 1.f, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); - speed_body_y_setpoint = math::interpolate(speed_body_y_setpoint_normalized, -1.f, 1.f, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); - } +Vector2f MecanumVelControl::calcSpeedSetpoint() +{ + 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_speed_diff; } - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; - speed_body_x_setpoint = fabsf(speed_body_x_setpoint) > _param_ro_speed_th.get() ? speed_body_x_setpoint : 0.f; - speed_body_y_setpoint = fabsf(speed_body_y_setpoint) > _param_ro_speed_th.get() ? speed_body_y_setpoint : 0.f; - rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_x_setpoint, _pid_speed_x, - speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), _dt); - rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_speed_y_setpoint, _pid_speed_y, - speed_body_y_setpoint, _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), _dt); - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + float speed_x_setpoint_normalized = math::interpolate(_speed_x_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + float speed_y_setpoint_normalized = math::interpolate(_speed_y_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + const float total_speed = fabsf(speed_x_setpoint_normalized) + fabsf(speed_y_setpoint_normalized) + fabsf( + _normalized_speed_diff); + + Vector2f speed_setpoint = Vector2f(_speed_x_setpoint, _speed_y_setpoint); + + if (total_speed > 1.f) { + const float theta = atan2f(fabsf(speed_y_setpoint_normalized), fabsf(speed_x_setpoint_normalized)); + const float magnitude = (1.f - fabsf(_normalized_speed_diff)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(speed_x_setpoint_normalized, + 2.f) + powf(speed_y_setpoint_normalized, 2.f))); + speed_x_setpoint_normalized *= magnitude * normalization; + speed_y_setpoint_normalized *= magnitude * normalization; + speed_setpoint(0) = math::interpolate(speed_x_setpoint_normalized, -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + speed_setpoint(1) = math::interpolate(speed_y_setpoint_normalized, -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + + speed_setpoint(0) = fabsf(speed_setpoint(0)) > _param_ro_speed_th.get() ? speed_setpoint(0) : 0.f; + speed_setpoint(1) = fabsf(speed_setpoint(1)) > _param_ro_speed_th.get() ? speed_setpoint(1) : 0.f; + + return speed_setpoint; } bool MecanumVelControl::runSanityChecks() @@ -228,25 +193,18 @@ bool MecanumVelControl::runSanityChecks() if (_param_ro_speed_limit.get() < FLT_EPSILON) { ret = false; - - if (_prev_param_check_passed) { - events::send(events::ID("mecanum_vel_control_conf_invalid_speed_lim"), events::Log::Error, - "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); - } + events::send(events::ID("mecanum_vel_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; - - if (_prev_param_check_passed) { - events::send(events::ID("mecanum_vel_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()); - } + events::send(events::ID("mecanum_vel_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()); } - _prev_param_check_passed = ret; return ret; } diff --git a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp index 9dd433b3b4..1502b8dccd 100644 --- a/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp +++ b/src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp @@ -52,10 +52,7 @@ #include #include #include -#include -#include #include -#include #include using namespace matrix; @@ -74,10 +71,21 @@ public: ~MecanumVelControl() = default; /** - * @brief Update velocity controller. + * @brief Generate and publish roverAttitudeSetpoint and RoverThrottleSetpoint from roverVelocitySetpoint. */ void updateVelControl(); + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + /** + * @brief Reset velocity 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;}; + protected: /** * @brief Update the parameters of the module. @@ -91,57 +99,38 @@ private: void updateSubscriptions(); /** - * @brief Generate and publish roverVelocitySetpoint from velocity of trajectorySetpoint. + * @brief Adjust the speed setpoint if they are infeasible + * @return Speed setpoints */ - void generateVelocitySetpoint(); - - /** - * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint - * from roverVelocitySetpoint. - */ - void generateAttitudeAndThrottleSetpoint(); - - /** - * @brief Check if the necessary parameters are set. - * @return True if all checks pass. - */ - bool runSanityChecks(); + Vector2f calcSpeedSetpoint(); // uORB subscriptions - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - 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::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _rover_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)}; - uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; - vehicle_control_mode_s _vehicle_control_mode{}; - offboard_control_mode_s _offboard_control_mode{}; - rover_steering_setpoint_s _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_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; - rover_velocity_setpoint_s _rover_velocity_setpoint{}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; // Variables hrt_abstime _timestamp{0}; - hrt_abstime _last_attitude_setpoint_update{0}; Quatf _vehicle_attitude_quaternion{}; float _vehicle_speed_body_x{0.f}; float _vehicle_speed_body_y{0.f}; float _vehicle_yaw{0.f}; - float _dt{0.f}; - bool _prev_param_check_passed{false}; + float _speed_x_setpoint{NAN}; + float _speed_y_setpoint{NAN}; + float _normalized_speed_diff{NAN}; + float _yaw_setpoint{NAN}; // Controllers PID _pid_speed_x; PID _pid_speed_y; - SlewRate _speed_x_setpoint; - SlewRate _speed_y_setpoint; + SlewRate _adjusted_speed_x_setpoint; + SlewRate _adjusted_speed_y_setpoint; DEFINE_PARAMETERS( (ParamFloat) _param_ro_max_thr_speed, diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index c1e4b0cfa9..67d811de7a 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -56,31 +56,130 @@ void RoverMecanum::updateParams() void RoverMecanum::Run() { if (_parameter_update_sub.updated()) { + parameter_update_s param_update{}; + _parameter_update_sub.copy(¶m_update); updateParams(); + runSanityChecks(); } - _mecanum_pos_control.updatePosControl(); - _mecanum_vel_control.updateVelControl(); - _mecanum_att_control.updateAttControl(); - _mecanum_rate_control.updateRateControl(); - if (_vehicle_control_mode_sub.updated()) { - _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + vehicle_control_mode_s vehicle_control_mode{}; + _vehicle_control_mode_sub.copy(&vehicle_control_mode); + + // Run sanity checks if the control mode changes (Note: This has to be done this way, because the topic is periodically updated at 2 Hz) + if (_vehicle_control_mode.flag_control_manual_enabled != vehicle_control_mode.flag_control_manual_enabled || + _vehicle_control_mode.flag_control_auto_enabled != vehicle_control_mode.flag_control_auto_enabled || + _vehicle_control_mode.flag_control_offboard_enabled != vehicle_control_mode.flag_control_offboard_enabled || + _vehicle_control_mode.flag_control_position_enabled != vehicle_control_mode.flag_control_position_enabled || + _vehicle_control_mode.flag_control_velocity_enabled != vehicle_control_mode.flag_control_velocity_enabled || + _vehicle_control_mode.flag_control_attitude_enabled != vehicle_control_mode.flag_control_attitude_enabled || + _vehicle_control_mode.flag_control_rates_enabled != vehicle_control_mode.flag_control_rates_enabled || + _vehicle_control_mode.flag_control_allocation_enabled != vehicle_control_mode.flag_control_allocation_enabled) { + _vehicle_control_mode = vehicle_control_mode; + runSanityChecks(); + reset(); + + } else { + _vehicle_control_mode = vehicle_control_mode; + } + } - const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled - && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled - && !_vehicle_control_mode.flag_control_rates_enabled; + if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) { - if (full_manual_mode_enabled) { // Manual mode - _mecanum_act_control.manualManualMode(); + _was_armed = true; + + // Generate setpoints + if (_vehicle_control_mode.flag_control_manual_enabled) { + manualControl(); + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { + _auto_mode.autoControl(); + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { + _offboard_mode.offboardControl(); + } + + updateControllers(); + + } else if (_was_armed) { // Reset all controllers and stop the vehicle + reset(); + _mecanum_act_control.stopVehicle(); + _was_armed = false; } - if (_vehicle_control_mode.flag_armed) { +} + +void RoverMecanum::manualControl() +{ + if (_vehicle_control_mode.flag_control_position_enabled) { + _manual_mode.position(); + + } else if (_vehicle_control_mode.flag_control_attitude_enabled) { + _manual_mode.stab(); + + } else if (_vehicle_control_mode.flag_control_rates_enabled) { + _manual_mode.acro(); + + } else if (_vehicle_control_mode.flag_control_allocation_enabled) { + _manual_mode.manual(); + } +} + +void RoverMecanum::updateControllers() +{ + if (_vehicle_control_mode.flag_control_position_enabled) { + _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(); + } + + if (_vehicle_control_mode.flag_control_rates_enabled) { + _mecanum_rate_control.updateRateControl(); + } + + if (_vehicle_control_mode.flag_control_allocation_enabled) { _mecanum_act_control.updateActControl(); + } +} +void RoverMecanum::runSanityChecks() +{ + if (_vehicle_control_mode.flag_control_rates_enabled && !_mecanum_rate_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; } + if (_vehicle_control_mode.flag_control_attitude_enabled && !_mecanum_att_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; + } + + if (_vehicle_control_mode.flag_control_velocity_enabled && !_mecanum_vel_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; + } + + if (_vehicle_control_mode.flag_control_position_enabled && !_mecanum_pos_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; + } + + _sanity_checks_passed = true; +} + +void RoverMecanum::reset() +{ + _mecanum_vel_control.reset(); + _mecanum_att_control.reset(); + _mecanum_rate_control.reset(); + _manual_mode.reset(); } int RoverMecanum::task_spawn(int argc, char *argv[]) diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index 9c149092fc..8b50094ecf 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -40,20 +40,13 @@ #include #include -// Libraries -#include -#include +// Library includes +#include // uORB includes #include -#include -#include #include -#include -#include -#include #include -#include // Local includes #include "MecanumActControl/MecanumActControl.hpp" @@ -61,6 +54,9 @@ #include "MecanumAttControl/MecanumAttControl.hpp" #include "MecanumVelControl/MecanumVelControl.hpp" #include "MecanumPosControl/MecanumPosControl.hpp" +#include "MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp" +#include "MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp" +#include "MecanumDriveModes/MecanumOffboardMode/MecanumOffboardMode.hpp" class RoverMecanum : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -92,15 +88,46 @@ protected: private: void Run() override; + /** + * @brief Handle manual control + */ + void manualControl(); + + /** + * @brief Update the controllers + */ + void updateControllers(); + + /** + * @brief Check proper parameter setup for the controllers + * + * Modifies: + * + * - _sanity_checks_passed: true if checks for all active controllers pass + */ + void runSanityChecks(); + + /** + * @brief Reset controllers and manual mode variables. + */ + void reset(); + // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_control_mode_s _vehicle_control_mode{}; // Class instances - MecanumActControl _mecanum_act_control{this}; - MecanumRateControl _mecanum_rate_control{this}; - MecanumAttControl _mecanum_att_control{this}; - MecanumVelControl _mecanum_vel_control{this}; - MecanumPosControl _mecanum_pos_control{this}; + MecanumActControl _mecanum_act_control{this}; + MecanumRateControl _mecanum_rate_control{this}; + MecanumAttControl _mecanum_att_control{this}; + MecanumVelControl _mecanum_vel_control{this}; + MecanumPosControl _mecanum_pos_control{this}; + MecanumAutoMode _auto_mode{this}; + MecanumManualMode _manual_mode{this}; + MecanumOffboardMode _offboard_mode{this}; + + // Variables + bool _sanity_checks_passed{true}; // True if checks for all active controllers pass + bool _was_armed{false}; // True if the vehicle was armed before the last reset };