fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

differential: centralize mode management, resets and checks

Browse Source
This commit is contained in:
chfriedrich98 2025-05-06 13:44:49 +02:00 committed by chfriedrich98
parent d5dc0a7eb8
commit 38bcc50127
23 changed files with 1248 additions and 757 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/rover_differential/CMakeLists.txt
View File

@ -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

15
src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp
View File

@ -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{};

10
src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp
View File

@ -67,11 +67,6 @@ public:
*/
void updateActControl();
/**
* @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint.
*/
void manualManualMode();
/**
* @brief Stop the vehicle by sending 0 commands to motors and servos.
*/
@ -96,12 +91,9 @@ private:
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
// uORB publications
uORB::Publication<actuator_motors_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)};
uORB::Publication<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
// Variables
hrt_abstime _timestamp{0};

132
src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp
View File

@ -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 (_rover_attitude_setpoint_sub.updated()) {
rover_attitude_setpoint_s rover_attitude_setpoint{};
_rover_attitude_setpoint_sub.copy(&rover_attitude_setpoint);
_yaw_setpoint = rover_attitude_setpoint.yaw_setpoint;
}
if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
generateAttitudeAndThrottleSetpoint();
}
if (PX4_ISFINITE(_yaw_setpoint)) {
const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate,
_vehicle_yaw, _yaw_setpoint, dt);
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = _timestamp;
rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
generateRateSetpoint();
} else { // Reset pid and slew rate when attitude control is not active
_pid_yaw.resetIntegral();
_adjusted_yaw_setpoint.setForcedValue(0.f);
}
// Publish attitude controller status (logging only)
@ -97,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<float>(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<float>(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<float>(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;
}

62
src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp
View File

@ -48,15 +48,9 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/rover_attitude_status.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
/**
* @brief Class for differential attitude control.
@ -72,10 +66,21 @@ public:
~DifferentialAttControl() = default;
/**
* @brief Update attitude controller.
* @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
*/
void updateAttControl();
/**
* @brief Reset attitude controller.
*/
void reset() {_pid_yaw.resetIntegral(); _yaw_setpoint = NAN;};
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
protected:
/**
* @brief Update the parameters of the module.
@ -83,52 +88,19 @@ protected:
void updateParams() override;
private:
/**
* @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode)
* or trajectorySetpoint (Offboard attitude control).
*/
void generateAttitudeAndThrottleSetpoint();
/**
* @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
*/
void generateRateSetpoint();
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
// uORB subscriptions
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
vehicle_control_mode_s _vehicle_control_mode{};
rover_attitude_setpoint_s _rover_attitude_setpoint{};
rover_rate_setpoint_s _rover_rate_setpoint{};
offboard_control_mode_s _offboard_control_mode{};
// uORB publications
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_attitude_status_s> _rover_attitude_status_pub{ORB_ID(rover_attitude_status)};
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_attitude_status_s> _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;

36
src/modules/rover_differential/DifferentialDriveModes/CMakeLists.txt Normal file
View File

@ -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)

38
src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/CMakeLists.txt Normal file
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(DifferentialAutoMode
DifferentialAutoMode.cpp
)
target_include_directories(DifferentialAutoMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

113
src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp Normal file
View File

@ -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
}

101
src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
// Libraries
#include <lib/rover_control/RoverControl.hpp>
#include <math.h>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/rover_position_setpoint.h>
/**
* @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_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
(ParamFloat<px4::params::RD_MISS_SPD_GAIN>) _param_rd_miss_spd_gain
)
};

38
src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/CMakeLists.txt Normal file
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(DifferentialManualMode
DifferentialManualMode.cpp
)
target_include_directories(DifferentialManualMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

219
src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp Normal file
View File

@ -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<float>(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<float>(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<float>(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<float>(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);
}

130
src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
// Libraries
#include <math.h>
#include <matrix/matrix/math.hpp>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
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_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)};
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_position_setpoint_s> _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<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit
)
};

38
src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/CMakeLists.txt Normal file
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(DifferentialOffboardMode
DifferentialOffboardMode.cpp
)
target_include_directories(DifferentialOffboardMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

92
src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.cpp Normal file
View File

@ -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);
}
}

89
src/modules/rover_differential/DifferentialDriveModes/DifferentialOffboardMode/DifferentialOffboardMode.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
// Libraries
#include <math.h>
#include <matrix/matrix/math.hpp>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
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_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_setpoint_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
};

265
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
View File

@ -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,96 +46,14 @@ 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();
} else if (_vehicle_control_mode.flag_control_auto_enabled) {
autoPositionMode();
}
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();
}
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]);
@ -144,163 +61,73 @@ void DifferentialPosControl::generateVelocitySetpoint()
}
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()) {
if (target_waypoint_ned.isAllFinite()) {
float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
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 (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));
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);
}
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()
void DifferentialPosControl::updateSubscriptions()
{
manual_control_setpoint_s manual_control_setpoint{};
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
const float speed_setpoint = math::interpolate<float>(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<float>(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;
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();
}
// 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);
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);
}
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;
}

91
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp
View File

@ -41,7 +41,6 @@
#include <lib/rover_control/RoverControl.hpp>
#include <matrix/matrix/math.hpp>
#include <lib/pure_pursuit/PurePursuit.hpp>
#include <lib/geo/geo.h>
#include <math.h>
// uORB includes
@ -50,13 +49,7 @@
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/pure_pursuit_status.h>
#include <uORB/topics/rover_position_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_local_position.h>
using namespace matrix;
@ -75,10 +68,16 @@ public:
~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 @@ private:
*/
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_s> _rover_velocity_setpoint_pub{ORB_ID(rover_velocity_setpoint)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
uORB::Publication<rover_position_setpoint_s> _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<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
(ParamFloat<px4::params::RD_MISS_SPD_GAIN>) _param_rd_miss_spd_gain,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
)
};

127
src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp
View File

@ -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();
}
if (_rover_rate_setpoint_sub.updated()) {
rover_rate_setpoint_s rover_rate_setpoint{};
_rover_rate_setpoint_sub.copy(&rover_rate_setpoint);
_yaw_rate_setpoint = rover_rate_setpoint.yaw_rate_setpoint;
}
generateSteeringSetpoint();
if (PX4_ISFINITE(_yaw_rate_setpoint)) {
const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ?
_yaw_rate_setpoint : 0.f;
const float speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate,
yaw_rate_setpoint, _vehicle_yaw_rate, _param_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 { // Reset controller and slew rate when rate control is not active
} else {
_pid_yaw_rate.resetIntegral();
_adjusted_yaw_rate_setpoint.setForcedValue(0.f);
}
// Publish rate controller status (logging only)
@ -95,96 +99,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<float> (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<float>(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<float, float, float>(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<float, float, float>(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;
}

56
src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp
View File

@ -47,15 +47,9 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_rate_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_rate_status.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
/**
* @brief Class for differential rate control.
@ -71,10 +65,21 @@ public:
~DifferentialRateControl() = default;
/**
* @brief Update rate controller.
* @brief Generate and publish roverSteeringSetpoint from roverRateSetpoint.
*/
void updateRateControl();
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
/**
* @brief Reset rate controller.
*/
void reset() {_pid_yaw_rate.resetIntegral(); _yaw_rate_setpoint = NAN;};
protected:
/**
* @brief Update the parameters of the module.
@ -83,48 +88,18 @@ protected:
private:
/**
* @brief Generate and publish roverRateSetpoint and roverThrottleSetpoint from manualControlSetpoint (Acro Mode).
*/
void generateRateAndThrottleSetpoint();
/**
* @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint.
*/
void generateSteeringSetpoint();
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
// uORB subscriptions
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
vehicle_control_mode_s _vehicle_control_mode{};
offboard_control_mode_s _offboard_control_mode{};
rover_rate_setpoint_s _rover_rate_setpoint{};
// uORB publications
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
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)};
uORB::Publication<rover_rate_status_s> _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 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_RATE_TH>) _param_ro_yaw_rate_th,
(ParamFloat<px4::params::RO_YAW_RATE_P>) _param_ro_yaw_rate_p,
(ParamFloat<px4::params::RO_YAW_RATE_I>) _param_ro_yaw_rate_i,

124
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp
View File

@ -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);
}
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()
{
if (_rover_velocity_setpoint_sub.updated()) {
_rover_velocity_setpoint_sub.copy(&_rover_velocity_setpoint);
rover_velocity_setpoint_s rover_velocity_setpoint;
_rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint);
_speed_setpoint = rover_velocity_setpoint.speed;
_bearing_setpoint = rover_velocity_setpoint.bearing;
}
// 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);
float DifferentialVelControl::calcSpeedSetpoint()
{
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<float>(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<float>(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<float>(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<float>(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<float>(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<float, float>(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<float, float>(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;
}

51
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp
View File

@ -47,15 +47,12 @@
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/rover_throttle_setpoint.h>
#include <uORB/topics/rover_velocity_status.h>
#include <uORB/topics/rover_velocity_setpoint.h>
#include <uORB/topics/rover_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/rover_steering_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/vehicle_local_position.h>
using namespace matrix;
@ -82,10 +79,21 @@ public:
~DifferentialVelControl() = default;
/**
* @brief Update velocity controller.
* @brief Generate and publish roverAttitudeSetpoint/RoverThrottleSetpoint from roverVelocitySetpoint.
*/
void updateVelControl();
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
/**
* @brief Reset velocity controller.
*/
void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _bearing_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);};
protected:
/**
* @brief Update the parameters of the module.
@ -99,54 +107,35 @@ private:
void updateSubscriptions();
/**
* @brief Generate and publish roverVelocitySetpoint from velocity of trajectorySetpoint.
* @brief Calculate the speed setpoint based on the current state.
* @return Speed setpoint.
*/
void generateVelocitySetpoint();
/**
* @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint
* from roverVelocitySetpoint.
*/
void generateAttitudeAndThrottleSetpoint();
/**
* @brief Check if the necessary parameters are set.
* @return True if all checks pass.
*/
bool runSanityChecks();
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_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<rover_velocity_setpoint_s> _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<float> _speed_setpoint;
SlewRate<float> _adjusted_speed_setpoint;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,

127
src/modules/rover_differential/RoverDifferential.cpp
View File

@ -56,35 +56,132 @@ void RoverDifferential::updateParams()
void RoverDifferential::Run()
{
if (_parameter_update_sub.updated()) {
parameter_update_s param_update{};
_parameter_update_sub.copy(&param_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;
}
}
const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled
&& !_vehicle_control_mode.flag_control_rates_enabled;
if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) {
if (full_manual_mode_enabled) { // Manual mode
_differential_act_control.manualManualMode();
}
_was_armed = true;
if (_vehicle_control_mode.flag_armed) {
_differential_act_control.updateActControl();
// Generate setpoints
if (_vehicle_control_mode.flag_control_manual_enabled) {
manualControl();
} else {
} 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();
}
if (_vehicle_control_mode.flag_control_velocity_enabled) {
_differential_vel_control.updateVelControl();
}
if (_vehicle_control_mode.flag_control_attitude_enabled) {
_differential_att_control.updateAttControl();
}
if (_vehicle_control_mode.flag_control_rates_enabled) {
_differential_rate_control.updateRateControl();
}
if (_vehicle_control_mode.flag_control_allocation_enabled) {
_differential_act_control.updateActControl();
}
}
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[])
{
RoverDifferential *instance = new RoverDifferential();

47
src/modules/rover_differential/RoverDifferential.hpp
View File

@ -40,6 +40,9 @@
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
// Library includes
#include <math.h>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
@ -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<RoverDifferential>, public ModuleParams,
public px4::ScheduledWorkItem
@ -82,15 +88,46 @@ protected:
private:
void Run() override;
/**
* @brief Handle manual control
*/
void manualControl();
/**
* @brief Update the controllers
*/
void updateControllers();
/**
* @brief Check proper parameter setup for the controllers
*
* Modifies:
*
* - _sanity_checks_passed: true if checks for all active controllers pass
*/
void runSanityChecks();
/**
* @brief Reset controllers and manual mode variables.
*/
void reset();
// uORB subscriptions
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
vehicle_control_mode_s _vehicle_control_mode{};
// 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
};