fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 23:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

preflight check: tilt collective actuation

Browse Source 
we "intercept" the tiltrotor_extra_controls message, which is read again
right after in ActuatorEffectivenessTiltrotorVTOL::updateSetpoint.
This commit is contained in:
Balduin
2025-02-17 11:40:41 +01:00
parent 7816fd14b7
commit 2a779663f6
2 changed files with 39 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+36 -9
View File
@@ -502,6 +502,15 @@ void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float,
int max_phase = 3 * 2;
// is this the proper way to do it?
// bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
PX4_INFO("tiltrotor = %d", (int) tiltrotor);
if (tiltrotor) {
max_phase = 4 * 2;
}
hrt_abstime now = hrt_absolute_time();
if (now - _last_preflight_check_update >= 500_ms) {
_preflight_check_phase++;
@@ -512,16 +521,34 @@ void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float,
int axis = _preflight_check_phase / 2;
int negative = _preflight_check_phase % 2;
c[0](0) = 0.;
c[0](1) = 0.;
c[0](2) = 0.;
c[0](axis) = negative ? -1.f : 1.f;
if (axis < 3) {
c[0](0) = 0.;
c[0](1) = 0.;
c[0](2) = 0.;
c[0](axis) = negative ? -1.f : 1.f;
if (_num_control_allocation > 1) {
c[1](0) = 0.;
c[1](1) = 0.;
c[1](2) = 0.;
c[1](axis) = negative ? -1.f : 1.f;
if (_num_control_allocation > 1) {
c[1](0) = 0.;
c[1](1) = 0.;
c[1](2) = 0.;
c[1](axis) = negative ? -1.f : 1.f;
}
} else {
// axis 4 = tiltrotor. Here, we modify the
// tiltrotor_extra_controls message, which is received in
// ActuatorEffectivenessTiltrotorVTOL::updateSetpoint
float modified_tilt_control = negative ? -1.f : 1.f;
tiltrotor_extra_controls_s tiltrotor_extra_controls;
if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
tiltrotor_extra_controls.collective_tilt_normalized_setpoint = modified_tilt_control;
tiltrotor_extra_controls.timestamp = hrt_absolute_time();
_tiltrotor_extra_controls_pub.publish(tiltrotor_extra_controls);
}
}
// PX4_INFO("_torque_sp: %f, %f, %f", (double) _torque_sp(0), (double) _torque_sp(1), (double) _torque_sp(2));
src/modules/control_allocator/ControlAllocator.hpp
+3
View File
@@ -181,6 +181,7 @@ private:
uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1}; /**< vehicle torque setpoint subscription (2. instance) */
uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; /**< vehicle thrust setpoint subscription (2. instance) */
uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};
// Outputs
uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
@@ -189,6 +190,8 @@ private:
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
uORB::Publication<actuator_servos_trim_s> _actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)};
uORB::Publication<tiltrotor_extra_controls_s> _tiltrotor_extra_controls_pub{ORB_ID(tiltrotor_extra_controls)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};