Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

fw att: inject system identification signal to controller

Browse Source
This commit is contained in:
bresch
2021-10-01 17:17:25 +02:00
committed by Mathieu Bresciani
parent 6af0856558
commit 95e2941b17
2 changed files with 20 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+18 -3
View File
@@ -519,15 +519,30 @@ void FixedwingAttitudeControl::Run()
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
const hrt_abstime now = hrt_absolute_time();
autotune_attitude_control_status_s pid_autotune;
matrix::Vector3f bodyrate_ff;
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)) {
bodyrate_ff = matrix::Vector3f(pid_autotune.rate_sp);
}
}
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_euler_rate(dt, control_input);
float roll_u = _roll_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(0));
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
}
float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input);
float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(1));
_actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
@@ -540,7 +555,7 @@ void FixedwingAttitudeControl::Run()
yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input);
} else {
yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input);
yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2));
}
_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+2
View File
@@ -55,6 +55,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_status.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/autotune_attitude_control_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
@@ -101,6 +102,7 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */
uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */