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

StandardVTOL: explicitly stop forward motor with zero thrust

This commit is contained in:
Matthias Grob 2023-04-26 19:36:07 +02:00
parent 6b7b34a71a
commit 5568afbb12
2 changed files with 19 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
View File

@ -53,7 +53,8 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
configuration.selected_matrix = 0;
_rotors.enablePropellerTorqueNonUpwards(false);
const bool mc_rotors_added_successfully = _rotors.addActuators(configuration);
_mc_motors_mask = _rotors.getUpwardsMotors();
_upwards_motors_mask = _rotors.getUpwardsMotors();
_forwards_motors_mask = _rotors.getForwardsMotors();
// Control Surfaces
configuration.selected_matrix = 1;
@ -83,6 +84,15 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt,
}
}
void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
if (matrix_index == 0) {
stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
}
}
void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
{
if (_flight_phase == flight_phase) {
@ -94,13 +104,13 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight
// update stopped motors
switch (flight_phase) {
case FlightPhase::FORWARD_FLIGHT:
_stopped_motors_mask = _mc_motors_mask;
_stopped_motors_mask |= _upwards_motors_mask;
break;
case FlightPhase::HOVER_FLIGHT:
case FlightPhase::TRANSITION_FF_TO_HF:
case FlightPhase::TRANSITION_HF_TO_FF:
_stopped_motors_mask = 0;
_stopped_motors_mask &= ~_upwards_motors_mask;
break;
}
}

7
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
View File

@ -75,13 +75,18 @@ public:
void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void setFlightPhase(const FlightPhase &flight_phase) override;
private:
ActuatorEffectivenessRotors _rotors;
ActuatorEffectivenessControlSurfaces _control_surfaces;
uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight)
uint32_t _upwards_motors_mask{};
uint32_t _forwards_motors_mask{};
int _first_control_surface_idx{0}; ///< applies to matrix 1