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

hacks to make unified effectiveness matrix work

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2022-07-26 15:45:22 +03:00
parent 8726698afd
commit 45c8b64810
9 changed files with 59 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol
+4 -4
View File
@@ -12,8 +12,8 @@
param set-default FD_ACT_EN 0
param set-default FD_ACT_MOT_TOUT 500
# param set-default SYS_CTRL_ALLOC 1
param set-default CA_AIRFRAME 2
param set-default SYS_CTRL_ALLOC 1
param set-default CA_AIRFRAME 11
param set-default CA_ROTOR_COUNT 5
param set-default CA_ROTOR0_PX 0.1515
@@ -84,5 +84,5 @@ param set-default VT_FW_MOT_OFFID 1234
param set-default VT_B_TRANS_DUR 8
param set-default VT_TYPE 2
set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix
set MIXER custom
#set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix
set MIXER skip
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
+12
View File
@@ -63,6 +63,7 @@ enum class EffectivenessUpdateReason {
NO_EXTERNAL_UPDATE = 0,
CONFIGURATION_UPDATE = 1,
MOTOR_ACTIVATION_UPDATE = 2,
AIRSPEED_CHANGED = 3
};
@@ -133,6 +134,16 @@ public:
_flight_phase = flight_phase;
}
/**
* Set the equivalent airspeed
*
* @param Equivalent airspeed
*/
virtual void setEquivalentAirspeed(const float airspeed)
{
_airspeed_equivalent = airspeed;
}
/**
* Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES.
* This is expected to stay constant.
@@ -198,4 +209,5 @@ public:
protected:
FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase
float _airspeed_equivalent{NAN};
};
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp
+7 -1
View File
@@ -34,6 +34,7 @@
#include <px4_platform_common/log.h>
#include "ActuatorEffectivenessControlSurfaces.hpp"
#include <lib/mathlib/mathlib.h>
using namespace matrix;
@@ -127,7 +128,12 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
bool ActuatorEffectivenessControlSurfaces::addActuators(Configuration &configuration)
{
for (int i = 0; i < _count; i++) {
int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, _params[i].torque, Vector3f{});
const float airspeed_trim = 15.0f;
_airspeed_equivalent = math::max(_airspeed_equivalent, 3.0f);
float airspeed_scaling = _airspeed_equivalent / airspeed_trim;
airspeed_scaling = math::constrain(airspeed_scaling, 0.001f, 1000.0f);
Vector3f actual_torque = _params[i].torque * airspeed_scaling * airspeed_scaling;
int actuator_idx = configuration.addActuator(ActuatorType::SERVOS, actual_torque, Vector3f{});
if (actuator_idx >= 0) {
configuration.trim[configuration.selected_matrix](actuator_idx) = _params[i].trim;
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
+1 -1
View File
@@ -89,7 +89,7 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight
// update stopped motors
switch (flight_phase) {
case FlightPhase::FORWARD_FLIGHT:
_stopped_motors = _mc_motors_mask;
//_stopped_motors = _mc_motors_mask;
break;
case FlightPhase::HOVER_FLIGHT:
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.cpp
+6 -20
View File
@@ -50,34 +50,20 @@ bool ActuatorEffectivenessStandardVTOLUnified::getEffectivenessMatrix(Configurat
return false;
}
// TODO: combine _rotors and _control_surfaces into one matrix
// TODO: airspeed
// _rotors.addActuators(configuration);
//
// int num_actuators = _rotors.computeEffectivenessMatrix(_rotors.geometry(), configuration.effectiveness_matrices[0],
// configuration.num_actuators_matrix[0]);
// configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators);
if (external_update == EffectivenessUpdateReason::AIRSPEED_CHANGED) {
//printf("Updating due to airspeed %.2f\n", (double)_airspeed_equivalent);
_control_surfaces.setEquivalentAirspeed(_airspeed_equivalent);
}
// Motors
configuration.selected_matrix = 0;
_rotors.enablePropellerTorqueNonUpwards(false);
const bool mc_rotors_added_successfully = _rotors.addActuators(configuration);
_mc_motors_mask = _rotors.getUpwardsMotors();
// Control Surfaces
configuration.selected_matrix = 1;
configuration.selected_matrix = 0;
_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
return (mc_rotors_added_successfully && surfaces_added_successfully);
@@ -109,7 +95,7 @@ void ActuatorEffectivenessStandardVTOLUnified::setFlightPhase(const FlightPhase
// update stopped motors
switch (flight_phase) {
case FlightPhase::FORWARD_FLIGHT:
_stopped_motors = _mc_motors_mask;
//_stopped_motors = _mc_motors_mask;
break;
case FlightPhase::HOVER_FLIGHT:
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOLUnified.hpp
+4 -4
View File
@@ -60,15 +60,15 @@ public:
void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
{
static_assert(MAX_NUM_MATRICES >= 2, "expecting at least 2 matrices");
allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
//static_assert(MAX_NUM_MATRICES >= 2, "expecting at least 2 matrices");
allocation_method_out[0] = AllocationMethod::PSEUDO_INVERSE;
//allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE;
}
void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
{
normalize[0] = true;
normalize[1] = false;
//normalize[1] = false;
}
void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
src/modules/control_allocator/ControlAllocator.cpp
+7
View File
@@ -365,6 +365,13 @@ ControlAllocator::Run()
vehicle_torque_setpoint_s vehicle_torque_setpoint;
vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
if (_airspeed_sub.updated()) {
airspeed_validated_s airspeed{};
_airspeed_sub.copy(&airspeed);
_actuator_effectiveness->setEquivalentAirspeed(airspeed.calibrated_airspeed_m_s);
update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::AIRSPEED_CHANGED);
}
// Run allocator on torque changes
if (_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)) {
_torque_sp = matrix::Vector3f(vehicle_torque_setpoint.xyz);
src/modules/control_allocator/ControlAllocator.hpp
+2
View File
@@ -76,6 +76,7 @@
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/failure_detector_status.h>
#include <uORB/topics/airspeed_validated.h>
class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
{
@@ -186,6 +187,7 @@ private:
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed_validated)};
matrix::Vector3f _torque_sp;
matrix::Vector3f _thrust_sp;
src/modules/vtol_att_control/standard.cpp
+16 -10
View File
@@ -400,11 +400,17 @@ void Standard::fill_actuator_outputs()
_torque_setpoint_0->xyz[1] = mc_out[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_0->xyz[2] = mc_out[actuator_controls_s::INDEX_YAW];
_torque_setpoint_1->timestamp = hrt_absolute_time();
_torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_torque_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_1->xyz[1] = fw_out[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_1->xyz[2] = fw_out[actuator_controls_s::INDEX_YAW];
if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
_torque_setpoint_0->xyz[0] = fw_out[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_0->xyz[1] = fw_out[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_0->xyz[2] = fw_out[actuator_controls_s::INDEX_YAW];
}
// _torque_setpoint_1->timestamp = hrt_absolute_time();
// _torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
// _torque_setpoint_1->xyz[0] = fw_out[actuator_controls_s::INDEX_ROLL];
// _torque_setpoint_1->xyz[1] = fw_out[actuator_controls_s::INDEX_PITCH];
// _torque_setpoint_1->xyz[2] = fw_out[actuator_controls_s::INDEX_YAW];
_thrust_setpoint_0->timestamp = hrt_absolute_time();
_thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
@@ -412,11 +418,11 @@ void Standard::fill_actuator_outputs()
_thrust_setpoint_0->xyz[1] = 0.f;
_thrust_setpoint_0->xyz[2] = -mc_out[actuator_controls_s::INDEX_THROTTLE];
_thrust_setpoint_1->timestamp = hrt_absolute_time();
_thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_thrust_setpoint_1->xyz[0] = 0.f;
_thrust_setpoint_1->xyz[1] = 0.f;
_thrust_setpoint_1->xyz[2] = 0.f;
// _thrust_setpoint_1->timestamp = hrt_absolute_time();
// _thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
// _thrust_setpoint_1->xyz[0] = 0.f;
// _thrust_setpoint_1->xyz[1] = 0.f;
// _thrust_setpoint_1->xyz[2] = 0.f;
_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;