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

ControlAllocator: enable custom saturation logic to override default one 

Custom saturation logic currently implemented for Tiltrotor VTOL and Helicopter.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-10-31 11:09:00 +01:00 committed by Beat Küng
parent 2e20fb7f97
commit f44713e8a3
6 changed files with 78 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
View File

@ -199,10 +199,10 @@ public:
virtual uint32_t getStoppedMotors() const { return 0; }
/**
* Fill in the allocated and unallocated torque and thrust.
* Should return false if not filled in and the effectivenes matrix should be used instead
* Fill in the allocated and unallocated torque and thrust, customized by effectiveness type.
* Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
*/
virtual bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const { return false; }
virtual void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
protected:
FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase

15
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
View File

@ -217,48 +217,37 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit
}
}
bool ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const
void ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(int matrix_index,
control_allocator_status_s &status)
{
status.torque_setpoint_achieved = true;
status.thrust_setpoint_achieved = true;
// Note: the values '-1', '1' and '0' are just to indicate a negative,
// positive or no saturation to the rate controller. The actual magnitude is not used.
if (_saturation_flags.roll_pos) {
status.unallocated_torque[0] = 1.f;
status.torque_setpoint_achieved = false;
} else if (_saturation_flags.roll_neg) {
status.unallocated_torque[0] = -1.f;
status.torque_setpoint_achieved = false;
}
if (_saturation_flags.pitch_pos) {
status.unallocated_torque[1] = 1.f;
status.torque_setpoint_achieved = false;
} else if (_saturation_flags.pitch_neg) {
status.unallocated_torque[1] = -1.f;
status.torque_setpoint_achieved = false;
}
if (_saturation_flags.yaw_pos) {
status.unallocated_torque[2] = 1.f;
status.torque_setpoint_achieved = false;
} else if (_saturation_flags.yaw_neg) {
status.unallocated_torque[2] = -1.f;
status.torque_setpoint_achieved = false;
}
if (_saturation_flags.thrust_pos) {
status.unallocated_thrust[2] = 1.f;
status.thrust_setpoint_achieved = false;
} else if (_saturation_flags.thrust_neg) {
status.unallocated_thrust[2] = -1.f;
status.thrust_setpoint_achieved = false;
}
return true;
}

2
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
View File

@ -79,7 +79,7 @@ public:
ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const override;
void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
private:
float throttleSpoolupProgress();
bool mainMotorEnaged();

36
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
View File

@ -147,6 +147,21 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
}
}
}
// Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from
// the other axes (for now neglecting the case of 0 collective thrust), we set the saturation flags
// directly if the (normalized) yaw torque setpoint is outside of range (-1, 1).
if (matrix_index == 0 && _tilts.hasYawControl()) {
_yaw_tilt_saturation_flags.tilt_yaw_neg = false;
_yaw_tilt_saturation_flags.tilt_yaw_pos = false;
if (control_sp(2) < -1.f) {
_yaw_tilt_saturation_flags.tilt_yaw_neg = true;
} else if (control_sp(2) > 1.f) {
_yaw_tilt_saturation_flags.tilt_yaw_pos = true;
}
}
}
void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
@ -170,3 +185,24 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
break;
}
}
void ActuatorEffectivenessTiltrotorVTOL::getAllocatedAndUnallocatedControl(int matrix_index,
control_allocator_status_s &status)
{
// only handle matrix 0 (motors and tilts)
if (matrix_index == 1) {
return;
}
// Note: the values '-1', '1' and '0' are just to indicate a negative,
// positive or no saturation to the rate controller. The actual magnitude is not used.
if (_yaw_tilt_saturation_flags.tilt_yaw_pos) {
status.unallocated_torque[2] = 1.f;
} else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) {
status.unallocated_torque[2] = -1.f;
} else {
status.unallocated_torque[2] = 0.f;
}
}

10
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
View File

@ -81,6 +81,9 @@ public:
const char *name() const override { return "VTOL Tiltrotor"; }
uint32_t getStoppedMotors() const override { return _stopped_motors; }
void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
protected:
bool _collective_tilt_updated{true};
ActuatorEffectivenessRotors _mc_rotors;
@ -97,4 +100,11 @@ protected:
uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
struct YawTiltSaturationFlags {
bool tilt_yaw_pos;
bool tilt_yaw_neg;
};
YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
};

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

@ -589,33 +589,35 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
// TODO: disabled motors (?)
if (!_actuator_effectiveness->getAllocatedAndUnallocatedControl(control_allocator_status)) {
// Allocated control
const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
control_allocator_status.allocated_torque[0] = allocated_control(0);
control_allocator_status.allocated_torque[1] = allocated_control(1);
control_allocator_status.allocated_torque[2] = allocated_control(2);
control_allocator_status.allocated_thrust[0] = allocated_control(3);
control_allocator_status.allocated_thrust[1] = allocated_control(4);
control_allocator_status.allocated_thrust[2] = allocated_control(5);
// Allocated control
const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
control_allocator_status.allocated_torque[0] = allocated_control(0);
control_allocator_status.allocated_torque[1] = allocated_control(1);
control_allocator_status.allocated_torque[2] = allocated_control(2);
control_allocator_status.allocated_thrust[0] = allocated_control(3);
control_allocator_status.allocated_thrust[1] = allocated_control(4);
control_allocator_status.allocated_thrust[2] = allocated_control(5);
// Unallocated control
const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
allocated_control;
control_allocator_status.unallocated_torque[0] = unallocated_control(0);
control_allocator_status.unallocated_torque[1] = unallocated_control(1);
control_allocator_status.unallocated_torque[2] = unallocated_control(2);
control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
// Unallocated control
const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
allocated_control;
control_allocator_status.unallocated_torque[0] = unallocated_control(0);
control_allocator_status.unallocated_torque[1] = unallocated_control(1);
control_allocator_status.unallocated_torque[2] = unallocated_control(2);
control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
// Allocation success flags
control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
unallocated_control(2)).norm_squared() < 1e-6f);
control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
unallocated_control(5)).norm_squared() < 1e-6f);
}
// override control_allocator_status in customized saturation logic for certain effectiveness types
_actuator_effectiveness->getAllocatedAndUnallocatedControl(matrix_index, control_allocator_status);
// Allocation success flags
control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0],
control_allocator_status.unallocated_torque[1],
control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f);
control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0],
control_allocator_status.unallocated_thrust[1],
control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f);
// Actuator saturation
const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();