fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 18:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

preflight check: format

Browse Source
This commit is contained in:
Balduin
2025-02-20 09:12:15 +01:00
parent 9654ac6b20
commit 6e6df2e47e
2 changed files with 12 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+10 -3
View File
@@ -451,6 +451,7 @@ ControlAllocator::Run()
_control_allocation[i]->allocate(); _control_allocation[i]->allocate();
bool is_tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL; bool is_tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
if (_preflight_check_running && is_tiltrotor) { if (_preflight_check_running && is_tiltrotor) {
float preflight_check_tilt_sp = preflight_check_get_tilt_control(); float preflight_check_tilt_sp = preflight_check_get_tilt_control();
_actuator_effectiveness->setBypassTiltrotorControls(true, preflight_check_tilt_sp, 0.0f); _actuator_effectiveness->setBypassTiltrotorControls(true, preflight_check_tilt_sp, 0.0f);
@@ -490,7 +491,8 @@ ControlAllocator::Run()
perf_end(_loop_perf); perf_end(_loop_perf);
} }
void ControlAllocator::preflight_check_update_state() { void ControlAllocator::preflight_check_update_state()
{
// bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr; // bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL; bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
@@ -499,6 +501,7 @@ void ControlAllocator::preflight_check_update_state() {
// negative torque setpoints. // negative torque setpoints.
int n_axes = 3; int n_axes = 3;
if (tiltrotor) { if (tiltrotor) {
n_axes = 4; n_axes = 4;
} }
@@ -506,6 +509,7 @@ void ControlAllocator::preflight_check_update_state() {
int max_phase = 2 * n_axes; int max_phase = 2 * n_axes;
hrt_abstime now = hrt_absolute_time(); hrt_abstime now = hrt_absolute_time();
if (now - _last_preflight_check_update >= 500_ms) { if (now - _last_preflight_check_update >= 500_ms) {
_preflight_check_phase++; _preflight_check_phase++;
_preflight_check_phase %= max_phase; // or quit once we did the whole thing once? _preflight_check_phase %= max_phase; // or quit once we did the whole thing once?
@@ -513,7 +517,9 @@ void ControlAllocator::preflight_check_update_state() {
} }
} }
void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]) { void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES>
(&c)[ActuatorEffectiveness::MAX_NUM_MATRICES])
{
int axis = _preflight_check_phase / 2; int axis = _preflight_check_phase / 2;
int negative = _preflight_check_phase % 2; int negative = _preflight_check_phase % 2;
@@ -535,7 +541,8 @@ void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float,
} }
float ControlAllocator::preflight_check_get_tilt_control() { float ControlAllocator::preflight_check_get_tilt_control()
{
int axis = _preflight_check_phase / 2; int axis = _preflight_check_phase / 2;
int negative = _preflight_check_phase % 2; int negative = _preflight_check_phase % 2;
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+2 -1
View File
@@ -124,7 +124,8 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt
} }
void ActuatorEffectivenessTiltrotorVTOL::setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust) void ActuatorEffectivenessTiltrotorVTOL::setBypassTiltrotorControls(bool bypass, float collective_tilt,
float collective_thrust)
{ {
_bypass_tiltrotor_controls = bypass; _bypass_tiltrotor_controls = bypass;
_collective_tilt_normalized_setpoint = collective_tilt; _collective_tilt_normalized_setpoint = collective_tilt;