Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

mecanum: seperate actuator control

Browse Source
This commit is contained in:
chfriedrich98
2025-05-06 15:52:54 +02:00
committed by Silvan Fuhrer
parent 85c2f8ebbb
commit 5e9322899b
6 changed files with 321 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_mecanum/CMakeLists.txt
+2
View File
@@ -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
src/modules/rover_mecanum/MecanumActControl/CMakeLists.txt
+38
View File
@@ -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})
src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp
+153
View File
@@ -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<float, 3, 1> 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<float, 4, 3> 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);
}
src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp
+124
View File
@@ -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 <px4_platform_common/module_params.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
/**
* @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_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
uORB::Publication<rover_throttle_setpoint_s> _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<float> _adjusted_throttle_x_setpoint{0.f};
SlewRate<float> _adjusted_throttle_y_setpoint{0.f};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
)
};
src/modules/rover_mecanum/RoverMecanum.cpp
+2 -87
View File
@@ -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<float, 3, 1> 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<float, 4, 3> m(m_data);
const Vector4f motor_commands = m * input;
return motor_commands;
}
int RoverMecanum::task_spawn(int argc, char *argv[])
{
RoverMecanum *instance = new RoverMecanum();
src/modules/rover_mecanum/RoverMecanum.hpp
+2 -48
View File
@@ -56,6 +56,7 @@
#include <uORB/topics/manual_control_setpoint.h>
// 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_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_steering_setpoint_s> _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<float> _throttle_body_x_setpoint{0.f};
SlewRate<float> _throttle_body_y_setpoint{0.f};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
)
};