forked from Archive/PX4-Autopilot
mc_rate: compute control energy and publish to status msg
This commit is contained in:
parent
0a662ef22c
commit
d504b49695
|
@ -39,6 +39,7 @@ include(px4_list_make_absolute)
|
|||
set(msg_files
|
||||
actuator_armed.msg
|
||||
actuator_controls.msg
|
||||
actuator_controls_status.msg
|
||||
actuator_outputs.msg
|
||||
adc_report.msg
|
||||
airspeed.msg
|
||||
|
|
|
@ -0,0 +1,10 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint8 INDEX_ROLL = 0
|
||||
uint8 INDEX_PITCH = 1
|
||||
uint8 INDEX_YAW = 2
|
||||
uint8 INDEX_THROTTLE = 3
|
||||
|
||||
float32[4] control_power
|
||||
|
||||
# TOPICS actuator_controls_status actuator_controls_status_0 actuator_controls_status_1
|
|
@ -52,6 +52,7 @@ void LoggedTopics::add_default_topics()
|
|||
add_topic("actuator_controls_3", 100);
|
||||
add_topic("actuator_controls_4", 100);
|
||||
add_topic("actuator_controls_5", 100);
|
||||
add_topic("actuator_controls_status_0", 300);
|
||||
add_topic("airspeed", 1000);
|
||||
add_topic("airspeed_validated", 200);
|
||||
add_topic("camera_capture");
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -47,6 +47,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/battery_status.h>
|
||||
#include <uORB/topics/landing_gear.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
|
@ -87,6 +88,8 @@ private:
|
|||
*/
|
||||
void parameters_updated();
|
||||
|
||||
void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt);
|
||||
|
||||
RateControl _rate_control; ///< class for rate control calculations
|
||||
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||
|
@ -104,8 +107,9 @@ private:
|
|||
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
|
||||
|
||||
uORB::Publication<actuator_controls_s> _actuators_0_pub;
|
||||
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */
|
||||
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
|
||||
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_sp_pub{ORB_ID(vehicle_rates_setpoint)};
|
||||
|
||||
vehicle_control_mode_s _v_control_mode{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
@ -126,6 +130,9 @@ private:
|
|||
|
||||
int8_t _landing_gear{landing_gear_s::GEAR_DOWN};
|
||||
|
||||
float _energy_integration_time{0.0f};
|
||||
float _control_energy[4] {};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,
|
||||
|
|
Loading…
Reference in New Issue