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

compute and publish fixed-wing control power

Browse Source
This commit is contained in:
bresch 2021-10-01 17:16:21 +02:00 committed by Mathieu Bresciani
parent d84b0296d2
commit 8dfdb1e3db
2 changed files with 46 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -44,6 +44,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
_actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)),
_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
@ -642,6 +643,8 @@ void FixedwingAttitudeControl::Run()
_vcontrol_mode.flag_control_manual_enabled) {
_actuators_0_pub.publish(_actuators);
}
updateActuatorControlsStatus(dt);
}
perf_end(_loop_perf);
@ -712,6 +715,41 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
}
}
void FixedwingAttitudeControl::updateActuatorControlsStatus(float dt)
{
for (int i = 0; i < 4; i++) {
float control_signal;
if (i <= actuator_controls_status_s::INDEX_YAW) {
// We assume that the attitude is actuated by control surfaces
// consuming power only when they move
control_signal = _actuators.control[i] - _control_prev[i];
_control_prev[i] = _actuators.control[i];
} else {
control_signal = _actuators.control[i];
}
_control_energy[i] += control_signal * control_signal * dt;
}
_energy_integration_time += dt;
if (_energy_integration_time > 500e-3f) {
actuator_controls_status_s status;
status.timestamp = _actuators.timestamp;
for (int i = 0; i < 4; i++) {
status.control_power[i] = _control_energy[i] / _energy_integration_time;
_control_energy[i] = 0.f;
}
_actuator_controls_status_pub.publish(status);
_energy_integration_time = 0.f;
}
}
int FixedwingAttitudeControl::task_spawn(int argc, char *argv[])
{
bool vtol = false;

8
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -53,6 +53,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
@ -112,6 +113,7 @@ private:
uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
uORB::Publication<actuator_controls_s> _actuators_0_pub;
uORB::Publication<actuator_controls_status_s> _actuator_controls_status_pub;
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
@ -141,6 +143,10 @@ private:
bool _is_tailsitter{false};
float _energy_integration_time{0.0f};
float _control_energy[4] {};
float _control_prev[3] {};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
@ -217,6 +223,8 @@ private:
void control_flaps(const float dt);
void updateActuatorControlsStatus(float dt);
/**
* Update our local parameter cache.
*/