fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -451,6 +451,7 @@ ControlAllocator::Run()
_control_allocation[i]->allocate();
bool is_tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
if (_preflight_check_running && is_tiltrotor) {
float preflight_check_tilt_sp = preflight_check_get_tilt_control();
_actuator_effectiveness->setBypassTiltrotorControls(true, preflight_check_tilt_sp, 0.0f);
@ -490,7 +491,8 @@ ControlAllocator::Run()
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 = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
@ -499,6 +501,7 @@ void ControlAllocator::preflight_check_update_state() {
// negative torque setpoints.
int n_axes = 3;
if (tiltrotor) {
n_axes = 4;
}
@ -506,6 +509,7 @@ void ControlAllocator::preflight_check_update_state() {
int max_phase = 2 * n_axes;
hrt_abstime now = hrt_absolute_time();
if (now - _last_preflight_check_update >= 500_ms) {
_preflight_check_phase++;
_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 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 negative = _preflight_check_phase % 2;

3
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
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;
_collective_tilt_normalized_setpoint = collective_tilt;