fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 19:13:34 +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);
}
}
}