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

Standardize variable naming and formatting across vehicle attitude controller files.

Browse Source
This commit is contained in:
mcsauder
2022-05-21 12:22:18 -04:00
committed by Daniel Agar
parent 422be90140
commit 6b0788ff46
12 changed files with 240 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_rate_control/MulticopterRateControl.cpp
+25 -28
View File
@@ -46,7 +46,7 @@ using math::radians;
MulticopterRateControl::MulticopterRateControl(bool vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)),
_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
@@ -138,7 +138,7 @@ MulticopterRateControl::Run()
const Vector3f rates{angular_velocity.xyz};
/* check for updates in other topics */
_v_control_mode_sub.update(&_v_control_mode);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected;
@@ -169,7 +169,10 @@ MulticopterRateControl::Run()
}
}
if (_v_control_mode.flag_control_manual_enabled && !_v_control_mode.flag_control_attitude_enabled) {
// use rates setpoint topic
vehicle_rates_setpoint_s vehicle_rates_setpoint{};
if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) {
// generate the rate setpoint from sticks
manual_control_setpoint_s manual_control_setpoint;
@@ -184,37 +187,31 @@ MulticopterRateControl::Run()
_thrust_setpoint = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
// publish rate setpoint
vehicle_rates_setpoint_s v_rates_setpoint{};
v_rates_setpoint.roll = _rates_setpoint(0);
v_rates_setpoint.pitch = _rates_setpoint(1);
v_rates_setpoint.yaw = _rates_setpoint(2);
v_rates_setpoint.thrust_body[0] = 0.0f;
v_rates_setpoint.thrust_body[1] = 0.0f;
v_rates_setpoint.thrust_body[2] = _thrust_setpoint;
v_rates_setpoint.timestamp = hrt_absolute_time();
vehicle_rates_setpoint.roll = _rates_setpoint(0);
vehicle_rates_setpoint.pitch = _rates_setpoint(1);
vehicle_rates_setpoint.yaw = _rates_setpoint(2);
vehicle_rates_setpoint.thrust_body[0] = 0.0f;
vehicle_rates_setpoint.thrust_body[1] = 0.0f;
vehicle_rates_setpoint.thrust_body[2] = _thrust_setpoint;
vehicle_rates_setpoint.timestamp = hrt_absolute_time();
_v_rates_setpoint_pub.publish(v_rates_setpoint);
_vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint);
}
} else {
// use rates setpoint topic
vehicle_rates_setpoint_s vehicle_rates_setpoint;
if (_v_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
if (_v_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) {
_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0);
_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2);
_thrust_setpoint = -vehicle_rates_setpoint.thrust_body[2];
}
} else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) {
_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0);
_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2);
_thrust_setpoint = -vehicle_rates_setpoint.thrust_body[2];
}
}
// run the rate controller
if (_v_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
if (_vehicle_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
// reset integral if disarmed
if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
_rate_control.resetIntegral();
}
@@ -281,16 +278,16 @@ MulticopterRateControl::Run()
}
actuators.timestamp = hrt_absolute_time();
_actuators_0_pub.publish(actuators);
_actuator_controls_0_pub.publish(actuators);
updateActuatorControlsStatus(actuators, dt);
} else if (_v_control_mode.flag_control_termination_enabled) {
} else if (_vehicle_control_mode.flag_control_termination_enabled) {
if (!_vehicle_status.is_vtol) {
// publish actuator controls
actuator_controls_s actuators{};
actuators.timestamp = hrt_absolute_time();
_actuators_0_pub.publish(actuators);
_actuator_controls_0_pub.publish(actuators);
}
}
}
src/modules/mc_rate_control/MulticopterRateControl.hpp
+18 -19
View File
@@ -89,60 +89,62 @@ private:
/**
* initialize some vectors/matrices from parameters
*/
void parameters_updated();
void parameters_updated();
void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt);
void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime &timestamp_sample);
RateControl _rate_control; ///< class for rate control calculations
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _v_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::Publication<actuator_controls_s> _actuators_0_pub;
uORB::Publication<actuator_controls_s> _actuator_controls_0_pub;
uORB::Publication<actuator_controls_status_s> _actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)};
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status)};
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_rates_setpoint_s> _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
orb_advert_t _mavlink_log_pub{nullptr};
vehicle_control_mode_s _v_control_mode{};
vehicle_status_s _vehicle_status{};
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _vehicle_status{};
bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */
bool _landed{true};
bool _maybe_landed{true};
float _battery_status_scale{0.0f};
hrt_abstime _last_run{0};
perf_counter_t _loop_perf; /**< loop duration performance counter */
// keep setpoint values between updates
matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */
matrix::Vector3f _rates_setpoint{};
float _battery_status_scale{0.0f};
float _thrust_setpoint{0.0f};
hrt_abstime _last_run{0};
int8_t _landing_gear{landing_gear_s::GEAR_DOWN};
float _energy_integration_time{0.0f};
float _control_energy[4] {};
int8_t _landing_gear{landing_gear_s::GEAR_DOWN};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,
@@ -170,16 +172,13 @@ private:
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
(ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,
(ParamFloat<px4::params::MC_ACRO_Y_MAX>) _param_mc_acro_y_max,
(ParamFloat<px4::params::MC_ACRO_EXPO>) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_EXPO>) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_EXPO_Y>) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */
(ParamBool<px4::params::MC_BAT_SCALE_EN>) _param_mc_bat_scale_en,
(ParamInt<px4::params::CBRK_RATE_CTRL>) _param_cbrk_rate_ctrl
)
matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */
};