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

mac_att_control: heading lock in stabilized only after ekf final yaw alignment

Browse Source
This commit is contained in:
Matthias Grob 2023-02-02 14:53:43 +01:00 committed by Silvan Fuhrer
parent f0571de731
commit ce5cff55b7
2 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -50,6 +50,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
@ -96,10 +97,11 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
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_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};
@ -123,6 +125,7 @@ private:
hrt_abstime _last_attitude_setpoint{0};
bool _reset_yaw_sp{true};
bool _heading_good_for_control{true}; ///< initialized true to have heading lock when local position never published
bool _vehicle_type_rotary_wing{true};
bool _vtol{false};
bool _vtol_tailsitter{false};

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

@ -284,6 +284,14 @@ MulticopterAttitudeControl::Run()
}
}
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position;
if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
_heading_good_for_control = vehicle_local_position.heading_good_for_control;
}
}
bool attitude_setpoint_generated = false;
const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode);
@ -337,7 +345,7 @@ MulticopterAttitudeControl::Run()
// reset yaw setpoint during transitions, tailsitter.cpp generates
// attitude setpoint for the transition
_reset_yaw_sp = !attitude_setpoint_generated || (_vtol && _vtol_in_transition_mode);
_reset_yaw_sp = !attitude_setpoint_generated || !_heading_good_for_control || (_vtol && _vtol_in_transition_mode);
}
perf_end(_loop_perf);