fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 14:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

preflight check: replace argument with setter

Browse Source 
this allows us to have *only* the setter in the base class
(ActuatorEffectiveness) with an empty implementation. Derived classes
can implement another implementation, or stay completely unchanged
w.r.t. previously. This may finally be the desired clean-ish OOP-ish
solution to this.
This commit is contained in:
Balduin
2025-02-19 17:06:53 +01:00
parent 10bcc52f13
commit 009c300216
23 changed files with 62 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp
+5
View File
@@ -101,3 +101,8 @@ void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_mo
}
}
}
void ActuatorEffectiveness::setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust)
{
// empty implementation to be overridden if needed.
}
src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp
+13 -1
View File
@@ -198,7 +198,7 @@ public:
*/
virtual 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, bool preflight_check_running) {}
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
/**
* Get a bitmask of motors to be stopped
@@ -219,6 +219,18 @@ public:
*/
virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);
/**
* If overridden by derived classes, optionally bypass the info usually
* contained in tiltrotor_extra_controls -- normalised collective thrust
* and tilt setpoints.
*
* Base class implementation is empty.
*
* @param bypass Flag indicating whether or not to use the other
* arguments to bypass setpoints
*/
virtual void setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust);
protected:
FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
uint32_t _stopped_motors_mask{0};
src/modules/control_allocator/ControlAllocator.cpp
+9 -26
View File
@@ -450,23 +450,19 @@ ControlAllocator::Run()
// Do allocation
_control_allocation[i]->allocate();
if (_preflight_check_running) {
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);
// alternative: specify these member variables in the general ActuatorEffectiveness,
// and just don't use them anywhere except TiltrotorVTOL
auto actuator_effectiveness_tiltrotor_vtol = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness);
} else {
_actuator_effectiveness->setBypassTiltrotorControls(false, 0.0f, 0.0f);
if (actuator_effectiveness_tiltrotor_vtol) {
float collective_tilt_sp = preflight_check_get_tilt_control();
actuator_effectiveness_tiltrotor_vtol->_collective_tilt_normalized_setpoint = collective_tilt_sp;
actuator_effectiveness_tiltrotor_vtol->_collective_thrust_normalized_setpoint = 0.0f;
}
}
_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax(),
_preflight_check_running);
_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
if (_has_slew_rate) {
_control_allocation[i]->applySlewRateLimit(dt);
@@ -494,23 +490,10 @@ ControlAllocator::Run()
perf_end(_loop_perf);
}
// void ControlAllocator::test_individual_control_surfaces() {
// goal here: modify actuation at the servo level.
// in here: small state machine cycling through servos (or taking info
// from outside about which servo to actuate)
// if test running: if enough time passed: go to next thing
// if last thing: test = not running
// elsewhere (probably Run()...)
// set test running if right message received
// if test running,
// }
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;
// cycle through roll, pitch, yaw, and for each one inject positive and
// negative torque setpoints.
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp
+1 -1
View File
@@ -61,7 +61,7 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration
void ActuatorEffectivenessCustom::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, bool preflight_check_running)
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
}
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp
+1 -1
View File
@@ -47,7 +47,7 @@ public:
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, bool preflight_check_running) override;
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
const char *name() const override { return "Custom"; }
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp
+1 -1
View File
@@ -63,7 +63,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
void ActuatorEffectivenessFixedWing::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, bool preflight_check_running)
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
}
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp
+1 -1
View File
@@ -53,7 +53,7 @@ public:
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, bool preflight_check_running) override;
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
private:
ActuatorEffectivenessRotors _rotors;
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+1 -1
View File
@@ -130,7 +130,7 @@ bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &conf
void ActuatorEffectivenessHelicopter::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, bool preflight_check_running)
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
_saturation_flags = {};
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+1 -1
View File
@@ -80,7 +80,7 @@ public:
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, bool preflight_check_running) override;
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
private:
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp
+1 -1
View File
@@ -104,7 +104,7 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio
void ActuatorEffectivenessHelicopterCoaxial::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, bool preflight_check_running)
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
_saturation_flags = {};
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp
+1 -1
View File
@@ -71,7 +71,7 @@ public:
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, bool preflight_check_running) override;
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
private:
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp
+1 -1
View File
@@ -78,7 +78,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
void ActuatorEffectivenessMCTilt::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, bool preflight_check_running)
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
actuator_sp += _tilt_offsets;
// TODO: dynamic matrix update
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp
+1 -1
View File
@@ -57,7 +57,7 @@ public:
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, bool preflight_check_running) override;
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
const char *name() const override { return "MC Tilt"; }
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp
+1 -1
View File
@@ -51,7 +51,7 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi
void ActuatorEffectivenessRoverAckermann::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, bool preflight_check_running)
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
}
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp
+1 -1
View File
@@ -45,7 +45,7 @@ public:
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, bool preflight_check_running) override;
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
const char *name() const override { return "Rover (Ackermann)"; }
private:
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
+1 -1
View File
@@ -85,7 +85,7 @@ 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, bool preflight_check_running)
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
if (matrix_index == 0) {
stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
+1 -1
View File
@@ -77,7 +77,7 @@ public:
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, bool preflight_check_running) override;
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void setFlightPhase(const FlightPhase &flight_phase) override;
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp
+1 -1
View File
@@ -90,7 +90,7 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d
void ActuatorEffectivenessTailsitterVTOL::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, bool preflight_check_running)
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
if (matrix_index == 0) {
stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp
+1 -1
View File
@@ -74,7 +74,7 @@ public:
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, bool preflight_check_running) override;
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
void setFlightPhase(const FlightPhase &flight_phase) override;
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+11 -4
View File
@@ -124,9 +124,16 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt
}
void ActuatorEffectivenessTiltrotorVTOL::setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust)
{
_bypass_tiltrotor_controls = bypass;
_collective_tilt_normalized_setpoint = collective_tilt;
_collective_thrust_normalized_setpoint = collective_thrust;
}
void ActuatorEffectivenessTiltrotorVTOL::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, bool preflight_check_running)
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
// apply tilt
@@ -141,12 +148,12 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
if (matrix_index == 0) {
tiltrotor_extra_controls_s tiltrotor_extra_controls;
if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || preflight_check_running) {
if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || _bypass_tiltrotor_controls) {
float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f;
float control_collective_thrust = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
if (preflight_check_running) {
if (_bypass_tiltrotor_controls) {
control_collective_tilt = _collective_tilt_normalized_setpoint * 2.f - 1.f;
control_collective_thrust = _collective_thrust_normalized_setpoint;
}
@@ -182,7 +189,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
// as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover)
if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT || preflight_check_running) {
if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT || _bypass_tiltrotor_controls) {
actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
} else {
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
+6 -3
View File
@@ -82,14 +82,13 @@ public:
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, bool preflight_check_running) override;
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
const char *name() const override { return "VTOL Tiltrotor"; }
void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
float _collective_tilt_normalized_setpoint{0.5f};
float _collective_thrust_normalized_setpoint{0.0f};
void setBypassTiltrotorControls(bool bypass, float collective_tilt, float collective_thrust) override;
protected:
bool _collective_tilt_updated{true};
@@ -117,6 +116,10 @@ protected:
uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};
bool _bypass_tiltrotor_controls{false};
float _collective_tilt_normalized_setpoint{0.5f};
float _collective_thrust_normalized_setpoint{0.0f};
private:
void updateParams() override;
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp
+1 -1
View File
@@ -57,7 +57,7 @@ bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configurati
void ActuatorEffectivenessUUV::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, bool preflight_check_running)
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
{
stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
}
src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp
+1 -1
View File
@@ -56,7 +56,7 @@ public:
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, bool preflight_check_running) override;
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
const char *name() const override { return "UUV"; }