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

VTOL: remove virtual actuator_controls and instead use virtual torque/thrust topics

Browse Source 
MC/FW rate controller and auto tuner: remove actuator_controls

AirshipAttControl: remove actuator_controls

MulticopterLandDetector: remove actuator_controls

mavlink streams vfr_hud and high_latency2: remove actuator_controls

RoverPositionController: remove actuator_controls

UUVAttitudeController: remove actuator_controls

battery: use length of thrust_setpoint for throttle compensation

VehicleMagnetometer: use length of thrust_setpoint for throttle compensation

Signed-off-by: Silvan Fuhrer
This commit is contained in:
Silvan Fuhrer
2023-03-16 11:55:45 +01:00
committed by Beat Küng
parent bcd6e7adee
commit b16f16598b
47 changed files with 332 additions and 549 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_rate_control/MulticopterRateControl.cpp
+29 -53
View File
@@ -46,7 +46,8 @@ using math::radians;
MulticopterRateControl::MulticopterRateControl(bool vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)),
_vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc) : ORB_ID(vehicle_torque_setpoint)),
_vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_mc) : ORB_ID(vehicle_thrust_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
@@ -221,21 +222,16 @@ MulticopterRateControl::Run()
rate_ctrl_status.timestamp = hrt_absolute_time();
_controller_status_pub.publish(rate_ctrl_status);
// publish actuator controls
actuator_controls_s actuators{};
actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint(2)) ? -_thrust_setpoint(
2) : 0.0f;
actuators.timestamp_sample = angular_velocity.timestamp_sample;
// publish thrust and torque setpoints
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
vehicle_torque_setpoint_s vehicle_torque_setpoint{};
if (!_vehicle_status.is_vtol) {
publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
}
_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.f;
vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.f;
vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.f;
// scale effort by battery status if enabled
// scale setpoints by battery status if enabled
if (_param_mc_bat_scale_en.get()) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status;
@@ -245,55 +241,35 @@ MulticopterRateControl::Run()
}
}
if (_battery_status_scale > 0.0f) {
for (int i = 0; i < 4; i++) {
actuators.control[i] *= _battery_status_scale;
if (_battery_status_scale > 0.f) {
for (int i = 0; i < 3; i++) {
vehicle_thrust_setpoint.xyz[i] = math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f);
vehicle_torque_setpoint.xyz[i] = math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f);
}
}
}
actuators.timestamp = hrt_absolute_time();
_actuator_controls_0_pub.publish(actuators);
vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
updateActuatorControlsStatus(actuators, dt);
vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
vehicle_torque_setpoint.timestamp = hrt_absolute_time();
_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
updateActuatorControlsStatus(vehicle_torque_setpoint, dt);
} 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();
_actuator_controls_0_pub.publish(actuators);
}
}
}
perf_end(_loop_perf);
}
void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime &timestamp_sample)
void MulticopterRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint,
float dt)
{
vehicle_torque_setpoint_s vehicle_torque_setpoint{};
vehicle_torque_setpoint.timestamp_sample = timestamp_sample;
vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
vehicle_torque_setpoint.timestamp = hrt_absolute_time();
_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
}
void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
{
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
vehicle_thrust_setpoint.timestamp_sample = timestamp_sample;
_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
}
void MulticopterRateControl::updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt)
{
for (int i = 0; i < 4; i++) {
_control_energy[i] += actuators.control[i] * actuators.control[i] * dt;
for (int i = 0; i < 3; i++) {
_control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt;
}
_energy_integration_time += dt;
@@ -301,14 +277,14 @@ void MulticopterRateControl::updateActuatorControlsStatus(const actuator_control
if (_energy_integration_time > 500e-3f) {
actuator_controls_status_s status;
status.timestamp = actuators.timestamp;
status.timestamp = vehicle_torque_setpoint.timestamp;
for (int i = 0; i < 4; i++) {
for (int i = 0; i < 3; i++) {
status.control_power[i] = _control_energy[i] / _energy_integration_time;
_control_energy[i] = 0.f;
}
_actuator_controls_status_0_pub.publish(status);
_actuator_controls_status_pub.publish(status);
_energy_integration_time = 0.f;
}
}
src/modules/mc_rate_control/MulticopterRateControl.hpp
+4 -9
View File
@@ -46,7 +46,6 @@
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_allocator_status.h>
@@ -88,10 +87,7 @@ private:
*/
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 hrt_abstime &timestamp_sample);
void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt);
RateControl _rate_control; ///< class for rate control calculations
@@ -107,12 +103,11 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
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::Publication<actuator_controls_status_s> _actuator_controls_status_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> _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)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub;
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub;
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _vehicle_status{};