mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ackermann: split modes into seperate folder
This commit is contained in:
chfriedrich98
chfriedrich98
parent
47a9b552f8
commit
a129a29793
@ -36,6 +36,7 @@ add_subdirectory(AckermannRateControl)
|
||||
add_subdirectory(AckermannAttControl)
|
||||
add_subdirectory(AckermannVelControl)
|
||||
add_subdirectory(AckermannPosControl)
|
||||
add_subdirectory(DriveModes)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__rover_ackermann
|
||||
@ -49,6 +50,9 @@ px4_add_module(
|
||||
AckermannAttControl
|
||||
AckermannVelControl
|
||||
AckermannPosControl
|
||||
AutoMode
|
||||
ManualMode
|
||||
OffboardMode
|
||||
px4_work_queue
|
||||
rover_control
|
||||
pure_pursuit
|
||||
|
||||
196
src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.cpp
Normal file
196
src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.cpp
Normal file
@ -0,0 +1,196 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "AutoMode.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
AutoMode::AutoMode(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
updateParams();
|
||||
_rover_position_setpoint_pub.advertise();
|
||||
}
|
||||
|
||||
void AutoMode::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
|
||||
|
||||
if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON
|
||||
&& _param_ra_max_str_ang.get() > FLT_EPSILON) {
|
||||
_min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get());
|
||||
}
|
||||
}
|
||||
|
||||
void AutoMode::autoControl()
|
||||
{
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
|
||||
if (_vehicle_local_position_sub.updated()) {
|
||||
vehicle_local_position_s vehicle_local_position{};
|
||||
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
||||
|
||||
if (!_global_ned_proj_ref.isInitialized()
|
||||
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
|
||||
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
|
||||
vehicle_local_position.ref_timestamp);
|
||||
}
|
||||
|
||||
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
|
||||
}
|
||||
|
||||
if (_position_setpoint_triplet_sub.updated()) {
|
||||
updateWaypointsAndAcceptanceRadius();
|
||||
}
|
||||
|
||||
// Distances to waypoints
|
||||
const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0),
|
||||
2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2));
|
||||
const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
|
||||
2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
|
||||
|
||||
rover_position_setpoint_s rover_position_setpoint{};
|
||||
rover_position_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_position_setpoint.position_ned[0] = _curr_wp_ned(0);
|
||||
rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
|
||||
rover_position_setpoint.start_ned[0] = _prev_wp_ned(0);
|
||||
rover_position_setpoint.start_ned[1] = _prev_wp_ned(1);
|
||||
rover_position_setpoint.arrival_speed = arrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type,
|
||||
_waypoint_transition_angle, _max_yaw_rate);
|
||||
rover_position_setpoint.cruising_speed = cruisingSpeed(_cruising_speed, _min_speed, distance_to_prev_wp,
|
||||
distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _waypoint_transition_angle,
|
||||
_prev_waypoint_transition_angle, _max_yaw_rate);
|
||||
rover_position_setpoint.yaw = NAN;
|
||||
_rover_position_setpoint_pub.publish(rover_position_setpoint);
|
||||
}
|
||||
|
||||
void AutoMode::updateWaypointsAndAcceptanceRadius()
|
||||
{
|
||||
position_setpoint_triplet_s position_setpoint_triplet{};
|
||||
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
|
||||
_curr_wp_type = position_setpoint_triplet.current.type;
|
||||
|
||||
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
|
||||
_curr_pos_ned, _global_ned_proj_ref);
|
||||
|
||||
_prev_waypoint_transition_angle = _waypoint_transition_angle;
|
||||
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
|
||||
|
||||
// Update acceptance radius
|
||||
_prev_acceptance_radius = _acceptance_radius;
|
||||
|
||||
if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
|
||||
_acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(),
|
||||
_param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get());
|
||||
|
||||
} else {
|
||||
_acceptance_radius = _param_nav_acc_rad.get();
|
||||
}
|
||||
|
||||
// Waypoint cruising speed
|
||||
_cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
|
||||
position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
|
||||
}
|
||||
|
||||
float AutoMode::updateAcceptanceRadius(const float waypoint_transition_angle,
|
||||
const float default_acceptance_radius, const float acceptance_radius_gain,
|
||||
const float acceptance_radius_max, const float wheel_base, const float max_steer_angle)
|
||||
{
|
||||
// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
|
||||
float acceptance_radius = default_acceptance_radius;
|
||||
|
||||
if (PX4_ISFINITE(_waypoint_transition_angle)) {
|
||||
const float theta = waypoint_transition_angle / 2.f;
|
||||
const float min_turning_radius = wheel_base / sinf(max_steer_angle);
|
||||
const float acceptance_radius_temp = min_turning_radius / tanf(theta);
|
||||
const float acceptance_radius_temp_scaled = acceptance_radius_gain *
|
||||
acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
|
||||
acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
|
||||
acceptance_radius_max);
|
||||
}
|
||||
|
||||
// Publish updated acceptance radius
|
||||
position_controller_status_s pos_ctrl_status{};
|
||||
pos_ctrl_status.acceptance_radius = acceptance_radius;
|
||||
pos_ctrl_status.timestamp = hrt_absolute_time();
|
||||
_position_controller_status_pub.publish(pos_ctrl_status);
|
||||
return acceptance_radius;
|
||||
}
|
||||
|
||||
float AutoMode::arrivalSpeed(const float cruising_speed, const float miss_speed_min, const float acc_rad,
|
||||
const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate)
|
||||
{
|
||||
if (!PX4_ISFINITE(waypoint_transition_angle)
|
||||
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
|
||||
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
|
||||
return 0.f; // Stop at the waypoint
|
||||
|
||||
} else {
|
||||
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
|
||||
const float cornering_speed = max_yaw_rate * turning_circle;
|
||||
return math::constrain(cornering_speed, miss_speed_min, cruising_speed); // Slow down for cornering
|
||||
}
|
||||
}
|
||||
|
||||
float AutoMode::cruisingSpeed(const float cruising_speed, const float miss_speed_min,
|
||||
const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, const float prev_acc_rad,
|
||||
const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_yaw_rate)
|
||||
{
|
||||
// Catch improper values
|
||||
if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) {
|
||||
return cruising_speed;
|
||||
}
|
||||
|
||||
// Cornering slow down effect
|
||||
if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON && PX4_ISFINITE(prev_waypoint_transition_angle)) {
|
||||
const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f);
|
||||
const float cornering_speed = max_yaw_rate * turning_circle;
|
||||
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
|
||||
|
||||
}
|
||||
|
||||
if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON && PX4_ISFINITE(waypoint_transition_angle)) {
|
||||
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
|
||||
const float cornering_speed = max_yaw_rate * turning_circle;
|
||||
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
|
||||
|
||||
}
|
||||
|
||||
return cruising_speed; // Fallthrough
|
||||
|
||||
}
|
||||
161
src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.hpp
Normal file
161
src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.hpp
Normal file
@ -0,0 +1,161 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
// PX4 includes
|
||||
#include <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_attitude.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/position_controller_status.h>
|
||||
#include <uORB/topics/rover_position_setpoint.h>
|
||||
|
||||
/**
|
||||
* @brief Class for ackermann auto mode.
|
||||
*/
|
||||
class AutoMode : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for auto mode.
|
||||
* @param parent The parent ModuleParams object.
|
||||
*/
|
||||
AutoMode(ModuleParams *parent);
|
||||
~AutoMode() = default;
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverPositionSetpoint from positionSetpointTriplet.
|
||||
*/
|
||||
void autoControl();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Update the parameters of the module.
|
||||
*/
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Update global/NED waypoint coordinates and acceptance radius.
|
||||
*/
|
||||
void updateWaypointsAndAcceptanceRadius();
|
||||
|
||||
/**
|
||||
* @brief Publish the acceptance radius for current waypoint based on the angle between a line segment
|
||||
* from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle.
|
||||
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param default_acceptance_radius Default acceptance radius for waypoints [m].
|
||||
* @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-].
|
||||
* @param acceptance_radius_max Maximum value for the acceptance radius [m].
|
||||
* @param wheel_base Rover wheelbase [m].
|
||||
* @param max_steer_angle Rover maximum steer angle [rad].
|
||||
* @return Updated acceptance radius [m].
|
||||
*/
|
||||
float updateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius,
|
||||
float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle);
|
||||
|
||||
/**
|
||||
* @brief Calculate the speed at which the rover should arrive at the current waypoint based on the upcoming corner.
|
||||
* @param cruising_speed Cruising speed [m/s].
|
||||
* @param miss_speed_min Minimum speed setpoint [m/s].
|
||||
* @param acc_rad Acceptance radius of the current waypoint [m].
|
||||
* @param curr_wp_type Type of the current waypoint.
|
||||
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param max_yaw_rate Maximum yaw rate setpoint [rad/s]
|
||||
* @return Speed setpoint [m/s].
|
||||
*/
|
||||
float arrivalSpeed(float cruising_speed, float miss_speed_min, float acc_rad, int curr_wp_type,
|
||||
float waypoint_transition_angle, float max_yaw_rate);
|
||||
|
||||
/**
|
||||
* @brief Calculate the cruising speed setpoint. During cornering the speed is restricted based on the radius of the corner.
|
||||
* @param cruising_speed Cruising speed [m/s].
|
||||
* @param miss_speed_min Minimum speed setpoint [m/s].
|
||||
* @param distance_to_prev_wp Distance to the previous waypoint [m].
|
||||
* @param distance_to_curr_wp Distance to the current waypoint [m].
|
||||
* @param acc_rad Acceptance radius of the current waypoint [m].
|
||||
* @param prev_acc_rad Acceptance radius of the previous waypoint [m].
|
||||
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param max_yaw_rate Maximum yaw rate setpoint [rad/s]
|
||||
* @return Speed setpoint [m/s].
|
||||
*/
|
||||
float cruisingSpeed(float cruising_speed, float miss_speed_min, float distance_to_prev_wp,
|
||||
float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float waypoint_transition_angle,
|
||||
float prev_waypoint_transition_angle, float max_yaw_rate);
|
||||
|
||||
// uORB subscriptions
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
|
||||
|
||||
// uORB publications
|
||||
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
|
||||
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
|
||||
|
||||
// Variables
|
||||
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
|
||||
Quatf _vehicle_attitude_quaternion{};
|
||||
Vector2f _curr_wp_ned{NAN, NAN};
|
||||
Vector2f _prev_wp_ned{NAN, NAN};
|
||||
Vector2f _next_wp_ned{NAN, NAN};
|
||||
Vector2f _curr_pos_ned{NAN, NAN};
|
||||
float _acceptance_radius{0.5f};
|
||||
float _prev_acceptance_radius{0.5f};
|
||||
float _cruising_speed{0.f};
|
||||
float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
float _max_yaw_rate{NAN};
|
||||
float _vehicle_yaw{NAN};
|
||||
float _min_speed{NAN}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base.
|
||||
int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
|
||||
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
|
||||
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
|
||||
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
|
||||
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
|
||||
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
|
||||
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain
|
||||
)
|
||||
};
|
||||
@ -0,0 +1,38 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(AutoMode
|
||||
AutoMode.cpp
|
||||
)
|
||||
|
||||
target_include_directories(AutoMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
36
src/modules/rover_ackermann/DriveModes/CMakeLists.txt
Normal file
36
src/modules/rover_ackermann/DriveModes/CMakeLists.txt
Normal 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(AutoMode)
|
||||
add_subdirectory(ManualMode)
|
||||
add_subdirectory(OffboardMode)
|
||||
@ -0,0 +1,38 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(ManualMode
|
||||
ManualMode.cpp
|
||||
)
|
||||
|
||||
target_include_directories(ManualMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
229
src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.cpp
Normal file
229
src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.cpp
Normal file
@ -0,0 +1,229 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ManualMode.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
ManualMode::ManualMode(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
updateParams();
|
||||
_rover_throttle_setpoint_pub.advertise();
|
||||
_rover_steering_setpoint_pub.advertise();
|
||||
_rover_rate_setpoint_pub.advertise();
|
||||
_rover_attitude_setpoint_pub.advertise();
|
||||
_rover_velocity_setpoint_pub.advertise();
|
||||
_rover_position_setpoint_pub.advertise();
|
||||
}
|
||||
|
||||
void ManualMode::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
|
||||
}
|
||||
|
||||
void ManualMode::manualControl(const int nav_state)
|
||||
{
|
||||
switch (nav_state) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
|
||||
manual();
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ACRO:
|
||||
acro();
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_STAB:
|
||||
stab();
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
|
||||
position();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void ManualMode::manual()
|
||||
{
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||
rover_steering_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll;
|
||||
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
|
||||
rover_throttle_setpoint.throttle_body_y = 0.f;
|
||||
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
|
||||
}
|
||||
|
||||
void ManualMode::acro()
|
||||
{
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
|
||||
rover_throttle_setpoint.throttle_body_y = 0.f;
|
||||
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
|
||||
rover_rate_setpoint_s rover_rate_setpoint{};
|
||||
rover_rate_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * math::interpolate<float>
|
||||
(manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
|
||||
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
|
||||
}
|
||||
|
||||
void ManualMode::stab()
|
||||
{
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
|
||||
rover_throttle_setpoint.throttle_body_y = 0.f;
|
||||
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
|
||||
|
||||
const float yaw_delta = math::interpolate<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_setpoint = NAN;
|
||||
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + matrix::sign(manual_control_setpoint.throttle) * yaw_delta);
|
||||
rover_attitude_setpoint_s rover_attitude_setpoint{};
|
||||
rover_attitude_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
|
||||
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
|
||||
|
||||
} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
|
||||
if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
|
||||
_stab_yaw_setpoint = _vehicle_yaw;
|
||||
}
|
||||
|
||||
rover_attitude_setpoint_s rover_attitude_setpoint{};
|
||||
rover_attitude_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
|
||||
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
void ManualMode::position()
|
||||
{
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
|
||||
if (_vehicle_local_position_sub.updated()) {
|
||||
vehicle_local_position_s vehicle_local_position{};
|
||||
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
||||
|
||||
if (!_global_ned_proj_ref.isInitialized()
|
||||
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
|
||||
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
|
||||
vehicle_local_position.ref_timestamp);
|
||||
}
|
||||
|
||||
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
|
||||
}
|
||||
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
|
||||
const float speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
|
||||
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
|
||||
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(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
|
||||
_pos_ctl_course_direction = Vector2f(NAN, NAN);
|
||||
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
|
||||
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta);
|
||||
const Vector2f pos_ctl_course_direction = Vector2f(cos(yaw_setpoint), sin(yaw_setpoint));
|
||||
const Vector2f target_waypoint_ned = _curr_pos_ned + sign(speed_setpoint) * _param_pp_lookahd_max.get() *
|
||||
pos_ctl_course_direction;
|
||||
rover_position_setpoint_s rover_position_setpoint{};
|
||||
rover_position_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
|
||||
rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
|
||||
rover_position_setpoint.start_ned[0] = NAN;
|
||||
rover_position_setpoint.start_ned[1] = NAN;
|
||||
rover_position_setpoint.arrival_speed = NAN;
|
||||
rover_position_setpoint.cruising_speed = speed_setpoint;
|
||||
rover_position_setpoint.yaw = NAN;
|
||||
_rover_position_setpoint_pub.publish(rover_position_setpoint);
|
||||
|
||||
} else { // Course control if the steering input is zero (keep driving on a straight line)
|
||||
if (!_pos_ctl_course_direction.isAllFinite()) {
|
||||
_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
|
||||
_pos_ctl_start_position_ned = _curr_pos_ned;
|
||||
}
|
||||
|
||||
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
|
||||
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
|
||||
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
|
||||
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_setpoint) *
|
||||
vector_scaling * _pos_ctl_course_direction;
|
||||
rover_position_setpoint_s rover_position_setpoint{};
|
||||
rover_position_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
|
||||
rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
|
||||
rover_position_setpoint.start_ned[0] = _pos_ctl_start_position_ned(0);
|
||||
rover_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1);
|
||||
rover_position_setpoint.arrival_speed = NAN;
|
||||
rover_position_setpoint.cruising_speed = speed_setpoint;
|
||||
rover_position_setpoint.yaw = NAN;
|
||||
_rover_position_setpoint_pub.publish(rover_position_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
void ManualMode::reset()
|
||||
{
|
||||
_stab_yaw_setpoint = NAN;
|
||||
_pos_ctl_course_direction = Vector2f(NAN, NAN);
|
||||
_pos_ctl_start_position_ned = Vector2f(NAN, NAN);
|
||||
_curr_pos_ned = Vector2f(NAN, NAN);
|
||||
}
|
||||
137
src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.hpp
Normal file
137
src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.hpp
Normal file
@ -0,0 +1,137 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
// PX4 includes
|
||||
#include <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/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.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>
|
||||
|
||||
/**
|
||||
* @brief Class for ackermann manual mode.
|
||||
*/
|
||||
class ManualMode : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for ManualMode.
|
||||
* @param parent The parent ModuleParams object.
|
||||
*/
|
||||
ManualMode(ModuleParams *parent);
|
||||
~ManualMode() = default;
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverSetpoints from manualControlSetpoints.
|
||||
*/
|
||||
void manualControl(int nav_state);
|
||||
|
||||
/**
|
||||
* @brief Reset manual mode variables.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Update the parameters of the module.
|
||||
*/
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint.
|
||||
*/
|
||||
void manual();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverThrottleSetpoint and RoverRateSetpoint from manualControlSetpoint.
|
||||
*/
|
||||
void acro();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverThrottleSetpoint and RoverAttitudeSetpoint from manualControlSetpoint.
|
||||
*/
|
||||
void stab();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint.
|
||||
*/
|
||||
void position();
|
||||
|
||||
// uORB subscriptions
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
|
||||
// uORB publications
|
||||
uORB::Publication<rover_throttle_setpoint_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
|
||||
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
|
||||
Quatf _vehicle_attitude_quaternion{};
|
||||
Vector2f _pos_ctl_course_direction{NAN, NAN};
|
||||
Vector2f _pos_ctl_start_position_ned{NAN, NAN};
|
||||
Vector2f _curr_pos_ned{NAN, NAN};
|
||||
float _stab_yaw_setpoint{NAN};
|
||||
float _vehicle_yaw{NAN};
|
||||
float _max_yaw_rate{NAN};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
|
||||
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
|
||||
(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
|
||||
)
|
||||
};
|
||||
@ -0,0 +1,38 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(OffboardMode
|
||||
OffboardMode.cpp
|
||||
)
|
||||
|
||||
target_include_directories(OffboardMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@ -0,0 +1,79 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "OffboardMode.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
OffboardMode::OffboardMode(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
updateParams();
|
||||
_rover_velocity_setpoint_pub.advertise();
|
||||
_rover_position_setpoint_pub.advertise();
|
||||
}
|
||||
|
||||
void OffboardMode::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
}
|
||||
|
||||
void OffboardMode::offboardControl()
|
||||
{
|
||||
offboard_control_mode_s offboard_control_mode{};
|
||||
_offboard_control_mode_sub.copy(&offboard_control_mode);
|
||||
|
||||
trajectory_setpoint_s trajectory_setpoint{};
|
||||
_trajectory_setpoint_sub.copy(&trajectory_setpoint);
|
||||
|
||||
if (offboard_control_mode.position) {
|
||||
rover_position_setpoint_s rover_position_setpoint{};
|
||||
rover_position_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0];
|
||||
rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1];
|
||||
rover_position_setpoint.start_ned[0] = NAN;
|
||||
rover_position_setpoint.start_ned[1] = NAN;
|
||||
rover_position_setpoint.cruising_speed = NAN;
|
||||
rover_position_setpoint.arrival_speed = NAN;
|
||||
rover_position_setpoint.yaw = NAN;
|
||||
_rover_position_setpoint_pub.publish(rover_position_setpoint);
|
||||
|
||||
} else if (offboard_control_mode.velocity) {
|
||||
const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
|
||||
rover_velocity_setpoint_s rover_velocity_setpoint{};
|
||||
rover_velocity_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_velocity_setpoint.speed = velocity_ned.norm();
|
||||
rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0));
|
||||
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
|
||||
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,85 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
// PX4 includes
|
||||
#include <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_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 ackermann manual mode.
|
||||
*/
|
||||
class OffboardMode : public ModuleParams
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for OffboardMode.
|
||||
* @param parent The parent ModuleParams object.
|
||||
*/
|
||||
OffboardMode(ModuleParams *parent);
|
||||
~OffboardMode() = default;
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverSetpoints from trajectorySetpoint.
|
||||
*/
|
||||
void offboardControl();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Update the parameters of the module.
|
||||
*/
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
// uORB subscriptions
|
||||
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
|
||||
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
|
||||
|
||||
// uORB publications
|
||||
uORB::Publication<rover_velocity_setpoint_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)};
|
||||
};
|
||||
@ -51,12 +51,6 @@ bool RoverAckermann::init()
|
||||
void RoverAckermann::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
|
||||
|
||||
if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON
|
||||
&& _param_ra_max_str_ang.get() > FLT_EPSILON) {
|
||||
_min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get());
|
||||
}
|
||||
}
|
||||
|
||||
void RoverAckermann::Run()
|
||||
@ -99,13 +93,13 @@ void RoverAckermann::Run()
|
||||
if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) {
|
||||
// Generate setpoints
|
||||
if (_vehicle_control_mode.flag_control_manual_enabled) {
|
||||
manualControl();
|
||||
_manual_mode.manualControl(_nav_state);
|
||||
|
||||
} else if (_vehicle_control_mode.flag_control_auto_enabled) {
|
||||
autoPositionMode();
|
||||
_auto_mode.autoControl();
|
||||
|
||||
} else if (_vehicle_control_mode.flag_control_offboard_enabled) {
|
||||
offboardControl();
|
||||
_offboard_mode.offboardControl();
|
||||
}
|
||||
|
||||
updateControllers();
|
||||
@ -118,348 +112,6 @@ void RoverAckermann::Run()
|
||||
|
||||
}
|
||||
|
||||
void RoverAckermann::manualControl()
|
||||
{
|
||||
switch (_nav_state) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
|
||||
manualManualMode();
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ACRO:
|
||||
manualAcroMode();
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_STAB:
|
||||
manualStabMode();
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
|
||||
manualPositionMode();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void RoverAckermann::manualManualMode()
|
||||
{
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
rover_steering_setpoint_s rover_steering_setpoint{};
|
||||
rover_steering_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll;
|
||||
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
|
||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
|
||||
rover_throttle_setpoint.throttle_body_y = 0.f;
|
||||
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
|
||||
}
|
||||
|
||||
void RoverAckermann::manualAcroMode()
|
||||
{
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
|
||||
rover_throttle_setpoint.throttle_body_y = 0.f;
|
||||
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
|
||||
rover_rate_setpoint_s rover_rate_setpoint{};
|
||||
rover_rate_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * math::interpolate<float>
|
||||
(manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
|
||||
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
|
||||
}
|
||||
|
||||
void RoverAckermann::manualStabMode()
|
||||
{
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
rover_throttle_setpoint_s rover_throttle_setpoint{};
|
||||
rover_throttle_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
|
||||
rover_throttle_setpoint.throttle_body_y = 0.f;
|
||||
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
|
||||
|
||||
const float yaw_delta = math::interpolate<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_setpoint = NAN;
|
||||
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + matrix::sign(manual_control_setpoint.throttle) * yaw_delta);
|
||||
rover_attitude_setpoint_s rover_attitude_setpoint{};
|
||||
rover_attitude_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
|
||||
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
|
||||
|
||||
} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
|
||||
if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
|
||||
_stab_yaw_setpoint = _vehicle_yaw;
|
||||
}
|
||||
|
||||
rover_attitude_setpoint_s rover_attitude_setpoint{};
|
||||
rover_attitude_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
|
||||
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
void RoverAckermann::manualPositionMode()
|
||||
{
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
|
||||
if (_vehicle_local_position_sub.updated()) {
|
||||
vehicle_local_position_s vehicle_local_position{};
|
||||
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
||||
|
||||
if (!_global_ned_proj_ref.isInitialized()
|
||||
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
|
||||
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
|
||||
vehicle_local_position.ref_timestamp);
|
||||
}
|
||||
|
||||
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
|
||||
}
|
||||
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
|
||||
const float speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
|
||||
-1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
|
||||
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(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
|
||||
_pos_ctl_course_direction = Vector2f(NAN, NAN);
|
||||
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
|
||||
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta);
|
||||
const Vector2f pos_ctl_course_direction = Vector2f(cos(yaw_setpoint), sin(yaw_setpoint));
|
||||
const Vector2f target_waypoint_ned = _curr_pos_ned + sign(speed_setpoint) * _param_pp_lookahd_max.get() *
|
||||
pos_ctl_course_direction;
|
||||
rover_position_setpoint_s rover_position_setpoint{};
|
||||
rover_position_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
|
||||
rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
|
||||
rover_position_setpoint.start_ned[0] = NAN;
|
||||
rover_position_setpoint.start_ned[1] = NAN;
|
||||
rover_position_setpoint.arrival_speed = NAN;
|
||||
rover_position_setpoint.cruising_speed = speed_setpoint;
|
||||
rover_position_setpoint.yaw = NAN;
|
||||
_rover_position_setpoint_pub.publish(rover_position_setpoint);
|
||||
|
||||
} else { // Course control if the steering input is zero (keep driving on a straight line)
|
||||
if (!_pos_ctl_course_direction.isAllFinite()) {
|
||||
_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
|
||||
_pos_ctl_start_position_ned = _curr_pos_ned;
|
||||
}
|
||||
|
||||
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
|
||||
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
|
||||
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
|
||||
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_setpoint) *
|
||||
vector_scaling * _pos_ctl_course_direction;
|
||||
rover_position_setpoint_s rover_position_setpoint{};
|
||||
rover_position_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
|
||||
rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
|
||||
rover_position_setpoint.start_ned[0] = _pos_ctl_start_position_ned(0);
|
||||
rover_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1);
|
||||
rover_position_setpoint.arrival_speed = NAN;
|
||||
rover_position_setpoint.cruising_speed = speed_setpoint;
|
||||
rover_position_setpoint.yaw = NAN;
|
||||
_rover_position_setpoint_pub.publish(rover_position_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
void RoverAckermann::autoPositionMode()
|
||||
{
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
|
||||
if (_vehicle_local_position_sub.updated()) {
|
||||
vehicle_local_position_s vehicle_local_position{};
|
||||
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
||||
|
||||
if (!_global_ned_proj_ref.isInitialized()
|
||||
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
|
||||
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
|
||||
vehicle_local_position.ref_timestamp);
|
||||
}
|
||||
|
||||
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
|
||||
}
|
||||
|
||||
if (_position_setpoint_triplet_sub.updated()) {
|
||||
autoUpdateWaypointsAndAcceptanceRadius();
|
||||
}
|
||||
|
||||
// Distances to waypoints
|
||||
const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0),
|
||||
2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2));
|
||||
const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
|
||||
2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
|
||||
|
||||
rover_position_setpoint_s rover_position_setpoint{};
|
||||
rover_position_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_position_setpoint.position_ned[0] = _curr_wp_ned(0);
|
||||
rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
|
||||
rover_position_setpoint.start_ned[0] = _prev_wp_ned(0);
|
||||
rover_position_setpoint.start_ned[1] = _prev_wp_ned(1);
|
||||
rover_position_setpoint.arrival_speed = autoArrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type,
|
||||
_waypoint_transition_angle, _max_yaw_rate);
|
||||
rover_position_setpoint.cruising_speed = autoCruisingSpeed(_cruising_speed, _min_speed, distance_to_prev_wp,
|
||||
distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _waypoint_transition_angle,
|
||||
_prev_waypoint_transition_angle, _max_yaw_rate);
|
||||
rover_position_setpoint.yaw = NAN;
|
||||
_rover_position_setpoint_pub.publish(rover_position_setpoint);
|
||||
}
|
||||
|
||||
void RoverAckermann::autoUpdateWaypointsAndAcceptanceRadius()
|
||||
{
|
||||
position_setpoint_triplet_s position_setpoint_triplet{};
|
||||
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
|
||||
_curr_wp_type = position_setpoint_triplet.current.type;
|
||||
|
||||
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
|
||||
_curr_pos_ned, _global_ned_proj_ref);
|
||||
|
||||
_prev_waypoint_transition_angle = _waypoint_transition_angle;
|
||||
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
|
||||
|
||||
// Update acceptance radius
|
||||
_prev_acceptance_radius = _acceptance_radius;
|
||||
|
||||
if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
|
||||
_acceptance_radius = autoUpdateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(),
|
||||
_param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get());
|
||||
|
||||
} else {
|
||||
_acceptance_radius = _param_nav_acc_rad.get();
|
||||
}
|
||||
|
||||
// Waypoint cruising speed
|
||||
_cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
|
||||
position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
|
||||
}
|
||||
|
||||
float RoverAckermann::autoUpdateAcceptanceRadius(const float waypoint_transition_angle,
|
||||
const float default_acceptance_radius, const float acceptance_radius_gain,
|
||||
const float acceptance_radius_max, const float wheel_base, const float max_steer_angle)
|
||||
{
|
||||
// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
|
||||
float acceptance_radius = default_acceptance_radius;
|
||||
|
||||
if (PX4_ISFINITE(_waypoint_transition_angle)) {
|
||||
const float theta = waypoint_transition_angle / 2.f;
|
||||
const float min_turning_radius = wheel_base / sinf(max_steer_angle);
|
||||
const float acceptance_radius_temp = min_turning_radius / tanf(theta);
|
||||
const float acceptance_radius_temp_scaled = acceptance_radius_gain *
|
||||
acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
|
||||
acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
|
||||
acceptance_radius_max);
|
||||
}
|
||||
|
||||
// Publish updated acceptance radius
|
||||
position_controller_status_s pos_ctrl_status{};
|
||||
pos_ctrl_status.acceptance_radius = acceptance_radius;
|
||||
pos_ctrl_status.timestamp = hrt_absolute_time();
|
||||
_position_controller_status_pub.publish(pos_ctrl_status);
|
||||
return acceptance_radius;
|
||||
}
|
||||
|
||||
float RoverAckermann::autoArrivalSpeed(const float cruising_speed, const float miss_speed_min, const float acc_rad,
|
||||
const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate)
|
||||
{
|
||||
if (!PX4_ISFINITE(waypoint_transition_angle)
|
||||
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
|
||||
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
|
||||
return 0.f; // Stop at the waypoint
|
||||
|
||||
} else {
|
||||
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
|
||||
const float cornering_speed = max_yaw_rate * turning_circle;
|
||||
return math::constrain(cornering_speed, miss_speed_min, cruising_speed); // Slow down for cornering
|
||||
}
|
||||
}
|
||||
|
||||
float RoverAckermann::autoCruisingSpeed(const float cruising_speed, const float miss_speed_min,
|
||||
const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, const float prev_acc_rad,
|
||||
const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_yaw_rate)
|
||||
{
|
||||
// Catch improper values
|
||||
if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) {
|
||||
return cruising_speed;
|
||||
}
|
||||
|
||||
// Cornering slow down effect
|
||||
if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON && PX4_ISFINITE(prev_waypoint_transition_angle)) {
|
||||
const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f);
|
||||
const float cornering_speed = max_yaw_rate * turning_circle;
|
||||
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
|
||||
|
||||
}
|
||||
|
||||
if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON && PX4_ISFINITE(waypoint_transition_angle)) {
|
||||
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
|
||||
const float cornering_speed = max_yaw_rate * turning_circle;
|
||||
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
|
||||
|
||||
}
|
||||
|
||||
return cruising_speed; // Fallthrough
|
||||
|
||||
}
|
||||
|
||||
void RoverAckermann::offboardControl()
|
||||
{
|
||||
offboard_control_mode_s offboard_control_mode{};
|
||||
_offboard_control_mode_sub.copy(&offboard_control_mode);
|
||||
|
||||
trajectory_setpoint_s trajectory_setpoint{};
|
||||
_trajectory_setpoint_sub.copy(&trajectory_setpoint);
|
||||
|
||||
if (offboard_control_mode.position) {
|
||||
rover_position_setpoint_s rover_position_setpoint{};
|
||||
rover_position_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_position_setpoint.position_ned[0] = trajectory_setpoint.position[0];
|
||||
rover_position_setpoint.position_ned[1] = trajectory_setpoint.position[1];
|
||||
rover_position_setpoint.start_ned[0] = NAN;
|
||||
rover_position_setpoint.start_ned[1] = NAN;
|
||||
rover_position_setpoint.cruising_speed = NAN;
|
||||
rover_position_setpoint.arrival_speed = NAN;
|
||||
rover_position_setpoint.yaw = NAN;
|
||||
_rover_position_setpoint_pub.publish(rover_position_setpoint);
|
||||
|
||||
} else if (offboard_control_mode.velocity) {
|
||||
const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
|
||||
rover_velocity_setpoint_s rover_velocity_setpoint{};
|
||||
rover_velocity_setpoint.timestamp = hrt_absolute_time();
|
||||
rover_velocity_setpoint.speed = velocity_ned.norm();
|
||||
rover_velocity_setpoint.bearing = atan2f(velocity_ned(1), velocity_ned(0));
|
||||
_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void RoverAckermann::updateControllers()
|
||||
{
|
||||
if (_vehicle_control_mode.flag_control_position_enabled) {
|
||||
@ -513,10 +165,7 @@ void RoverAckermann::reset()
|
||||
_ackermann_vel_control.reset();
|
||||
_ackermann_att_control.reset();
|
||||
_ackermann_rate_control.reset();
|
||||
_stab_yaw_setpoint = NAN;
|
||||
_pos_ctl_course_direction = Vector2f(NAN, NAN);
|
||||
_pos_ctl_start_position_ned = Vector2f(NAN, NAN);
|
||||
_curr_pos_ned = Vector2f(NAN, NAN);
|
||||
_manual_mode.reset();
|
||||
}
|
||||
|
||||
int RoverAckermann::task_spawn(int argc, char *argv[])
|
||||
|
||||
@ -42,28 +42,13 @@
|
||||
|
||||
// Library includes
|
||||
#include <math.h>
|
||||
#include <lib/rover_control/RoverControl.hpp>
|
||||
|
||||
// uORB includes
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/rover_velocity_setpoint.h>
|
||||
#include <uORB/topics/rover_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/offboard_control_mode.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
#include <uORB/topics/rover_steering_setpoint.h>
|
||||
#include <uORB/topics/rover_throttle_setpoint.h>
|
||||
#include <uORB/topics/rover_rate_setpoint.h>
|
||||
#include <uORB/topics/rover_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/position_setpoint.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/position_controller_status.h>
|
||||
|
||||
// Local includes
|
||||
#include "AckermannActControl/AckermannActControl.hpp"
|
||||
@ -71,6 +56,9 @@
|
||||
#include "AckermannAttControl/AckermannAttControl.hpp"
|
||||
#include "AckermannVelControl/AckermannVelControl.hpp"
|
||||
#include "AckermannPosControl/AckermannPosControl.hpp"
|
||||
#include "DriveModes/AutoMode/AutoMode.hpp"
|
||||
#include "DriveModes/ManualMode/ManualMode.hpp"
|
||||
#include "DriveModes/OffboardMode/OffboardMode.hpp"
|
||||
|
||||
class RoverAckermann : public ModuleBase<RoverAckermann>, public ModuleParams,
|
||||
public px4::ScheduledWorkItem
|
||||
@ -103,91 +91,7 @@ private:
|
||||
void Run() override;
|
||||
|
||||
/**
|
||||
* @brief Handle manual control
|
||||
*/
|
||||
void manualControl();
|
||||
|
||||
/**
|
||||
* @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint.
|
||||
*/
|
||||
void manualManualMode();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverThrottleSetpoint and RoverRateSetpoint from manualControlSetpoint.
|
||||
*/
|
||||
void manualAcroMode();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverThrottleSetpoint and RoverAttitudeSetpoint from manualControlSetpoint.
|
||||
*/
|
||||
void manualStabMode();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverVelocitySetpoint from manualControlSetpoint.
|
||||
*/
|
||||
void manualPositionMode();
|
||||
|
||||
/**
|
||||
* @brief Generate and publish roverVelocitySetpoint from positionSetpointTriplet.
|
||||
*/
|
||||
void autoPositionMode();
|
||||
|
||||
/**
|
||||
* @brief Update global/NED waypoint coordinates and acceptance radius.
|
||||
*/
|
||||
void autoUpdateWaypointsAndAcceptanceRadius();
|
||||
|
||||
/**
|
||||
* @brief Publish the acceptance radius for current waypoint based on the angle between a line segment
|
||||
* from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle.
|
||||
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param default_acceptance_radius Default acceptance radius for waypoints [m].
|
||||
* @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-].
|
||||
* @param acceptance_radius_max Maximum value for the acceptance radius [m].
|
||||
* @param wheel_base Rover wheelbase [m].
|
||||
* @param max_steer_angle Rover maximum steer angle [rad].
|
||||
* @return Updated acceptance radius [m].
|
||||
*/
|
||||
float autoUpdateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius,
|
||||
float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle);
|
||||
|
||||
/**
|
||||
* @brief Calculate the speed at which the rover should arrive at the current waypoint based on the upcoming corner.
|
||||
* @param cruising_speed Cruising speed [m/s].
|
||||
* @param miss_speed_min Minimum speed setpoint [m/s].
|
||||
* @param acc_rad Acceptance radius of the current waypoint [m].
|
||||
* @param curr_wp_type Type of the current waypoint.
|
||||
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param max_yaw_rate Maximum yaw rate setpoint [rad/s]
|
||||
* @return Speed setpoint [m/s].
|
||||
*/
|
||||
float autoArrivalSpeed(float cruising_speed, float miss_speed_min, float acc_rad, int curr_wp_type,
|
||||
float waypoint_transition_angle, float max_yaw_rate);
|
||||
|
||||
/**
|
||||
* @brief Calculate the cruising speed setpoint. During cornering the speed is restricted based on the radius of the corner.
|
||||
* @param cruising_speed Cruising speed [m/s].
|
||||
* @param miss_speed_min Minimum speed setpoint [m/s].
|
||||
* @param distance_to_prev_wp Distance to the previous waypoint [m].
|
||||
* @param distance_to_curr_wp Distance to the current waypoint [m].
|
||||
* @param acc_rad Acceptance radius of the current waypoint [m].
|
||||
* @param prev_acc_rad Acceptance radius of the previous waypoint [m].
|
||||
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
* @param max_yaw_rate Maximum yaw rate setpoint [rad/s]
|
||||
* @return Speed setpoint [m/s].
|
||||
*/
|
||||
float autoCruisingSpeed(float cruising_speed, float miss_speed_min, float distance_to_prev_wp,
|
||||
float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float waypoint_transition_angle,
|
||||
float prev_waypoint_transition_angle, float max_yaw_rate);
|
||||
|
||||
/**
|
||||
* @brief Translate trajectorySetpoint to roverSetpoints and publish them
|
||||
*/
|
||||
void offboardControl();
|
||||
|
||||
/**
|
||||
* @brief Update the controllers
|
||||
* @brief Update the active controllers.
|
||||
*/
|
||||
void updateControllers();
|
||||
|
||||
@ -206,70 +110,23 @@ private:
|
||||
void reset();
|
||||
|
||||
// uORB subscriptions
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
|
||||
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
|
||||
vehicle_control_mode_s _vehicle_control_mode{};
|
||||
|
||||
// uORB publications
|
||||
uORB::Publication<rover_velocity_setpoint_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)};
|
||||
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<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
|
||||
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
|
||||
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
|
||||
|
||||
// Class instances
|
||||
AckermannActControl _ackermann_act_control{this};
|
||||
AckermannRateControl _ackermann_rate_control{this};
|
||||
AckermannAttControl _ackermann_att_control{this};
|
||||
AckermannVelControl _ackermann_vel_control{this};
|
||||
AckermannPosControl _ackermann_pos_control{this};
|
||||
AutoMode _auto_mode{this};
|
||||
ManualMode _manual_mode{this};
|
||||
OffboardMode _offboard_mode{this};
|
||||
|
||||
// Variables
|
||||
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
|
||||
Quatf _vehicle_attitude_quaternion{};
|
||||
float _max_yaw_rate{NAN};
|
||||
float _vehicle_yaw{NAN};
|
||||
float _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base.
|
||||
int _nav_state{0}; // Navigation state of the vehicle
|
||||
bool _sanity_checks_passed{true}; // True if checks for all active controllers pass
|
||||
bool _was_armed{false}; // True if the vehicle was armed before the last reset
|
||||
|
||||
// Auto Mode Variables
|
||||
Vector2f _curr_wp_ned{};
|
||||
Vector2f _prev_wp_ned{};
|
||||
Vector2f _next_wp_ned{};
|
||||
float _acceptance_radius{0.5f};
|
||||
float _prev_acceptance_radius{0.5f};
|
||||
float _cruising_speed{0.f};
|
||||
float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
|
||||
int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
|
||||
|
||||
// Manual Mode Variables
|
||||
Vector2f _pos_ctl_course_direction{NAN, NAN};
|
||||
Vector2f _pos_ctl_start_position_ned{NAN, NAN};
|
||||
Vector2f _curr_pos_ned{NAN, NAN};
|
||||
float _stab_yaw_setpoint{NAN};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
|
||||
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
|
||||
(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,
|
||||
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
|
||||
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
|
||||
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
|
||||
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
|
||||
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain
|
||||
)
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user