fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 15:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_rate: compute control energy and publish to status msg

Browse Source
This commit is contained in:
bresch
2021-09-29 11:12:38 +02:00
committed by Daniel Agar
parent 0a662ef22c
commit d504b49695
5 changed files with 46 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_rate_control/MulticopterRateControl.cpp
+25
View File
@@ -256,6 +256,8 @@ MulticopterRateControl::Run()
actuators.timestamp = hrt_absolute_time();
_actuators_0_pub.publish(actuators);
updateActuatorControlsStatus(actuators, dt);
} else if (_v_control_mode.flag_control_termination_enabled) {
if (!_vehicle_status.is_vtol) {
// publish actuator controls
@@ -269,6 +271,29 @@ MulticopterRateControl::Run()
perf_end(_loop_perf);
}
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;
}
_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_0_pub.publish(status);
_energy_integration_time = 0.f;
}
}
int MulticopterRateControl::task_spawn(int argc, char *argv[])
{
bool vtol = false;