diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 8f71478554..d2d25280a1 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -36,6 +36,7 @@ add_subdirectory(AckermannRateControl) add_subdirectory(AckermannAttControl) add_subdirectory(AckermannVelControl) add_subdirectory(AckermannPosControl) +add_subdirectory(DriveModes) px4_add_module( MODULE modules__rover_ackermann @@ -49,6 +50,9 @@ px4_add_module( AckermannAttControl AckermannVelControl AckermannPosControl + AutoMode + ManualMode + OffboardMode px4_work_queue rover_control pure_pursuit diff --git a/src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.cpp b/src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.cpp new file mode 100644 index 0000000000..53ab04dcd9 --- /dev/null +++ b/src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.cpp @@ -0,0 +1,196 @@ +/**************************************************************************** + * + * 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 "AutoMode.hpp" + +using namespace time_literals; + +AutoMode::AutoMode(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_position_setpoint_pub.advertise(); +} + +void AutoMode::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON + && _param_ra_max_str_ang.get() > FLT_EPSILON) { + _min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get()); + } +} + +void AutoMode::autoControl() +{ + 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); + + 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); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + } + + if (_position_setpoint_triplet_sub.updated()) { + updateWaypointsAndAcceptanceRadius(); + } + + // Distances to waypoints + const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2)); + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + 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, _min_speed, _acceptance_radius, _curr_wp_type, + _waypoint_transition_angle, _max_yaw_rate); + rover_position_setpoint.cruising_speed = cruisingSpeed(_cruising_speed, _min_speed, distance_to_prev_wp, + distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _waypoint_transition_angle, + _prev_waypoint_transition_angle, _max_yaw_rate); + rover_position_setpoint.yaw = NAN; + _rover_position_setpoint_pub.publish(rover_position_setpoint); +} + +void AutoMode::updateWaypointsAndAcceptanceRadius() +{ + 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); + + _prev_waypoint_transition_angle = _waypoint_transition_angle; + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Update acceptance radius + _prev_acceptance_radius = _acceptance_radius; + + if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { + _acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(), + _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get()); + + } else { + _acceptance_radius = _param_nav_acc_rad.get(); + } + + // Waypoint cruising speed + _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(); +} + +float AutoMode::updateAcceptanceRadius(const float waypoint_transition_angle, + const float default_acceptance_radius, const float acceptance_radius_gain, + const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) +{ + // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment + float acceptance_radius = default_acceptance_radius; + + if (PX4_ISFINITE(_waypoint_transition_angle)) { + const float theta = waypoint_transition_angle / 2.f; + const float min_turning_radius = wheel_base / sinf(max_steer_angle); + const float acceptance_radius_temp = min_turning_radius / tanf(theta); + const float acceptance_radius_temp_scaled = acceptance_radius_gain * + acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects + acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, + acceptance_radius_max); + } + + // Publish updated acceptance radius + position_controller_status_s pos_ctrl_status{}; + pos_ctrl_status.acceptance_radius = acceptance_radius; + pos_ctrl_status.timestamp = hrt_absolute_time(); + _position_controller_status_pub.publish(pos_ctrl_status); + return acceptance_radius; +} + +float AutoMode::arrivalSpeed(const float cruising_speed, const float miss_speed_min, const float acc_rad, + const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate) +{ + 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; // Stop at the waypoint + + } else { + const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); + const float cornering_speed = max_yaw_rate * turning_circle; + return math::constrain(cornering_speed, miss_speed_min, cruising_speed); // Slow down for cornering + } +} + +float AutoMode::cruisingSpeed(const float cruising_speed, const float miss_speed_min, + const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, const float prev_acc_rad, + const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_yaw_rate) +{ + // Catch improper values + if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) { + return cruising_speed; + } + + // Cornering slow down effect + if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON && PX4_ISFINITE(prev_waypoint_transition_angle)) { + const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f); + const float cornering_speed = max_yaw_rate * turning_circle; + return math::constrain(cornering_speed, miss_speed_min, cruising_speed); + + } + + if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON && PX4_ISFINITE(waypoint_transition_angle)) { + const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); + const float cornering_speed = max_yaw_rate * turning_circle; + return math::constrain(cornering_speed, miss_speed_min, cruising_speed); + + } + + return cruising_speed; // Fallthrough + +} diff --git a/src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.hpp b/src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.hpp new file mode 100644 index 0000000000..c20a7c006f --- /dev/null +++ b/src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.hpp @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * 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 + +/** + * @brief Class for ackermann auto mode. + */ +class AutoMode : public ModuleParams +{ +public: + /** + * @brief Constructor for auto mode. + * @param parent The parent ModuleParams object. + */ + AutoMode(ModuleParams *parent); + ~AutoMode() = default; + + /** + * @brief Generate and publish roverPositionSetpoint from positionSetpointTriplet. + */ + void autoControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update global/NED waypoint coordinates and acceptance radius. + */ + void updateWaypointsAndAcceptanceRadius(); + + /** + * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment + * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param default_acceptance_radius Default acceptance radius for waypoints [m]. + * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. + * @param acceptance_radius_max Maximum value for the acceptance radius [m]. + * @param wheel_base Rover wheelbase [m]. + * @param max_steer_angle Rover maximum steer angle [rad]. + * @return Updated acceptance radius [m]. + */ + float updateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius, + float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle); + + /** + * @brief Calculate the speed at which the rover should arrive at the current waypoint based on the upcoming corner. + * @param cruising_speed Cruising speed [m/s]. + * @param miss_speed_min Minimum speed setpoint [m/s]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param curr_wp_type Type of the current waypoint. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_yaw_rate Maximum yaw rate setpoint [rad/s] + * @return Speed setpoint [m/s]. + */ + float arrivalSpeed(float cruising_speed, float miss_speed_min, float acc_rad, int curr_wp_type, + float waypoint_transition_angle, float max_yaw_rate); + + /** + * @brief Calculate the cruising speed setpoint. During cornering the speed is restricted based on the radius of the corner. + * @param cruising_speed Cruising speed [m/s]. + * @param miss_speed_min Minimum speed setpoint [m/s]. + * @param distance_to_prev_wp Distance to the previous waypoint [m]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_yaw_rate Maximum yaw rate setpoint [rad/s] + * @return Speed setpoint [m/s]. + */ + float cruisingSpeed(float cruising_speed, float miss_speed_min, float distance_to_prev_wp, + float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float waypoint_transition_angle, + float prev_waypoint_transition_angle, float max_yaw_rate); + + // uORB subscriptions + 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 publications + uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + Quatf _vehicle_attitude_quaternion{}; + Vector2f _curr_wp_ned{NAN, NAN}; + Vector2f _prev_wp_ned{NAN, NAN}; + Vector2f _next_wp_ned{NAN, NAN}; + Vector2f _curr_pos_ned{NAN, NAN}; + float _acceptance_radius{0.5f}; + float _prev_acceptance_radius{0.5f}; + float _cruising_speed{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + float _max_yaw_rate{NAN}; + float _vehicle_yaw{NAN}; + float _min_speed{NAN}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base. + int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_ra_acc_rad_max, + (ParamFloat) _param_ra_acc_rad_gain + ) +}; diff --git a/src/modules/rover_ackermann/DriveModes/AutoMode/CMakeLists.txt b/src/modules/rover_ackermann/DriveModes/AutoMode/CMakeLists.txt new file mode 100644 index 0000000000..6f68333be1 --- /dev/null +++ b/src/modules/rover_ackermann/DriveModes/AutoMode/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(AutoMode + AutoMode.cpp +) + +target_include_directories(AutoMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/DriveModes/CMakeLists.txt b/src/modules/rover_ackermann/DriveModes/CMakeLists.txt new file mode 100644 index 0000000000..1918b9644e --- /dev/null +++ b/src/modules/rover_ackermann/DriveModes/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(AutoMode) +add_subdirectory(ManualMode) +add_subdirectory(OffboardMode) diff --git a/src/modules/rover_ackermann/DriveModes/ManualMode/CMakeLists.txt b/src/modules/rover_ackermann/DriveModes/ManualMode/CMakeLists.txt new file mode 100644 index 0000000000..827489ad9f --- /dev/null +++ b/src/modules/rover_ackermann/DriveModes/ManualMode/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(ManualMode + ManualMode.cpp +) + +target_include_directories(ManualMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.cpp b/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.cpp new file mode 100644 index 0000000000..f2810f06f4 --- /dev/null +++ b/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.cpp @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * 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 "ManualMode.hpp" + +using namespace time_literals; + +ManualMode::ManualMode(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 ManualMode::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; +} + +void ManualMode::manualControl(const int nav_state) +{ + switch (nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + manual(); + break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: + acro(); + break; + + case vehicle_status_s::NAVIGATION_STATE_STAB: + stab(); + break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + position(); + break; + } +} + +void ManualMode::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_steering_angle = manual_control_setpoint.roll; + _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 = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); +} + +void ManualMode::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 = 0.f; + _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 = matrix::sign(manual_control_setpoint.throttle) * math::interpolate + (manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +void ManualMode::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 = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + const float yaw_delta = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _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 + || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control + _stab_yaw_setpoint = NAN; + const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + matrix::sign(manual_control_setpoint.throttle) * yaw_delta); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = hrt_absolute_time(); + 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 = hrt_absolute_time(); + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } +} + +void ManualMode::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); + + 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); + } + + _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); + + const float speed_setpoint = math::interpolate(manual_control_setpoint.throttle, + -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.roll, + _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 + || fabsf(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _pos_ctl_course_direction = Vector2f(NAN, NAN); + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta); + const Vector2f pos_ctl_course_direction = Vector2f(cos(yaw_setpoint), sin(yaw_setpoint)); + const Vector2f target_waypoint_ned = _curr_pos_ned + sign(speed_setpoint) * _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 = speed_setpoint; + rover_position_setpoint.yaw = NAN; + _rover_position_setpoint_pub.publish(rover_position_setpoint); + + } else { // Course control if the steering input is zero (keep driving on a straight line) + if (!_pos_ctl_course_direction.isAllFinite()) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + } + + // 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 + sign(speed_setpoint) * + 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] = _pos_ctl_start_position_ned(0); + rover_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1); + rover_position_setpoint.arrival_speed = NAN; + rover_position_setpoint.cruising_speed = speed_setpoint; + rover_position_setpoint.yaw = NAN; + _rover_position_setpoint_pub.publish(rover_position_setpoint); + } +} + +void ManualMode::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_ackermann/DriveModes/ManualMode/ManualMode.hpp b/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.hpp new file mode 100644 index 0000000000..8079b326ac --- /dev/null +++ b/src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * 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 +#include + +/** + * @brief Class for ackermann manual mode. + */ +class ManualMode : public ModuleParams +{ +public: + /** + * @brief Constructor for ManualMode. + * @param parent The parent ModuleParams object. + */ + ManualMode(ModuleParams *parent); + ~ManualMode() = default; + + /** + * @brief Generate and publish roverSetpoints from manualControlSetpoints. + */ + void manualControl(int nav_state); + + /** + * @brief Reset manual mode variables. + */ + void reset(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. + */ + void manual(); + + /** + * @brief Generate and publish roverThrottleSetpoint and RoverRateSetpoint from manualControlSetpoint. + */ + void acro(); + + /** + * @brief Generate and publish roverThrottleSetpoint and RoverAttitudeSetpoint from manualControlSetpoint. + */ + void stab(); + + /** + * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint. + */ + void position(); + + // 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 + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + Quatf _vehicle_attitude_quaternion{}; + Vector2f _pos_ctl_course_direction{NAN, NAN}; + Vector2f _pos_ctl_start_position_ned{NAN, NAN}; + Vector2f _curr_pos_ned{NAN, 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_p, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_speed_limit + ) +}; diff --git a/src/modules/rover_ackermann/DriveModes/OffboardMode/CMakeLists.txt b/src/modules/rover_ackermann/DriveModes/OffboardMode/CMakeLists.txt new file mode 100644 index 0000000000..a73b2dedd8 --- /dev/null +++ b/src/modules/rover_ackermann/DriveModes/OffboardMode/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(OffboardMode + OffboardMode.cpp +) + +target_include_directories(OffboardMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/DriveModes/OffboardMode/OffboardMode.cpp b/src/modules/rover_ackermann/DriveModes/OffboardMode/OffboardMode.cpp new file mode 100644 index 0000000000..dfd12cddb0 --- /dev/null +++ b/src/modules/rover_ackermann/DriveModes/OffboardMode/OffboardMode.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 "OffboardMode.hpp" + +using namespace time_literals; + +OffboardMode::OffboardMode(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_velocity_setpoint_pub.advertise(); + _rover_position_setpoint_pub.advertise(); +} + +void OffboardMode::updateParams() +{ + ModuleParams::updateParams(); +} + +void OffboardMode::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); + + } +} diff --git a/src/modules/rover_ackermann/DriveModes/OffboardMode/OffboardMode.hpp b/src/modules/rover_ackermann/DriveModes/OffboardMode/OffboardMode.hpp new file mode 100644 index 0000000000..331df5e907 --- /dev/null +++ b/src/modules/rover_ackermann/DriveModes/OffboardMode/OffboardMode.hpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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 + +using namespace matrix; + +/** + * @brief Class for ackermann manual mode. + */ +class OffboardMode : public ModuleParams +{ +public: + /** + * @brief Constructor for OffboardMode. + * @param parent The parent ModuleParams object. + */ + OffboardMode(ModuleParams *parent); + ~OffboardMode() = 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_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_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index fcc0f059c6..236ecb6af2 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -51,12 +51,6 @@ bool RoverAckermann::init() void RoverAckermann::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; - - if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON - && _param_ra_max_str_ang.get() > FLT_EPSILON) { - _min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get()); - } } void RoverAckermann::Run() @@ -99,13 +93,13 @@ void RoverAckermann::Run() if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) { // Generate setpoints if (_vehicle_control_mode.flag_control_manual_enabled) { - manualControl(); + _manual_mode.manualControl(_nav_state); } else if (_vehicle_control_mode.flag_control_auto_enabled) { - autoPositionMode(); + _auto_mode.autoControl(); } else if (_vehicle_control_mode.flag_control_offboard_enabled) { - offboardControl(); + _offboard_mode.offboardControl(); } updateControllers(); @@ -118,348 +112,6 @@ void RoverAckermann::Run() } -void RoverAckermann::manualControl() -{ - switch (_nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: - manualManualMode(); - break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: - manualAcroMode(); - break; - - case vehicle_status_s::NAVIGATION_STATE_STAB: - manualStabMode(); - break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: - manualPositionMode(); - break; - } -} - -void RoverAckermann::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 = hrt_absolute_time(); - rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll; - _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 = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); -} - -void RoverAckermann::manualAcroMode() -{ - 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 = 0.f; - _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 = matrix::sign(manual_control_setpoint.throttle) * math::interpolate - (manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - _rover_rate_setpoint_pub.publish(rover_rate_setpoint); -} - -void RoverAckermann::manualStabMode() -{ - 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 = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - - const float yaw_delta = math::interpolate(math::deadzone(manual_control_setpoint.roll, - _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 - || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control - _stab_yaw_setpoint = NAN; - const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + matrix::sign(manual_control_setpoint.throttle) * yaw_delta); - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = hrt_absolute_time(); - 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 = hrt_absolute_time(); - rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - } -} - -void RoverAckermann::manualPositionMode() -{ - 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); - - 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); - } - - _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); - - const float speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -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.roll, - _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 - || fabsf(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control - _pos_ctl_course_direction = Vector2f(NAN, NAN); - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta); - const Vector2f pos_ctl_course_direction = Vector2f(cos(yaw_setpoint), sin(yaw_setpoint)); - const Vector2f target_waypoint_ned = _curr_pos_ned + sign(speed_setpoint) * _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 = speed_setpoint; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); - - } else { // Course control if the steering input is zero (keep driving on a straight line) - if (!_pos_ctl_course_direction.isAllFinite()) { - _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); - _pos_ctl_start_position_ned = _curr_pos_ned; - } - - // 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 + sign(speed_setpoint) * - 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] = _pos_ctl_start_position_ned(0); - rover_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1); - rover_position_setpoint.arrival_speed = NAN; - rover_position_setpoint.cruising_speed = speed_setpoint; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); - } -} - -void RoverAckermann::autoPositionMode() -{ - 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); - - 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); - } - - _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - } - - if (_position_setpoint_triplet_sub.updated()) { - autoUpdateWaypointsAndAcceptanceRadius(); - } - - // Distances to waypoints - const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0), - 2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2)); - const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), - 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); - - 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(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type, - _waypoint_transition_angle, _max_yaw_rate); - rover_position_setpoint.cruising_speed = autoCruisingSpeed(_cruising_speed, _min_speed, distance_to_prev_wp, - distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _waypoint_transition_angle, - _prev_waypoint_transition_angle, _max_yaw_rate); - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); -} - -void RoverAckermann::autoUpdateWaypointsAndAcceptanceRadius() -{ - 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); - - _prev_waypoint_transition_angle = _waypoint_transition_angle; - _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); - - // Update acceptance radius - _prev_acceptance_radius = _acceptance_radius; - - if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { - _acceptance_radius = autoUpdateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(), - _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get()); - - } else { - _acceptance_radius = _param_nav_acc_rad.get(); - } - - // Waypoint cruising speed - _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(); -} - -float RoverAckermann::autoUpdateAcceptanceRadius(const float waypoint_transition_angle, - const float default_acceptance_radius, const float acceptance_radius_gain, - const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) -{ - // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment - float acceptance_radius = default_acceptance_radius; - - if (PX4_ISFINITE(_waypoint_transition_angle)) { - const float theta = waypoint_transition_angle / 2.f; - const float min_turning_radius = wheel_base / sinf(max_steer_angle); - const float acceptance_radius_temp = min_turning_radius / tanf(theta); - const float acceptance_radius_temp_scaled = acceptance_radius_gain * - acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects - acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, - acceptance_radius_max); - } - - // Publish updated acceptance radius - position_controller_status_s pos_ctrl_status{}; - pos_ctrl_status.acceptance_radius = acceptance_radius; - pos_ctrl_status.timestamp = hrt_absolute_time(); - _position_controller_status_pub.publish(pos_ctrl_status); - return acceptance_radius; -} - -float RoverAckermann::autoArrivalSpeed(const float cruising_speed, const float miss_speed_min, const float acc_rad, - const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate) -{ - 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; // Stop at the waypoint - - } else { - const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); - const float cornering_speed = max_yaw_rate * turning_circle; - return math::constrain(cornering_speed, miss_speed_min, cruising_speed); // Slow down for cornering - } -} - -float RoverAckermann::autoCruisingSpeed(const float cruising_speed, const float miss_speed_min, - const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, const float prev_acc_rad, - const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_yaw_rate) -{ - // Catch improper values - if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) { - return cruising_speed; - } - - // Cornering slow down effect - if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON && PX4_ISFINITE(prev_waypoint_transition_angle)) { - const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f); - const float cornering_speed = max_yaw_rate * turning_circle; - return math::constrain(cornering_speed, miss_speed_min, cruising_speed); - - } - - if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON && PX4_ISFINITE(waypoint_transition_angle)) { - const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); - const float cornering_speed = max_yaw_rate * turning_circle; - return math::constrain(cornering_speed, miss_speed_min, cruising_speed); - - } - - return cruising_speed; // Fallthrough - -} - -void RoverAckermann::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); - - } -} - void RoverAckermann::updateControllers() { if (_vehicle_control_mode.flag_control_position_enabled) { @@ -513,10 +165,7 @@ void RoverAckermann::reset() _ackermann_vel_control.reset(); _ackermann_att_control.reset(); _ackermann_rate_control.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); + _manual_mode.reset(); } int RoverAckermann::task_spawn(int argc, char *argv[]) diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index 968f99ed4f..c9f8370afa 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -42,28 +42,13 @@ // Library includes #include -#include // uORB includes #include #include -#include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // Local includes #include "AckermannActControl/AckermannActControl.hpp" @@ -71,6 +56,9 @@ #include "AckermannAttControl/AckermannAttControl.hpp" #include "AckermannVelControl/AckermannVelControl.hpp" #include "AckermannPosControl/AckermannPosControl.hpp" +#include "DriveModes/AutoMode/AutoMode.hpp" +#include "DriveModes/ManualMode/ManualMode.hpp" +#include "DriveModes/OffboardMode/OffboardMode.hpp" class RoverAckermann : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -103,91 +91,7 @@ private: void Run() override; /** - * @brief Handle manual control - */ - void manualControl(); - - /** - * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. - */ - void manualManualMode(); - - /** - * @brief Generate and publish roverThrottleSetpoint and RoverRateSetpoint from manualControlSetpoint. - */ - void manualAcroMode(); - - /** - * @brief Generate and publish roverThrottleSetpoint and RoverAttitudeSetpoint from manualControlSetpoint. - */ - void manualStabMode(); - - /** - * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint. - */ - void manualPositionMode(); - - /** - * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet. - */ - void autoPositionMode(); - - /** - * @brief Update global/NED waypoint coordinates and acceptance radius. - */ - void autoUpdateWaypointsAndAcceptanceRadius(); - - /** - * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment - * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. - * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param default_acceptance_radius Default acceptance radius for waypoints [m]. - * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. - * @param acceptance_radius_max Maximum value for the acceptance radius [m]. - * @param wheel_base Rover wheelbase [m]. - * @param max_steer_angle Rover maximum steer angle [rad]. - * @return Updated acceptance radius [m]. - */ - float autoUpdateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius, - float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle); - - /** - * @brief Calculate the speed at which the rover should arrive at the current waypoint based on the upcoming corner. - * @param cruising_speed Cruising speed [m/s]. - * @param miss_speed_min Minimum speed setpoint [m/s]. - * @param acc_rad Acceptance radius of the current waypoint [m]. - * @param curr_wp_type Type of the current waypoint. - * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param max_yaw_rate Maximum yaw rate setpoint [rad/s] - * @return Speed setpoint [m/s]. - */ - float autoArrivalSpeed(float cruising_speed, float miss_speed_min, float acc_rad, int curr_wp_type, - float waypoint_transition_angle, float max_yaw_rate); - - /** - * @brief Calculate the cruising speed setpoint. During cornering the speed is restricted based on the radius of the corner. - * @param cruising_speed Cruising speed [m/s]. - * @param miss_speed_min Minimum speed setpoint [m/s]. - * @param distance_to_prev_wp Distance to the previous waypoint [m]. - * @param distance_to_curr_wp Distance to the current waypoint [m]. - * @param acc_rad Acceptance radius of the current waypoint [m]. - * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. - * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param max_yaw_rate Maximum yaw rate setpoint [rad/s] - * @return Speed setpoint [m/s]. - */ - float autoCruisingSpeed(float cruising_speed, float miss_speed_min, float distance_to_prev_wp, - float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float waypoint_transition_angle, - float prev_waypoint_transition_angle, float max_yaw_rate); - - /** - * @brief Translate trajectorySetpoint to roverSetpoints and publish them - */ - void offboardControl(); - - /** - * @brief Update the controllers + * @brief Update the active controllers. */ void updateControllers(); @@ -206,70 +110,23 @@ private: void reset(); // uORB subscriptions - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; - uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; vehicle_control_mode_s _vehicle_control_mode{}; - // uORB publications - uORB::Publication _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)}; - uORB::Publication _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)}; - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_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 _position_controller_status_pub{ORB_ID(position_controller_status)}; - // Class instances AckermannActControl _ackermann_act_control{this}; AckermannRateControl _ackermann_rate_control{this}; AckermannAttControl _ackermann_att_control{this}; AckermannVelControl _ackermann_vel_control{this}; AckermannPosControl _ackermann_pos_control{this}; + AutoMode _auto_mode{this}; + ManualMode _manual_mode{this}; + OffboardMode _offboard_mode{this}; // Variables - MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates - Quatf _vehicle_attitude_quaternion{}; - float _max_yaw_rate{NAN}; - float _vehicle_yaw{NAN}; - float _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base. int _nav_state{0}; // Navigation state of the vehicle 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 - - // Auto Mode Variables - Vector2f _curr_wp_ned{}; - Vector2f _prev_wp_ned{}; - Vector2f _next_wp_ned{}; - float _acceptance_radius{0.5f}; - float _prev_acceptance_radius{0.5f}; - float _cruising_speed{0.f}; - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; - - // Manual Mode Variables - Vector2f _pos_ctl_course_direction{NAN, NAN}; - Vector2f _pos_ctl_start_position_ned{NAN, NAN}; - Vector2f _curr_pos_ned{NAN, NAN}; - float _stab_yaw_setpoint{NAN}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ro_yaw_p, - (ParamFloat) _param_ro_yaw_stick_dz, - (ParamFloat) _param_pp_lookahd_max, - (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ra_wheel_base, - (ParamFloat) _param_ra_max_str_ang, - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_ra_acc_rad_max, - (ParamFloat) _param_ra_acc_rad_gain - ) };