diff --git a/src/modules/rover_mecanum/CMakeLists.txt b/src/modules/rover_mecanum/CMakeLists.txt index 998a2c3808..38dd9a2b63 100644 --- a/src/modules/rover_mecanum/CMakeLists.txt +++ b/src/modules/rover_mecanum/CMakeLists.txt @@ -31,6 +31,7 @@ # ############################################################################ +add_subdirectory(MecanumActControl) add_subdirectory(MecanumRateControl) add_subdirectory(MecanumAttControl) add_subdirectory(MecanumVelControl) @@ -43,6 +44,7 @@ px4_add_module( RoverMecanum.cpp RoverMecanum.hpp DEPENDS + MecanumActControl MecanumRateControl MecanumAttControl MecanumVelControl diff --git a/src/modules/rover_mecanum/MecanumActControl/CMakeLists.txt b/src/modules/rover_mecanum/MecanumActControl/CMakeLists.txt new file mode 100644 index 0000000000..2b40df09e6 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumActControl/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(MecanumActControl + MecanumActControl.cpp +) + +target_include_directories(MecanumActControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp new file mode 100644 index 0000000000..f01a8b6c57 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * 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 "MecanumActControl.hpp" + +using namespace time_literals; + +MecanumActControl::MecanumActControl(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); +} + +void MecanumActControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _adjusted_throttle_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + _adjusted_throttle_y_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } +} + +void MecanumActControl::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_x_setpoint = rover_throttle_setpoint.throttle_body_x; + _throttle_y_setpoint = rover_throttle_setpoint.throttle_body_y; + } + + 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_x_setpoint) && PX4_ISFINITE(_throttle_y_setpoint) && PX4_ISFINITE(_speed_diff_setpoint)) { + actuator_motors_s actuator_motors_sub{}; + _actuator_motors_sub.copy(&actuator_motors_sub); + const float current_throttle_x = (actuator_motors_sub.control[0] + actuator_motors_sub.control[1]) / 2.f; + const float current_throttle_y = (actuator_motors_sub.control[2] - actuator_motors_sub.control[0]) / 2.f; + const float adjusted_throttle_x_setpoint = RoverControl::throttleControl(_adjusted_throttle_x_setpoint, + _throttle_x_setpoint, current_throttle_x, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); + const float adjusted_throttle_y_setpoint = RoverControl::throttleControl(_adjusted_throttle_y_setpoint, + _throttle_y_setpoint, current_throttle_y, _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_x_setpoint, adjusted_throttle_y_setpoint, + _speed_diff_setpoint).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + } else { + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = 0.f; + actuator_motors.control[1] = 0.f; + actuator_motors.control[2] = 0.f; + actuator_motors.control[3] = 0.f; + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + } + +} + +Vector4f MecanumActControl::computeInverseKinematics(float throttle_body_x, float throttle_body_y, + const float speed_diff_normalized) +{ + const float total_speed = fabsf(throttle_body_x) + fabsf(throttle_body_y) + fabsf(speed_diff_normalized); + + if (total_speed > 1.f) { // Adjust speed setpoints if infeasible + const float theta = atan2f(fabsf(throttle_body_y), fabsf(throttle_body_x)); + const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(throttle_body_x, 2.f) + powf(throttle_body_y, 2.f))); + throttle_body_x *= magnitude * normalization; + throttle_body_y *= magnitude * normalization; + + } + + // Calculate motor commands + const float input_data[3] = {throttle_body_x, throttle_body_y, speed_diff_normalized}; + const Matrix input(input_data); + const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f}; + const Matrix m(m_data); + const Vector4f motor_commands = m * input; + + return motor_commands; +} + +void MecanumActControl::manualManualMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.yaw; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); +} + +void MecanumActControl::stopVehicle() +{ + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = 0.f; + actuator_motors.control[1] = 0.f; + actuator_motors.control[2] = 0.f; + actuator_motors.control[3] = 0.f; + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); +} diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp new file mode 100644 index 0000000000..40f6dfbd58 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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 mecanum actuator control. + */ +class MecanumActControl : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumActControl. + * @param parent The parent ModuleParams object. + */ + MecanumActControl(ModuleParams *parent); + ~MecanumActControl() = 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_body_x Normalized speed in body x direction [-1, 1]. + * @param throttle_body_y Normalized speed in body y direction [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return Motor speeds [-1, 1]. + */ + Vector4f computeInverseKinematics(float throttle_body_x, float throttle_body_y, const 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_x_setpoint{NAN}; + float _throttle_y_setpoint{NAN}; + float _speed_diff_setpoint{NAN}; + + // Controllers + SlewRate _adjusted_throttle_x_setpoint{0.f}; + SlewRate _adjusted_throttle_y_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_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index 724246770d..c1e4b0cfa9 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -39,8 +39,6 @@ RoverMecanum::RoverMecanum() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - _rover_throttle_setpoint_pub.advertise(); - _rover_steering_setpoint_pub.advertise(); updateParams(); } @@ -53,11 +51,6 @@ bool RoverMecanum::init() void RoverMecanum::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()); - _throttle_body_y_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); - } } void RoverMecanum::Run() @@ -66,10 +59,6 @@ void RoverMecanum::Run() updateParams(); } - const hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - _mecanum_pos_control.updatePosControl(); _mecanum_vel_control.updateVelControl(); _mecanum_att_control.updateAttControl(); @@ -84,90 +73,16 @@ void RoverMecanum::Run() && !_vehicle_control_mode.flag_control_rates_enabled; if (full_manual_mode_enabled) { // Manual mode - generateSteeringAndThrottleSetpoint(); + _mecanum_act_control.manualManualMode(); } if (_vehicle_control_mode.flag_armed) { - generateActuatorSetpoint(); + _mecanum_act_control.updateActControl(); } } -void RoverMecanum::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.yaw; - _rover_steering_setpoint_pub.publish(rover_steering_setpoint); - rover_throttle_setpoint_s rover_throttle_setpoint{}; - rover_throttle_setpoint.timestamp = _timestamp; - rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; - rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; - _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); - } -} - -void RoverMecanum::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; - _current_throttle_body_y = (actuator_motors.control[2] - actuator_motors.control[0]) / 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); - const float throttle_body_y = RoverControl::throttleControl(_throttle_body_y_setpoint, - _rover_throttle_setpoint.throttle_body_y, _current_throttle_body_y, _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, throttle_body_y, - _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); - - -} - -Vector4f RoverMecanum::computeInverseKinematics(float throttle_body_x, float throttle_body_y, - const float speed_diff_normalized) -{ - const float total_speed = fabsf(throttle_body_x) + fabsf(throttle_body_y) + fabsf(speed_diff_normalized); - - if (total_speed > 1.f) { // Adjust speed setpoints if infeasible - const float theta = atan2f(fabsf(throttle_body_y), fabsf(throttle_body_x)); - const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta)); - const float normalization = 1.f / (sqrtf(powf(throttle_body_x, 2.f) + powf(throttle_body_y, 2.f))); - throttle_body_x *= magnitude * normalization; - throttle_body_y *= magnitude * normalization; - - } - - // Calculate motor commands - const float input_data[3] = {throttle_body_x, throttle_body_y, speed_diff_normalized}; - const Matrix input(input_data); - const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f}; - const Matrix m(m_data); - const Vector4f motor_commands = m * input; - - return motor_commands; -} - int RoverMecanum::task_spawn(int argc, char *argv[]) { RoverMecanum *instance = new RoverMecanum(); diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index 95ffe311bb..9c149092fc 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -56,6 +56,7 @@ #include // Local includes +#include "MecanumActControl/MecanumActControl.hpp" #include "MecanumRateControl/MecanumRateControl.hpp" #include "MecanumAttControl/MecanumAttControl.hpp" #include "MecanumVelControl/MecanumVelControl.hpp" @@ -91,62 +92,15 @@ protected: 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 throttle_body_y Normalized speed in body y direction [-1, 1]. - * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. - * @return Motor speeds [-1, 1]. - */ - Vector4f computeInverseKinematics(float throttle_body_x, float throttle_body_y, const 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 + MecanumActControl _mecanum_act_control{this}; MecanumRateControl _mecanum_rate_control{this}; MecanumAttControl _mecanum_att_control{this}; MecanumVelControl _mecanum_vel_control{this}; MecanumPosControl _mecanum_pos_control{this}; - - // Variables - hrt_abstime _timestamp{0}; - float _dt{0.f}; - float _current_throttle_body_x{0.f}; - float _current_throttle_body_y{0.f}; - - // Controllers - SlewRate _throttle_body_x_setpoint{0.f}; - SlewRate _throttle_body_y_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 - ) };