From de94c3b803cf414e4f92066005a64a63b2db3f78 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 6 May 2025 09:35:42 +0200 Subject: [PATCH 1/3] differential: seperate actuator control --- src/modules/rover_differential/CMakeLists.txt | 2 + .../DifferentialActControl/CMakeLists.txt | 38 ++++++ .../DifferentialActControl.cpp | 125 ++++++++++++++++++ .../DifferentialActControl.hpp | 121 +++++++++++++++++ .../rover_differential/RoverDifferential.cpp | 74 +---------- .../rover_differential/RoverDifferential.hpp | 57 +------- 6 files changed, 292 insertions(+), 125 deletions(-) create mode 100644 src/modules/rover_differential/DifferentialActControl/CMakeLists.txt create mode 100644 src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp create mode 100644 src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index a850e5949cef..ebae1e8723ec 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -31,6 +31,7 @@ # ############################################################################ +add_subdirectory(DifferentialActControl) add_subdirectory(DifferentialRateControl) add_subdirectory(DifferentialAttControl) add_subdirectory(DifferentialVelControl) @@ -43,6 +44,7 @@ px4_add_module( RoverDifferential.cpp RoverDifferential.hpp DEPENDS + DifferentialActControl DifferentialRateControl DifferentialAttControl DifferentialVelControl diff --git a/src/modules/rover_differential/DifferentialActControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialActControl/CMakeLists.txt new file mode 100644 index 000000000000..b3ab8187bd36 --- /dev/null +++ b/src/modules/rover_differential/DifferentialActControl/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(DifferentialActControl + DifferentialActControl.cpp +) + +target_include_directories(DifferentialActControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp new file mode 100644 index 000000000000..dd9c8c99e088 --- /dev/null +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * 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 "DifferentialActControl.hpp" + +using namespace time_literals; + +DifferentialActControl::DifferentialActControl(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); +} + +void DifferentialActControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _adjusted_throttle_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } +} + +void DifferentialActControl::updateActControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // Motor control + if (_rover_throttle_setpoint_sub.updated()) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + _rover_throttle_setpoint_sub.copy(&rover_throttle_setpoint); + _throttle_setpoint = rover_throttle_setpoint.throttle_body_x; + } + + if (_rover_steering_setpoint_sub.updated()) { + rover_steering_setpoint_s rover_steering_setpoint{}; + _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); + _speed_diff_setpoint = rover_steering_setpoint.normalized_speed_diff; + } + + if (PX4_ISFINITE(_throttle_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) { + actuator_motors_s actuator_motors_sub{}; + _actuator_motors_sub.copy(&actuator_motors_sub); + const float current_throttle = (actuator_motors_sub.control[0] + actuator_motors_sub.control[1]) / 2.f; + const float adjusted_throttle_setpoint = RoverControl::throttleControl(_adjusted_throttle_setpoint, + _throttle_setpoint, current_throttle, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(adjusted_throttle_setpoint, _speed_diff_setpoint).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + } + +} + +Vector2f DifferentialActControl::computeInverseKinematics(float throttle, const float speed_diff_normalized) +{ + float max_motor_command = fabsf(throttle) + fabsf(speed_diff_normalized); + + if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 + float excess = fabsf(max_motor_command - 1.0f); + throttle -= sign(throttle) * excess; + } + + // Calculate the left and right wheel speeds + return Vector2f(throttle - speed_diff_normalized, + throttle + speed_diff_normalized); +} + +void DifferentialActControl::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_speed_diff = 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 DifferentialActControl::stopVehicle() +{ + 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.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); +} diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp new file mode 100644 index 000000000000..ff893fbfbbc8 --- /dev/null +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 +#include + +// uORB includes +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for differential actuator control. + */ +class DifferentialActControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialActControl. + * @param parent The parent ModuleParams object. + */ + DifferentialActControl(ModuleParams *parent); + ~DifferentialActControl() = default; + + /** + * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void updateActControl(); + + /** + * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. + */ + void manualManualMode(); + + /** + * @brief Stop the vehicle by sending 0 commands to motors and servos. + */ + void stopVehicle(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Compute normalized motor commands based on normalized setpoints. + * @param throttle Normalized speed in body x direction [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return Motor speeds for the right and left motors [-1, 1]. + */ + Vector2f computeInverseKinematics(float throttle, float speed_diff_normalized); + + // uORB subscriptions + 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)}; + + // Variables + hrt_abstime _timestamp{0}; + float _throttle_setpoint{NAN}; + float _speed_diff_setpoint{NAN}; + + // Controllers + SlewRate _adjusted_throttle_setpoint{0.f}; + + // Parameters + DEFINE_PARAMETERS( + (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_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index 9a2a71646aee..99dd28d55f2b 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -39,8 +39,6 @@ RoverDifferential::RoverDifferential() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - _rover_throttle_setpoint_pub.advertise(); - _rover_steering_setpoint_pub.advertise(); updateParams(); } @@ -53,10 +51,6 @@ bool RoverDifferential::init() void RoverDifferential::updateParams() { ModuleParams::updateParams(); - - if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { - _throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); - } } void RoverDifferential::Run() @@ -65,10 +59,6 @@ void RoverDifferential::Run() updateParams(); } - const hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - _differential_pos_control.updatePosControl(); _differential_vel_control.updateVelControl(); _differential_att_control.updateAttControl(); @@ -83,72 +73,16 @@ void RoverDifferential::Run() && !_vehicle_control_mode.flag_control_rates_enabled; if (full_manual_mode_enabled) { // Manual mode - generateSteeringAndThrottleSetpoint(); + _differential_act_control.manualManualMode(); } if (_vehicle_control_mode.flag_armed) { - generateActuatorSetpoint(); - - } - -} - -void RoverDifferential::generateSteeringAndThrottleSetpoint() -{ - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_steering_setpoint_s rover_steering_setpoint{}; - rover_steering_setpoint.timestamp = _timestamp; - rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.roll; - _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 = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - } -} + _differential_act_control.updateActControl(); -void RoverDifferential::generateActuatorSetpoint() -{ - if (_rover_throttle_setpoint_sub.updated()) { - _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); - } - - if (_actuator_motors_sub.updated()) { - actuator_motors_s actuator_motors{}; - _actuator_motors_sub.copy(&actuator_motors); - _current_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f; - } - - if (_rover_steering_setpoint_sub.updated()) { - _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); - } - - const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint, - _rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(), - _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(throttle_body_x, - _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); -} - -Vector2f RoverDifferential::computeInverseKinematics(float throttle_body_x, const float speed_diff_normalized) -{ - float max_motor_command = fabsf(throttle_body_x) + fabsf(speed_diff_normalized); - - if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 - float excess = fabsf(max_motor_command - 1.0f); - throttle_body_x -= sign(throttle_body_x) * excess; + } else { + _differential_act_control.stopVehicle(); } - // Calculate the left and right wheel speeds - return Vector2f(throttle_body_x - speed_diff_normalized, - throttle_body_x + speed_diff_normalized); } int RoverDifferential::task_spawn(int argc, char *argv[]) diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 5a98ef7d3cd9..73fb3e735de7 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -40,22 +40,13 @@ #include #include -// Libraries -#include -#include - // uORB includes #include -#include -#include #include -#include -#include -#include #include -#include // Local includes +#include "DifferentialActControl/DifferentialActControl.hpp" #include "DifferentialRateControl/DifferentialRateControl.hpp" #include "DifferentialAttControl/DifferentialAttControl.hpp" #include "DifferentialVelControl/DifferentialVelControl.hpp" @@ -91,59 +82,15 @@ class RoverDifferential : public ModuleBase, public ModulePar private: void Run() override; - /** - * @brief Generate and publish roverSteeringSetpoint and roverThrottleSetpoint from manualControlSetpoint (Manual Mode). - */ - void generateSteeringAndThrottleSetpoint(); - - /** - * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. - */ - void generateActuatorSetpoint(); - - /** - * @brief Compute normalized motor commands based on normalized setpoints. - * @param throttle_body_x Normalized speed in body x direction [-1, 1]. - * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. - * @return Motor speeds for the right and left motors [-1, 1]. - */ - Vector2f computeInverseKinematics(float throttle_body_x, float speed_diff_normalized); - // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; - uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; vehicle_control_mode_s _vehicle_control_mode{}; - rover_steering_setpoint_s _rover_steering_setpoint{}; - rover_throttle_setpoint_s _rover_throttle_setpoint{}; - - // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; - uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; // Class instances + DifferentialActControl _differential_act_control{this}; DifferentialRateControl _differential_rate_control{this}; DifferentialAttControl _differential_att_control{this}; DifferentialVelControl _differential_vel_control{this}; DifferentialPosControl _differential_pos_control{this}; - - // Variables - hrt_abstime _timestamp{0}; - float _dt{0.f}; - float _current_throttle_body_x{0.f}; - - // Controllers - SlewRate _throttle_body_x_setpoint{0.f}; - - // Parameters - DEFINE_PARAMETERS( - (ParamInt) _param_r_rev, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_max_thr_speed - ) }; From fb9650456091b0c0ddcd0610150a1bf1d606f415 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 6 May 2025 11:06:22 +0200 Subject: [PATCH 2/3] differential: update position control --- .../DifferentialPosControl.cpp | 234 ++++++++---------- .../DifferentialPosControl.hpp | 28 +-- 2 files changed, 110 insertions(+), 152 deletions(-) diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp index f2f37d0a0a9b..06363c42cb02 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp @@ -37,13 +37,9 @@ using namespace time_literals; DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModuleParams(parent) { - _rover_velocity_setpoint_pub.advertise(); - _rover_position_setpoint_pub.advertise(); _pure_pursuit_status_pub.advertise(); - - // Initially set to NaN to indicate that the rover has no position setpoint - _rover_position_setpoint.position_ned[0] = NAN; - _rover_position_setpoint.position_ned[1] = NAN; + _rover_position_setpoint_pub.advertise(); + _rover_velocity_setpoint_pub.advertise(); updateParams(); } @@ -65,6 +61,13 @@ void DifferentialPosControl::updatePosControl() if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { if (_vehicle_control_mode.flag_control_offboard_enabled) { generatePositionSetpoint(); + + } else if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_control_position_enabled) { + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { + autoPositionMode(); + } generateVelocitySetpoint(); @@ -120,10 +123,13 @@ void DifferentialPosControl::generatePositionSetpoint() // Translate trajectory setpoint to rover position setpoint rover_position_setpoint_s rover_position_setpoint{}; - rover_position_setpoint.timestamp = _timestamp; + 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.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); @@ -131,15 +137,50 @@ void DifferentialPosControl::generatePositionSetpoint() void DifferentialPosControl::generateVelocitySetpoint() { - 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; + } + + 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; - } else if (_vehicle_control_mode.flag_control_auto_enabled) { - autoPositionMode(); + 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_pub.publish(rover_velocity_setpoint); - } else if (_rover_position_setpoint_sub.copy(&_rover_position_setpoint) - && PX4_ISFINITE(_rover_position_setpoint.position_ned[0]) && PX4_ISFINITE(_rover_position_setpoint.position_ned[1])) { - goToPositionMode(); + } 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); } } @@ -156,29 +197,28 @@ void DifferentialPosControl::manualPositionMode() const float bearing_delta = math::interpolate(math::deadzone(manual_control_setpoint.roll, _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -bearing_scaling, bearing_scaling); - if (fabsf(speed_setpoint) < FLT_EPSILON) { // Turn on spot - _course_control = false; - const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = _timestamp; - rover_velocity_setpoint.speed = 0.f; - rover_velocity_setpoint.bearing = bearing_setpoint; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); - - } else if (fabsf(bearing_delta) > FLT_EPSILON) { // Closed loop yaw rate control - _course_control = false; - const float bearing_setpoint = matrix::wrap_pi(_vehicle_yaw + bearing_delta); - rover_velocity_setpoint_s rover_velocity_setpoint{}; - rover_velocity_setpoint.timestamp = _timestamp; - rover_velocity_setpoint.speed = speed_setpoint; - rover_velocity_setpoint.bearing = bearing_setpoint; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + if (fabsf(bearing_delta) > FLT_EPSILON || fabsf(speed_setpoint) < FLT_EPSILON) { + _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 + bearing_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 (!_course_control) { + 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; - _course_control = true; } // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover @@ -186,18 +226,16 @@ void DifferentialPosControl::manualPositionMode() 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; - 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, _pos_ctl_start_position_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_pub.publish(rover_velocity_setpoint); + 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); } } @@ -216,107 +254,41 @@ void DifferentialPosControl::autoPositionMode() // 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(); - } - - // Distances to waypoints - 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)); - - // Check stopping conditions - bool auto_stop{false}; - - if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND - || _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE - || !_next_wp_ned.isAllFinite()) { // Check stopping conditions - auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - if (auto_stop) { - 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); - } else { - const float speed_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), - _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), - _param_rd_miss_spd_gain.get(), _curr_wp_type); - 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(), _curr_wp_ned, _prev_wp_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 = bearing_setpoint; - _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + 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, _waypoint_transition_angle, + _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_rd_miss_spd_gain.get(), _curr_wp_type); + rover_position_setpoint.cruising_speed = _cruising_speed; + rover_position_setpoint.yaw = NAN; + _rover_position_setpoint_pub.publish(rover_position_setpoint); } - } -float DifferentialPosControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp, - const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, - const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type) +float DifferentialPosControl::autoArrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, + const float max_speed, const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type) { // Upcoming stop - if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle) - || _waypoint_transition_angle < M_PI_F - trans_drv_trn || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND - || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { - const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp, 0.f); - return math::min(straight_line_speed, cruising_speed); + if (!PX4_ISFINITE(waypoint_transition_angle) || waypoint_transition_angle < M_PI_F - trans_drv_trn + || 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 (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) { - const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle, + 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); - const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, - distance_to_curr_wp, - max_speed * (1.f - speed_reduction)); - return math::min(straight_line_speed, cruising_speed); + return max_speed * (1.f - speed_reduction); } return cruising_speed; // Fallthrough } -void DifferentialPosControl::goToPositionMode() -{ - const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]); - const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); - - if (distance_to_target > _param_nav_acc_rad.get()) { - float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance_to_target, 0.f); - const float max_speed = PX4_ISFINITE(_rover_position_setpoint.cruising_speed) ? - _rover_position_setpoint.cruising_speed : - _param_ro_speed_limit.get(); - speed_setpoint = math::min(speed_setpoint, max_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, _curr_pos_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 = bearing_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); - } -} - bool DifferentialPosControl::runSanityChecks() { bool ret = true; diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp index 4cf34ac7f3e7..88ca5bb500f4 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp @@ -50,7 +50,6 @@ #include #include #include -#include #include #include #include @@ -98,35 +97,24 @@ class DifferentialPosControl : public ModuleParams void generatePositionSetpoint(); /** - * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint (Position Mode) or - * positionSetpointTriplet (Auto Mode) or roverPositionSetpoint. + * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint. */ void generateVelocitySetpoint(); /** - * @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint. + * @brief Generate and publish roverPositionSetpoint from manualControlSetpoint. */ void manualPositionMode(); /** - * @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet. + * @brief Generate and publish roverPositionSetpoint from positionSetpointTriplet. */ void autoPositionMode(); /** - * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint. - */ - void goToPositionMode(); - - /** - * @brief Calculate the speed setpoint. During waypoint transition the speed is restricted to + * @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). - * On straight lines it is based on a speed trajectory such that the rover will arrive at the next waypoint transition - * with the desired waypoiny transition speed under consideration of the maximum deceleration and jerk. * @param cruising_speed Cruising speed [m/s]. - * @param distance_to_curr_wp Distance to the current waypoint [m]. - * @param max_decel Maximum allowed deceleration [m/s^2]. - * @param max_jerk Maximum allowed jerk [m/s^3]. * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] * @param max_speed Maximum speed setpoint [m/s] * @param trans_drv_trn Heading error threshold to switch from driving to turning [rad]. @@ -134,9 +122,8 @@ class DifferentialPosControl : public ModuleParams * @param curr_wp_type Type of the current waypoint. * @return Speed setpoint [m/s]. */ - float calcSpeedSetpoint(float cruising_speed, float distance_to_curr_wp, float max_decel, float max_jerk, - float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain, int curr_wp_type); - + float autoArrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, const float max_speed, + const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type); /** * @brief Check if the necessary parameters are set. @@ -160,7 +147,6 @@ class DifferentialPosControl : public ModuleParams // uORB publications - uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; 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)}; @@ -169,6 +155,7 @@ class DifferentialPosControl : public ModuleParams hrt_abstime _timestamp{0}; Quatf _vehicle_attitude_quaternion{}; Vector2f _curr_pos_ned{}; + Vector2f _start_ned{}; Vector2f _pos_ctl_course_direction{}; Vector2f _pos_ctl_start_position_ned{}; float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards @@ -176,7 +163,6 @@ class DifferentialPosControl : public ModuleParams float _max_yaw_rate{0.f}; float _dt{0.f}; int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; - bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. bool _prev_param_check_passed{true}; // Waypoint variables From 12d2f690d0c239e42136081b5f9de5cf2557fef2 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Tue, 6 May 2025 13:44:49 +0200 Subject: [PATCH 3/3] differential: centralize mode management, resets and checks --- src/modules/rover_differential/CMakeLists.txt | 4 + .../DifferentialActControl.cpp | 15 - .../DifferentialActControl.hpp | 10 +- .../DifferentialAttControl.cpp | 132 ++------- .../DifferentialAttControl.hpp | 58 +--- .../DifferentialDriveModes/CMakeLists.txt | 36 +++ .../DifferentialAutoMode/CMakeLists.txt | 38 +++ .../DifferentialAutoMode.cpp | 113 ++++++++ .../DifferentialAutoMode.hpp | 101 +++++++ .../DifferentialManualMode/CMakeLists.txt | 38 +++ .../DifferentialManualMode.cpp | 219 +++++++++++++++ .../DifferentialManualMode.hpp | 130 +++++++++ .../DifferentialOffboardMode/CMakeLists.txt | 38 +++ .../DifferentialOffboardMode.cpp | 92 ++++++ .../DifferentialOffboardMode.hpp | 89 ++++++ .../DifferentialPosControl.cpp | 265 +++--------------- .../DifferentialPosControl.hpp | 91 +----- .../DifferentialRateControl.cpp | 129 ++------- .../DifferentialRateControl.hpp | 52 +--- .../DifferentialVelControl.cpp | 122 +++----- .../DifferentialVelControl.hpp | 51 ++-- .../rover_differential/RoverDifferential.cpp | 125 ++++++++- .../rover_differential/RoverDifferential.hpp | 47 +++- 23 files changed, 1243 insertions(+), 752 deletions(-) create mode 100644 src/modules/rover_differential/DifferentialDriveModes/CMakeLists.txt create mode 100644 src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/CMakeLists.txt create mode 100644 src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp create mode 100644 src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp create mode 100644 src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/CMakeLists.txt create mode 100644 src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp create mode 100644 src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp create mode 100644 src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/CMakeLists.txt create mode 100644 src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp create mode 100644 src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index ebae1e8723ec..05aef49cfedd 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -36,6 +36,7 @@ add_subdirectory(DifferentialRateControl) add_subdirectory(DifferentialAttControl) add_subdirectory(DifferentialVelControl) add_subdirectory(DifferentialPosControl) +add_subdirectory(DifferentialDriveModes) px4_add_module( MODULE modules__rover_differential @@ -49,6 +50,9 @@ px4_add_module( DifferentialAttControl DifferentialVelControl DifferentialPosControl + DifferentialAutoMode + DifferentialManualMode + DifferentialOffboardMode px4_work_queue rover_control pure_pursuit diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp index dd9c8c99e088..c9b67b0b6cd2 100644 --- a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp @@ -99,21 +99,6 @@ Vector2f DifferentialActControl::computeInverseKinematics(float throttle, const throttle + speed_diff_normalized); } -void DifferentialActControl::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_speed_diff = 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 DifferentialActControl::stopVehicle() { actuator_motors_s actuator_motors{}; diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp index ff893fbfbbc8..2076542e4a98 100644 --- a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp @@ -67,11 +67,6 @@ class DifferentialActControl : public ModuleParams */ void updateActControl(); - /** - * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint. - */ - void manualManualMode(); - /** * @brief Stop the vehicle by sending 0 commands to motors and servos. */ @@ -96,12 +91,9 @@ class DifferentialActControl : public ModuleParams 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}; diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp index 5bc9ab380aab..99217341d793 100644 --- a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp @@ -38,8 +38,6 @@ using namespace time_literals; DifferentialAttControl::DifferentialAttControl(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 DifferentialAttControl::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 DifferentialAttControl::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 DifferentialAttControl::updateAttControl() _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); } - if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { - - if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { - generateAttitudeAndThrottleSetpoint(); - } + 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; + } - generateRateSetpoint(); + 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); - } 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,96 +97,6 @@ void DifferentialAttControl::updateAttControl() } -void DifferentialAttControl::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 = 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_ctl = false; - 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 (!_stab_yaw_ctl) { - _stab_yaw_setpoint = _vehicle_yaw; - _stab_yaw_ctl = true; - } - - 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 && !_offboard_control_mode.position - && !_offboard_control_mode.velocity; - - 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 DifferentialAttControl::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 DifferentialAttControl::runSanityChecks() { bool ret = true; @@ -197,13 +107,9 @@ bool DifferentialAttControl::runSanityChecks() if (_param_ro_yaw_p.get() < FLT_EPSILON) { ret = false; - - if (_prev_param_check_passed) { - events::send(events::ID("differential_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("differential_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_differential/DifferentialAttControl/DifferentialAttControl.hpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp index 82087f82789d..205d1d3d0e23 100644 --- a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp @@ -48,15 +48,9 @@ #include #include #include -#include -#include -#include +#include #include #include -#include -#include -#include -#include /** * @brief Class for differential attitude control. @@ -72,27 +66,14 @@ class DifferentialAttControl : public ModuleParams ~DifferentialAttControl() = default; /** - * @brief Update attitude controller. + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. */ void updateAttControl(); -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: /** - * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode) - * or trajectorySetpoint (Offboard attitude control). + * @brief Reset attitude controller. */ - void generateAttitudeAndThrottleSetpoint(); - - /** - * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. - */ - void generateRateSetpoint(); + void reset() {_pid_yaw.resetIntegral(); _yaw_setpoint = NAN;}; /** * @brief Check if the necessary parameters are set. @@ -100,35 +81,26 @@ class DifferentialAttControl : public ModuleParams */ bool runSanityChecks(); +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: // 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_attitude_status_pub{ORB_ID(rover_attitude_status)}; + 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{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode - bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode - bool _prev_param_check_passed{true}; + float _yaw_setpoint{NAN}; // Controllers PID _pid_yaw; diff --git a/src/modules/rover_differential/DifferentialDriveModes/CMakeLists.txt b/src/modules/rover_differential/DifferentialDriveModes/CMakeLists.txt new file mode 100644 index 000000000000..66fcb084b092 --- /dev/null +++ b/src/modules/rover_differential/DifferentialDriveModes/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(DifferentialAutoMode) +add_subdirectory(DifferentialManualMode) +add_subdirectory(DifferentialOffboardMode) diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/CMakeLists.txt b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/CMakeLists.txt new file mode 100644 index 000000000000..cce9baecc34a --- /dev/null +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/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(DifferentialAutoMode +DifferentialAutoMode.cpp +) + +target_include_directories(DifferentialAutoMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp new file mode 100644 index 000000000000..bf3e4bf98b4e --- /dev/null +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 "DifferentialAutoMode.hpp" + +using namespace time_literals; + +DifferentialAutoMode::DifferentialAutoMode(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_position_setpoint_pub.advertise(); +} + +void DifferentialAutoMode::updateParams() +{ + ModuleParams::updateParams(); +} + +void DifferentialAutoMode::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_rd_trans_drv_trn.get(), _param_rd_miss_spd_gain.get(), curr_wp_type); + rover_position_setpoint.cruising_speed = cruising_speed; + rover_position_setpoint.yaw = NAN; + _rover_position_setpoint_pub.publish(rover_position_setpoint); + } +} + +float DifferentialAutoMode::arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, + const float max_speed, const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type) +{ + // Upcoming stop + if (!PX4_ISFINITE(waypoint_transition_angle) || waypoint_transition_angle < M_PI_F - trans_drv_trn + || 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_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp new file mode 100644 index 000000000000..72e5259cb2f3 --- /dev/null +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 differential auto mode. + */ +class DifferentialAutoMode : public ModuleParams +{ +public: + /** + * @brief Constructor for auto mode. + * @param parent The parent ModuleParams object. + */ + DifferentialAutoMode(ModuleParams *parent); + ~DifferentialAutoMode() = 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 trans_drv_trn Heading error threshold to switch from driving to turning [rad]. + * @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 trans_drv_trn, 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_rd_trans_drv_trn, + (ParamFloat) _param_rd_miss_spd_gain + ) +}; diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/CMakeLists.txt b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/CMakeLists.txt new file mode 100644 index 000000000000..b5323230ff4c --- /dev/null +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/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(DifferentialManualMode + DifferentialManualMode.cpp +) + +target_include_directories(DifferentialManualMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp new file mode 100644 index 000000000000..99f8f8e2d092 --- /dev/null +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * 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 "DifferentialManualMode.hpp" + +using namespace time_literals; + +DifferentialManualMode::DifferentialManualMode(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 DifferentialManualMode::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; +} + +void DifferentialManualMode::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.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 DifferentialManualMode::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 = 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 DifferentialManualMode::stab() +{ + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf 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); + + if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_x) < 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.roll, + _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 DifferentialManualMode::position() +{ + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf 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); + + const float speed_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + + if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON + || fabsf(speed_setpoint) < FLT_EPSILON) { + _pos_ctl_course_direction = Vector2f(NAN, NAN); + + // Speed control + rover_velocity_setpoint_s rover_velocity_setpoint{}; + rover_velocity_setpoint.timestamp = hrt_absolute_time(); + rover_velocity_setpoint.speed = speed_setpoint; + rover_velocity_setpoint.bearing = NAN; + rover_velocity_setpoint.yaw = NAN; + _rover_velocity_setpoint_pub.publish(rover_velocity_setpoint); + + // 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.roll, + _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 + 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 DifferentialManualMode::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_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp new file mode 100644 index 000000000000..bce5d36ea537 --- /dev/null +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * 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 differential manual mode. + */ +class DifferentialManualMode : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialManualMode. + * @param parent The parent ModuleParams object. + */ + DifferentialManualMode(ModuleParams *parent); + ~DifferentialManualMode() = 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}; + 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 + ) +}; diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/CMakeLists.txt b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/CMakeLists.txt new file mode 100644 index 000000000000..8e782323bdb5 --- /dev/null +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/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(DifferentialOffboardMode + DifferentialOffboardMode.cpp +) + +target_include_directories(DifferentialOffboardMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp new file mode 100644 index 000000000000..eca17e18ff15 --- /dev/null +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.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 "DifferentialOffboardMode.hpp" + +using namespace time_literals; + +DifferentialOffboardMode::DifferentialOffboardMode(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 DifferentialOffboardMode::updateParams() +{ + ModuleParams::updateParams(); +} + +void DifferentialOffboardMode::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_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp new file mode 100644 index 000000000000..ebdeef0112d8 --- /dev/null +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.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 differential manual mode. + */ +class DifferentialOffboardMode : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialOffboardMode. + * @param parent The parent ModuleParams object. + */ + DifferentialOffboardMode(ModuleParams *parent); + ~DifferentialOffboardMode() = 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_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp index 06363c42cb02..52a5a00eeb7b 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp @@ -38,7 +38,6 @@ using namespace time_literals; DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModuleParams(parent) { _pure_pursuit_status_pub.advertise(); - _rover_position_setpoint_pub.advertise(); _rover_velocity_setpoint_pub.advertise(); updateParams(); @@ -47,260 +46,88 @@ DifferentialPosControl::DifferentialPosControl(ModuleParams *parent) : ModulePar void DifferentialPosControl::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; } void DifferentialPosControl::updatePosControl() { - const hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - updateSubscriptions(); - if (_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { - if (_vehicle_control_mode.flag_control_offboard_enabled) { - generatePositionSetpoint(); + 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; + } - } 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 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_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 DifferentialPosControl::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); - _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + matrix::Quatf 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); - Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); - Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); - _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; - } - -} - -void DifferentialPosControl::generatePositionSetpoint() -{ - 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.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); - -} - -void DifferentialPosControl::generateVelocitySetpoint() -{ - 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; - } - - 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_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); } } -void DifferentialPosControl::manualPositionMode() -{ - 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 bearing_scaling = math::min(_max_yaw_rate / _param_ro_yaw_p.get(), - _param_rd_trans_drv_trn.get() - FLT_EPSILON); - const float bearing_delta = math::interpolate(math::deadzone(manual_control_setpoint.roll, - _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -bearing_scaling, bearing_scaling); - - if (fabsf(bearing_delta) > FLT_EPSILON || fabsf(speed_setpoint) < FLT_EPSILON) { - _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 + bearing_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 DifferentialPosControl::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 - _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 = autoArrivalSpeed(_cruising_speed, _waypoint_transition_angle, - _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_rd_miss_spd_gain.get(), _curr_wp_type); - rover_position_setpoint.cruising_speed = _cruising_speed; - rover_position_setpoint.yaw = NAN; - _rover_position_setpoint_pub.publish(rover_position_setpoint); - } -} - -float DifferentialPosControl::autoArrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, - const float max_speed, const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type) -{ - // Upcoming stop - if (!PX4_ISFINITE(waypoint_transition_angle) || waypoint_transition_angle < M_PI_F - trans_drv_trn - || 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 - -} - bool DifferentialPosControl::runSanityChecks() { bool ret = true; - if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { - ret = false; - } - if (_param_ro_speed_limit.get() < FLT_EPSILON) { ret = false; } - _prev_param_check_passed = ret; return ret; } diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp index 88ca5bb500f4..52c5253ef77e 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp @@ -41,7 +41,6 @@ #include #include #include -#include #include // uORB includes @@ -50,13 +49,7 @@ #include #include #include -#include -#include -#include #include -#include -#include -#include #include using namespace matrix; @@ -75,10 +68,16 @@ class DifferentialPosControl : public ModuleParams ~DifferentialPosControl() = 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. @@ -91,104 +90,28 @@ class DifferentialPosControl : public ModuleParams */ void updateSubscriptions(); - /** - * @brief Generate and publish roverPositionSetpoint from position of trajectorySetpoint. - */ - void generatePositionSetpoint(); - - /** - * @brief Generate and publish roverVelocitySetpoint from roverPositionSetpoint. - */ - void generateVelocitySetpoint(); - - /** - * @brief Generate and publish roverPositionSetpoint from manualControlSetpoint. - */ - void manualPositionMode(); - - /** - * @brief Generate and publish roverPositionSetpoint 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 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 trans_drv_trn Heading error threshold to switch from driving to turning [rad]. - * @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 autoArrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, const float max_speed, - const float trans_drv_trn, const 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_velocity_setpoint_sub{ORB_ID(rover_velocity_setpoint)}; 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)}; // Variables - hrt_abstime _timestamp{0}; - Quatf _vehicle_attitude_quaternion{}; Vector2f _curr_pos_ned{}; Vector2f _start_ned{}; - Vector2f _pos_ctl_course_direction{}; - Vector2f _pos_ctl_start_position_ned{}; - float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards float _vehicle_yaw{0.f}; - float _max_yaw_rate{0.f}; - float _dt{0.f}; - 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 DEFINE_PARAMETERS( - (ParamFloat) _param_rd_trans_drv_trn, - (ParamFloat) _param_rd_miss_spd_gain, - (ParamFloat) _param_ro_max_thr_speed, - (ParamFloat) _param_ro_yaw_stick_dz, (ParamFloat) _param_ro_decel_limit, (ParamFloat) _param_ro_jerk_limit, (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ro_speed_th, - (ParamFloat) _param_ro_yaw_p, (ParamFloat) _param_pp_lookahd_gain, (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_pp_lookahd_min, - (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_nav_acc_rad ) }; diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp index 8b8e7005a10f..045d99215796 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -37,8 +37,6 @@ using namespace time_literals; DifferentialRateControl::DifferentialRateControl(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 @@ DifferentialRateControl::DifferentialRateControl(ModuleParams *parent) : ModuleP void DifferentialRateControl::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 DifferentialRateControl::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 DifferentialRateControl::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(); - } - - generateSteeringSetpoint(); + 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; + } - } else { // Reset controller and slew rate when rate control is not active + 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_rd_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_rd_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 { _pid_yaw_rate.resetIntegral(); - _adjusted_yaw_rate_setpoint.setForcedValue(0.f); } // Publish rate controller status (logging only) @@ -95,96 +99,17 @@ void DifferentialRateControl::updateRateControl() } -void DifferentialRateControl::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 = 0.f; - _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.roll, -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.position - && !_offboard_control_mode.velocity && !_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 DifferentialRateControl::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_rd_max_thr_yaw_r.get(), _max_yaw_accel, - _max_yaw_decel, _param_rd_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 DifferentialRateControl::runSanityChecks() { bool ret = true; - if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { - ret = false; - - if (_prev_param_check_passed) { - events::send(events::ID("differential_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_rd_wheel_track.get() < FLT_EPSILON || _param_rd_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("differential_rate_control_conf_invalid_rate_control"), events::Log::Error, - "Invalid configuration for rate control: Neither feed forward nor feedback is setup", _param_rd_wheel_track.get(), - _param_rd_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); - } + events::send(events::ID("differential_rate_control_conf_invalid_rate_control"), events::Log::Error, + "Invalid configuration for rate control: Neither feed forward nor feedback is setup", _param_rd_wheel_track.get(), + _param_rd_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); } - _prev_param_check_passed = ret; return ret; } diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp index da9b682f15c1..a58908fc9eee 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -47,15 +47,9 @@ #include #include #include -#include -#include -#include -#include #include #include -#include -#include -#include +#include /** * @brief Class for differential rate control. @@ -71,60 +65,41 @@ class DifferentialRateControl : public ModuleParams ~DifferentialRateControl() = default; /** - * @brief Update rate controller. + * @brief Generate and publish roverSteeringSetpoint from roverRateSetpoint. */ void updateRateControl(); -protected: /** - * @brief Update the parameters of the module. + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. */ - void updateParams() override; - -private: + bool runSanityChecks(); /** - * @brief Generate and publish roverRateSetpoint and roverThrottleSetpoint from manualControlSetpoint (Acro Mode). + * @brief Reset rate controller. */ - void generateRateAndThrottleSetpoint(); + void reset() {_pid_yaw_rate.resetIntegral(); _yaw_rate_setpoint = NAN;}; +protected: /** - * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. + * @brief Update the parameters of the module. */ - void generateSteeringSetpoint(); + void updateParams() override; - /** - * @brief Check if the necessary parameters are set. - * @return True if all checks pass. - */ - bool runSanityChecks(); +private: // 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}; + hrt_abstime _timestamp{0}; // Controllers PID _pid_yaw_rate; @@ -133,7 +108,6 @@ class DifferentialRateControl : public ModuleParams DEFINE_PARAMETERS( (ParamFloat) _param_rd_wheel_track, (ParamFloat) _param_rd_max_thr_yaw_r, - (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_ro_yaw_rate_th, (ParamFloat) _param_ro_yaw_rate_p, (ParamFloat) _param_ro_yaw_rate_i, diff --git a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp index 2f8e3c88852a..83d9542865d5 100644 --- a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp +++ b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp @@ -39,7 +39,6 @@ DifferentialVelControl::DifferentialVelControl(ModuleParams *parent) : ModulePar { _rover_throttle_setpoint_pub.advertise(); _rover_attitude_setpoint_pub.advertise(); - _rover_velocity_setpoint_pub.advertise(); _rover_velocity_status_pub.advertise(); updateParams(); } @@ -47,12 +46,15 @@ DifferentialVelControl::DifferentialVelControl(ModuleParams *parent) : ModulePar void DifferentialVelControl::updateParams() { ModuleParams::updateParams(); + + // Set up PID controller _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); _pid_speed.setIntegralLimit(1.f); _pid_speed.setOutputLimit(1.f); + // Set up slew rate if (_param_ro_accel_limit.get() > FLT_EPSILON) { - _speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); + _adjusted_speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); } } @@ -60,39 +62,45 @@ void DifferentialVelControl::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(_bearing_setpoint)) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _bearing_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } - generateAttitudeAndThrottleSetpoint(); + // Throttle Setpoint + if (PX4_ISFINITE(_speed_setpoint)) { + const float speed_setpoint = calcSpeedSetpoint(); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed, + speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), dt); + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - } else { // Reset controller and slew rate when velocity control is not active - _pid_speed.resetIntegral(); - _speed_setpoint.setForcedValue(0.f); } // Publish velocity controller status (logging only) rover_velocity_status_s rover_velocity_status; rover_velocity_status.timestamp = _timestamp; rover_velocity_status.measured_speed_body_x = _vehicle_speed; - rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState(); + rover_velocity_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState(); rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); rover_velocity_status.measured_speed_body_y = NAN; rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; rover_velocity_status.pid_throttle_body_y_integral = NAN; _rover_velocity_status_pub.publish(rover_velocity_status); } + void DifferentialVelControl::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); @@ -109,46 +117,18 @@ void DifferentialVelControl::updateSubscriptions() _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } -} - -void DifferentialVelControl::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); + if (_rover_velocity_setpoint_sub.updated()) { + rover_velocity_setpoint_s rover_velocity_setpoint; + _rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint); + _speed_setpoint = rover_velocity_setpoint.speed; + _bearing_setpoint = rover_velocity_setpoint.bearing; } - 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_pub.publish(rover_velocity_setpoint); - } } -void DifferentialVelControl::generateAttitudeAndThrottleSetpoint() +float DifferentialVelControl::calcSpeedSetpoint() { - if (_rover_velocity_setpoint_sub.updated()) { - _rover_velocity_setpoint_sub.copy(&_rover_velocity_setpoint); - } - - // Attitude Setpoint - if (PX4_ISFINITE(_rover_velocity_setpoint.bearing)) { - rover_attitude_setpoint_s rover_attitude_setpoint{}; - rover_attitude_setpoint.timestamp = _timestamp; - rover_attitude_setpoint.yaw_setpoint = _rover_velocity_setpoint.bearing; - _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); - } - - // Throttle Setpoint - const float heading_error = matrix::wrap_pi(_rover_velocity_setpoint.bearing - _vehicle_yaw); + const float heading_error = matrix::wrap_pi(_bearing_setpoint - _vehicle_yaw); if (_current_state == DrivingState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { _current_state = DrivingState::SPOT_TURNING; @@ -160,32 +140,27 @@ void DifferentialVelControl::generateAttitudeAndThrottleSetpoint() float speed_setpoint = 0.f; if (_current_state == DrivingState::DRIVING) { - speed_setpoint = math::constrain(_rover_velocity_setpoint.speed, -_param_ro_speed_limit.get(), + speed_setpoint = math::constrain(_speed_setpoint, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); const float speed_setpoint_normalized = math::interpolate(speed_setpoint, -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); if (_rover_steering_setpoint_sub.updated()) { - _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + rover_steering_setpoint_s rover_steering_setpoint{}; + _rover_steering_setpoint_sub.copy(&rover_steering_setpoint); + _normalized_speed_diff = rover_steering_setpoint.normalized_speed_diff; } if (fabsf(speed_setpoint_normalized) > 1.f - fabsf( - _rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels - speed_setpoint = math::interpolate(sign(speed_setpoint_normalized) * (1.f - fabsf( - _rover_steering_setpoint.normalized_speed_diff)), -1.f, 1.f, + _normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels + speed_setpoint = math::interpolate(sign(speed_setpoint_normalized) * (1.f - fabsf(_normalized_speed_diff)), -1.f, + 1.f, - _param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); } } - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; - rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, - speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), _dt); - rover_throttle_setpoint.throttle_body_y = 0.f; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - + return speed_setpoint; } bool DifferentialVelControl::runSanityChecks() @@ -194,25 +169,16 @@ bool DifferentialVelControl::runSanityChecks() if (_param_ro_speed_limit.get() < FLT_EPSILON) { ret = false; - - if (_prev_param_check_passed) { - events::send(events::ID("differential_posVel_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("differential_posVel_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("differential_posVel_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("differential_posVel_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_differential/DifferentialVelControl/DifferentialVelControl.hpp b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp index c962382653ce..6a7a3c968931 100644 --- a/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp +++ b/src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp @@ -47,15 +47,12 @@ // uORB includes #include #include -#include #include #include #include #include -#include -#include +#include #include -#include #include using namespace matrix; @@ -82,71 +79,63 @@ class DifferentialVelControl : public ModuleParams ~DifferentialVelControl() = default; /** - * @brief Update velocity controller. + * @brief Generate and publish roverAttitudeSetpoint/RoverThrottleSetpoint from roverVelocitySetpoint. */ void updateVelControl(); -protected: /** - * @brief Update the parameters of the module. + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. */ - void updateParams() override; + bool runSanityChecks(); -private: /** - * @brief Update uORB subscriptions used in velocity controller. + * @brief Reset velocity controller. */ - void updateSubscriptions(); + void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _bearing_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);}; +protected: /** - * @brief Generate and publish roverVelocitySetpoint from velocity of trajectorySetpoint. + * @brief Update the parameters of the module. */ - void generateVelocitySetpoint(); + void updateParams() override; +private: /** - * @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint - * from roverVelocitySetpoint. + * @brief Update uORB subscriptions used in velocity controller. */ - void generateAttitudeAndThrottleSetpoint(); + void updateSubscriptions(); /** - * @brief Check if the necessary parameters are set. - * @return True if all checks pass. + * @brief Calculate the speed setpoint based on the current state. + * @return Speed setpoint. */ - bool runSanityChecks(); + float 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_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{}; // Variables hrt_abstime _timestamp{0}; Quatf _vehicle_attitude_quaternion{}; float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards float _vehicle_yaw{0.f}; - float _dt{0.f}; - bool _prev_param_check_passed{false}; + float _speed_setpoint{NAN}; + float _bearing_setpoint{NAN}; + float _normalized_speed_diff{NAN}; DrivingState _current_state{DrivingState::DRIVING}; - // Controllers PID _pid_speed; - SlewRate _speed_setpoint; + SlewRate _adjusted_speed_setpoint; DEFINE_PARAMETERS( (ParamFloat) _param_rd_trans_trn_drv, diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index 99dd28d55f2b..4ab2d471873a 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -56,33 +56,130 @@ void RoverDifferential::updateParams() void RoverDifferential::Run() { if (_parameter_update_sub.updated()) { + parameter_update_s param_update{}; + _parameter_update_sub.copy(¶m_update); updateParams(); + runSanityChecks(); } - _differential_pos_control.updatePosControl(); - _differential_vel_control.updateVelControl(); - _differential_att_control.updateAttControl(); - _differential_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; + } + + } + + if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) { + + _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(); + _differential_act_control.stopVehicle(); + _was_armed = false; + } + +} + +void RoverDifferential::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 RoverDifferential::updateControllers() +{ + if (_vehicle_control_mode.flag_control_position_enabled) { + _differential_pos_control.updatePosControl(); } - 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_control_velocity_enabled) { + _differential_vel_control.updateVelControl(); + } - if (full_manual_mode_enabled) { // Manual mode - _differential_act_control.manualManualMode(); + if (_vehicle_control_mode.flag_control_attitude_enabled) { + _differential_att_control.updateAttControl(); } - if (_vehicle_control_mode.flag_armed) { + if (_vehicle_control_mode.flag_control_rates_enabled) { + _differential_rate_control.updateRateControl(); + } + + if (_vehicle_control_mode.flag_control_allocation_enabled) { _differential_act_control.updateActControl(); + } +} - } else { - _differential_act_control.stopVehicle(); +void RoverDifferential::runSanityChecks() +{ + if (_vehicle_control_mode.flag_control_rates_enabled && !_differential_rate_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && !_differential_att_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; + } + + if (_vehicle_control_mode.flag_control_velocity_enabled && !_differential_vel_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; } + if (_vehicle_control_mode.flag_control_position_enabled && !_differential_pos_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; + } + + _sanity_checks_passed = true; +} + +void RoverDifferential::reset() +{ + _differential_vel_control.reset(); + _differential_att_control.reset(); + _differential_rate_control.reset(); + _manual_mode.reset(); } int RoverDifferential::task_spawn(int argc, char *argv[]) diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 73fb3e735de7..39ac21d2a83f 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -40,6 +40,9 @@ #include #include +// Library includes +#include + // uORB includes #include #include @@ -51,6 +54,9 @@ #include "DifferentialAttControl/DifferentialAttControl.hpp" #include "DifferentialVelControl/DifferentialVelControl.hpp" #include "DifferentialPosControl/DifferentialPosControl.hpp" +#include "DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp" +#include "DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp" +#include "DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp" class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -82,15 +88,46 @@ class RoverDifferential : public ModuleBase, public ModulePar 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{}; // Class instances - DifferentialActControl _differential_act_control{this}; - DifferentialRateControl _differential_rate_control{this}; - DifferentialAttControl _differential_att_control{this}; - DifferentialVelControl _differential_vel_control{this}; - DifferentialPosControl _differential_pos_control{this}; + DifferentialActControl _differential_act_control{this}; + DifferentialRateControl _differential_rate_control{this}; + DifferentialAttControl _differential_att_control{this}; + DifferentialVelControl _differential_vel_control{this}; + DifferentialPosControl _differential_pos_control{this}; + DifferentialAutoMode _auto_mode{this}; + DifferentialManualMode _manual_mode{this}; + DifferentialOffboardMode _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 };