dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_att: inject identification signal in att control output

This commit is contained in:
bresch 2021-09-29 11:11:01 +02:00 committed by Daniel Agar
parent 0498ee92d0
commit 9681f819a8
2 changed files with 16 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -47,6 +47,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/autotune_attitude_control_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
@ -104,6 +105,7 @@ private:
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */

15
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -316,13 +316,26 @@ MulticopterAttitudeControl::Run()
Vector3f rates_sp = _attitude_control.update(q);
const hrt_abstime now = hrt_absolute_time();
autotune_attitude_control_status_s pid_autotune;
if (_autotune_attitude_control_status_sub.copy(&pid_autotune)) {
if ((pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST)
&& ((now - pid_autotune.timestamp) < 1_s)) {
rates_sp += Vector3f(pid_autotune.rate_sp);
}
}
// publish rate setpoint
vehicle_rates_setpoint_s v_rates_sp{};
v_rates_sp.roll = rates_sp(0);
v_rates_sp.pitch = rates_sp(1);
v_rates_sp.yaw = rates_sp(2);
_thrust_setpoint_body.copyTo(v_rates_sp.thrust_body);
v_rates_sp.timestamp = hrt_absolute_time();
v_rates_sp.timestamp = now;
_v_rates_sp_pub.publish(v_rates_sp);
}