dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

DifferentialDrive: move spoolup consideration to the main module

This commit is contained in:
Matthias Grob 2024-02-12 13:17:49 +01:00
parent bef694f9ba
commit 1998f54ea6
4 changed files with 6 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/differential_drive/DifferentialDrive.cpp
View File

@ -82,7 +82,6 @@ void DifferentialDrive::Run()
vehicle_control_mode_s vehicle_control_mode{}; vehicle_control_mode_s vehicle_control_mode{};
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
_differential_drive_kinematics.setArmed(vehicle_control_mode.flag_armed);
_manual_driving = vehicle_control_mode.flag_control_manual_enabled; _manual_driving = vehicle_control_mode.flag_control_manual_enabled;
_mission_driving = vehicle_control_mode.flag_control_auto_enabled; _mission_driving = vehicle_control_mode.flag_control_auto_enabled;
} }
@ -92,6 +91,9 @@ void DifferentialDrive::Run()
vehicle_status_s vehicle_status{}; vehicle_status_s vehicle_status{};
if (_vehicle_status_sub.copy(&vehicle_status)) { if (_vehicle_status_sub.copy(&vehicle_status)) {
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s);
_differential_drive_kinematics.setArmed(spooled_up);
_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO); _acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
} }
} }

3
src/modules/differential_drive/DifferentialDrive.hpp
View File

@ -99,6 +99,7 @@ private:
(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale, (ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base, (ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
(ParamFloat<px4::params::RDD_WHEEL_SPEED>) _param_rdd_wheel_speed, (ParamFloat<px4::params::RDD_WHEEL_SPEED>) _param_rdd_wheel_speed,
(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius (ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
) )
}; };

17
src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp
View File

@ -34,7 +34,6 @@
#include "DifferentialDriveControl.hpp" #include "DifferentialDriveControl.hpp"
using namespace matrix; using namespace matrix;
using namespace time_literals;
DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent) DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent)
{ {
@ -90,16 +89,6 @@ void DifferentialDriveControl::control(float dt)
} }
} }
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.copy(&vehicle_status)) {
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
_spooled_up = armed && hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s;
}
}
_differential_drive_setpoint_sub.update(&_differential_drive_setpoint); _differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
// PID to reach setpoint using control_output // PID to reach setpoint using control_output
@ -115,12 +104,6 @@ void DifferentialDriveControl::control(float dt)
pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt); pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
} }
if (!_spooled_up) {
differential_drive_control_output.speed = 0.0f;
differential_drive_control_output.yaw_rate = 0.0f;
}
differential_drive_control_output.timestamp = hrt_absolute_time(); differential_drive_control_output.timestamp = hrt_absolute_time();
_differential_drive_control_output_pub.publish(differential_drive_control_output); _differential_drive_control_output_pub.publish(differential_drive_control_output);
} }

8
src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp
View File

@ -48,7 +48,6 @@
#include <uORB/topics/vehicle_angular_velocity.h> #include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
class DifferentialDriveControl : public ModuleParams class DifferentialDriveControl : public ModuleParams
{ {
@ -68,7 +67,6 @@ private:
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)}; uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};
@ -84,14 +82,10 @@ private:
PID_t _pid_angular_velocity; ///< The PID controller for yaw rate. PID_t _pid_angular_velocity; ///< The PID controller for yaw rate.
PID_t _pid_speed; ///< The PID controller for velocity. PID_t _pid_speed; ///< The PID controller for velocity.
bool _spooled_up{false};
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed, (ParamFloat<px4::params::RDD_P_SPEED>) _param_rdd_p_gain_speed,
(ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed, (ParamFloat<px4::params::RDD_I_SPEED>) _param_rdd_i_gain_speed,
(ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity, (ParamFloat<px4::params::RDD_P_ANG_VEL>) _param_rdd_p_gain_angular_velocity,
(ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity, (ParamFloat<px4::params::RDD_I_ANG_VEL>) _param_rdd_i_gain_angular_velocity
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time
) )
}; };