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

differential: Add stabilized and position mode (#23669)

Browse Source 
* differential: add position and stabilized mode

* differential: add hardcoded stick input deadzones
This commit is contained in:
chfriedrich98
2024-09-16 12:09:51 +02:00
committed by GitHub
parent 81747f35bb
commit 2fd4150b38
9 changed files with 136 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_differential/RoverDifferential.cpp
+91 -1
View File
@@ -99,12 +99,94 @@ void RoverDifferential::Run()
} break;
case vehicle_status_s::NAVIGATION_STATE_STAB: {
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
rover_differential_setpoint_s rover_differential_setpoint{};
rover_differential_setpoint.timestamp = timestamp;
rover_differential_setpoint.forward_speed_setpoint = NAN;
rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_setpoint = NAN;
if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
|| fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control
_yaw_ctl = false;
} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
if (!_yaw_ctl) {
_stab_desired_yaw = _vehicle_yaw;
_yaw_ctl = true;
}
rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw;
rover_differential_setpoint.yaw_rate_setpoint = NAN;
}
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
}
} break;
case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
rover_differential_setpoint_s rover_differential_setpoint{};
rover_differential_setpoint.timestamp = timestamp;
rover_differential_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get());
rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_setpoint = NAN;
if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
|| fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
_yaw_ctl = false;
} else { // Course control if the yaw rate input is zero (keep driving on a straight line)
if (!_yaw_ctl) {
_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
_pos_ctl_start_position_ned = _curr_pos_ned;
_yaw_ctl = true;
}
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(
rover_differential_setpoint.forward_speed_setpoint) *
vector_scaling * _pos_ctl_course_direction;
// Calculate yaw setpoint
const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned,
_pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed));
rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ?
yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards
rover_differential_setpoint.yaw_rate_setpoint = NAN;
}
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
}
} break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state);
break;
default: // Unimplemented nav states will stop the rover
_rover_differential_control.resetControllers();
_yaw_ctl = false;
rover_differential_setpoint_s rover_differential_setpoint{};
rover_differential_setpoint.forward_speed_setpoint = NAN;
rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f;
@@ -115,8 +197,9 @@ void RoverDifferential::Run()
break;
}
if (!_armed) { // Reset on disarm
if (!_armed) { // Reset when disarmed
_rover_differential_control.resetControllers();
_yaw_ctl = false;
}
_rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed);
@@ -135,6 +218,12 @@ void RoverDifferential::updateSubscriptions()
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
if (vehicle_status.nav_state != _nav_state) { // Reset on mode change
_rover_differential_control.resetControllers();
_yaw_ctl = false;
}
_nav_state = vehicle_status.nav_state;
_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
}
@@ -155,6 +244,7 @@ void RoverDifferential::updateSubscriptions()
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);
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
src/modules/rover_differential/RoverDifferential.hpp
+14 -1
View File
@@ -39,6 +39,7 @@
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/pure_pursuit/PurePursuit.hpp>
// uORB includes
#include <uORB/Publication.hpp>
@@ -103,8 +104,10 @@ private:
// Instances
RoverDifferentialGuidance _rover_differential_guidance{this};
RoverDifferentialControl _rover_differential_control{this};
PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
// Variables
Vector2f _curr_pos_ned{};
matrix::Quatf _vehicle_attitude_quaternion{};
float _vehicle_yaw_rate{0.f};
float _vehicle_forward_speed{0.f};
@@ -112,6 +115,10 @@ private:
float _max_yaw_rate{0.f};
int _nav_state{0};
bool _armed{false};
bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode
float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode
Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode
Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode
// Thresholds to avoid moving at rest due to measurement noise
static constexpr float YAW_RATE_THRESHOLD =
@@ -119,8 +126,14 @@ private:
static constexpr float SPEED_THRESHOLD =
0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
// Stick input deadzone
static constexpr float STICK_DEADZONE =
0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_MAN_YAW_SCALE>) _param_rd_man_yaw_scale,
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
)
};
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
+1 -1
View File
@@ -216,7 +216,7 @@ void RoverDifferentialGuidance::updateWaypoints()
// Waypoint cruising speed
if (position_setpoint_triplet.current.cruising_speed > 0.f) {
_max_forward_speed = position_setpoint_triplet.current.cruising_speed;
_max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get());
} else {
_max_forward_speed = _param_rd_miss_spd_def.get();
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
+1
View File
@@ -143,6 +143,7 @@ private:
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RD_MAX_JERK>) _param_rd_max_jerk,
(ParamFloat<px4::params::RD_MAX_ACCEL>) _param_rd_max_accel,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::RD_MISS_SPD_DEF>) _param_rd_miss_spd_def,
(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn
src/modules/rover_differential/module.yaml
+15 -1
View File
@@ -113,6 +113,19 @@ parameters:
decimal: 2
default: 0.5
RD_MAX_SPEED:
description:
short: Maximum speed setpoint
long: |
This parameter is used to cap desired forward speed and map controller inputs to desired speeds in Position mode.
type: float
unit: m/s
min: 0
max: 100
increment: 0.01
decimal: 2
default: 1
RD_MAX_THR_SPD:
description:
short: Speed the rover drives at maximum throttle
@@ -133,7 +146,8 @@ parameters:
description:
short: Maximum allowed yaw rate for the rover
long: |
This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode.
This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates
in Acro,Stabilized and Position mode.
type: float
unit: deg/s
min: 0.01