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

ackermann: refactor main files

Browse Source
This commit is contained in:
chfriedrich98
2024-08-05 15:46:41 +02:00
committed by chfriedrich98
parent cd231d0eed
commit 1af295f1a9
2 changed files with 107 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/RoverAckermann.cpp
+84 -66
View File
@@ -73,32 +73,15 @@ void RoverAckermann::Run()
return;
}
// uORB subscriber updates
if (_parameter_update_sub.updated()) {
updateParams();
}
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
_vehicle_status_sub.copy(&vehicle_status);
_nav_state = vehicle_status.nav_state;
_armed = vehicle_status.arming_state == 2;
}
if (_local_position_sub.updated()) {
vehicle_local_position_s local_position{};
_local_position_sub.copy(&local_position);
const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
_actual_speed = rover_velocity.norm();
}
updateSubscriptions();
// Timestamps
hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
// Generate motor setpoints
if (_armed) {
// Navigation modes
switch (_nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
manual_control_setpoint_s manual_control_setpoint{};
@@ -118,62 +101,97 @@ void RoverAckermann::Run()
default: // Unimplemented nav states will stop the rover
_motor_setpoint.steering = 0.f;
_motor_setpoint.throttle = 0.f;
_throttle_with_accel_limit.setForcedValue(0.f);
_steering_with_rate_limit.setForcedValue(0.f);
break;
}
// Sanitize actuator commands
if (!PX4_ISFINITE(_motor_setpoint.steering)) {
_motor_setpoint.steering = 0.f;
}
if (!PX4_ISFINITE(_motor_setpoint.throttle)) {
_motor_setpoint.throttle = 0.f;
}
// Acceleration slew rate
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
&& fabsf(_motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) {
_throttle_with_accel_limit.update(_motor_setpoint.throttle, dt);
} else {
_throttle_with_accel_limit.setForcedValue(_motor_setpoint.throttle);
}
// Steering slew rate
if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
_steering_with_rate_limit.update(_motor_setpoint.steering, dt);
} else {
_steering_with_rate_limit.setForcedValue(_motor_setpoint.steering);
}
// Publish rover ackermann status (logging)
rover_ackermann_status_s rover_ackermann_status{};
rover_ackermann_status.timestamp = _timestamp;
rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle;
rover_ackermann_status.steering_setpoint = _motor_setpoint.steering;
rover_ackermann_status.actual_speed = _actual_speed;
_rover_ackermann_status_pub.publish(rover_ackermann_status);
// Publish to motor
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
actuator_motors.control[0] = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
// Publish to servo
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f);
actuator_servos.timestamp = _timestamp;
_actuator_servos_pub.publish(actuator_servos);
} else { // Reset on disarm
_motor_setpoint.steering = 0.f;
_motor_setpoint.throttle = 0.f;
_throttle_with_accel_limit.setForcedValue(0.f);
_steering_with_rate_limit.setForcedValue(0.f);
}
publishMotorSetpoints(applySlewRates(_motor_setpoint, dt));
}
void RoverAckermann::updateSubscriptions()
{
if (_parameter_update_sub.updated()) {
updateParams();
}
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
_vehicle_status_sub.copy(&vehicle_status);
_nav_state = vehicle_status.nav_state;
_armed = vehicle_status.arming_state == 2;
}
if (_local_position_sub.updated()) {
vehicle_local_position_s local_position{};
_local_position_sub.copy(&local_position);
const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
_actual_speed = rover_velocity.norm();
}
}
motor_setpoint_struct RoverAckermann::applySlewRates(motor_setpoint_struct motor_setpoint, const float dt)
{
// Sanitize actuator commands
if (!PX4_ISFINITE(motor_setpoint.steering)) {
motor_setpoint.steering = 0.f;
}
if (!PX4_ISFINITE(motor_setpoint.throttle)) {
motor_setpoint.throttle = 0.f;
}
// Acceleration slew rate
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
&& fabsf(motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) {
_throttle_with_accel_limit.update(motor_setpoint.throttle, dt);
} else {
_throttle_with_accel_limit.setForcedValue(motor_setpoint.throttle);
}
// Steering slew rate
if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
_steering_with_rate_limit.update(motor_setpoint.steering, dt);
} else {
_steering_with_rate_limit.setForcedValue(motor_setpoint.steering);
}
motor_setpoint_struct motor_setpoint_temp{};
motor_setpoint_temp.steering = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f);
motor_setpoint_temp.throttle = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f);
return motor_setpoint_temp;
}
void RoverAckermann::publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates)
{
// Publish rover Ackermann status (logging)
rover_ackermann_status_s rover_ackermann_status{};
rover_ackermann_status.timestamp = _timestamp;
rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle;
rover_ackermann_status.steering_setpoint = _motor_setpoint.steering;
rover_ackermann_status.actual_speed = _actual_speed;
_rover_ackermann_status_pub.publish(rover_ackermann_status);
// Publish to motor
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get();
actuator_motors.control[0] = motor_setpoint_with_slew_rates.throttle;
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
// Publish to servo
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = motor_setpoint_with_slew_rates.steering;
actuator_servos.timestamp = _timestamp;
_actuator_servos_pub.publish(actuator_servos);
}
int RoverAckermann::task_spawn(int argc, char *argv[])
src/modules/rover_ackermann/RoverAckermann.hpp
+23 -2
View File
@@ -59,6 +59,7 @@
// Local includes
#include "RoverAckermannGuidance/RoverAckermannGuidance.hpp"
using motor_setpoint_struct = RoverAckermannGuidance::motor_setpoint;
using namespace time_literals;
@@ -83,12 +84,32 @@ public:
bool init();
protected:
void updateParams() override;
private:
void Run() override;
/**
* @brief Update uORB subscriptions.
*/
void updateSubscriptions();
/**
* @brief Apply slew rates to motor setpoints.
* @param motor_setpoint Normalized steering and throttle setpoints.
* @param dt Time since last update [s].
* @return Motor setpoint with applied slew rates.
*/
motor_setpoint_struct applySlewRates(motor_setpoint_struct motor_setpoint, float dt);
/**
* @brief Publish motor setpoints to ActuatorMotors/ActuatorServos and logging values to RoverAckermannStatus.
* @param motor_setpoint_with_slew_rate Normalized motor_setpoint with applied slew rates.
*/
void publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates);
// uORB subscriptions
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
@@ -100,12 +121,12 @@ private:
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
uORB::Publication<rover_ackermann_status_s> _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)};
// Instances
// Class instances
RoverAckermannGuidance _ackermann_guidance{this};
// Variables
int _nav_state{0};
RoverAckermannGuidance::motor_setpoint _motor_setpoint;
motor_setpoint_struct _motor_setpoint;
hrt_abstime _timestamp{0};
float _actual_speed{0.f};
SlewRate<float> _steering_with_rate_limit{0.f};