Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

control_allocator: handle thrust allocation for VTOL's properly

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-12-03 17:02:13 +01:00
committed by Daniel Agar
parent 936f2dff52
commit 0568cff299
6 changed files with 87 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/standard.cpp
+24
View File
@@ -413,6 +413,30 @@ void Standard::fill_actuator_outputs()
break;
}
_torque_setpoint_0->timestamp = hrt_absolute_time();
_torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_torque_setpoint_0->xyz[0] = mc_out[actuator_controls_s::INDEX_ROLL];
_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];
_thrust_setpoint_0->timestamp = hrt_absolute_time();
_thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_thrust_setpoint_0->xyz[0] = 0.f;
_thrust_setpoint_0->xyz[1] = 0.f;
_thrust_setpoint_0->xyz[2] = -mc_in[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] = fw_in[actuator_controls_s::INDEX_THROTTLE];
_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;
src/modules/vtol_att_control/tiltrotor.cpp
+39
View File
@@ -462,21 +462,57 @@ void Tiltrotor::fill_actuator_outputs()
auto &mc_out = _actuators_out_0->control;
auto &fw_out = _actuators_out_1->control;
_torque_setpoint_0->timestamp = hrt_absolute_time();
_torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_torque_setpoint_0->xyz[0] = 0.f;
_torque_setpoint_0->xyz[1] = 0.f;
_torque_setpoint_0->xyz[2] = 0.f;
_torque_setpoint_1->timestamp = hrt_absolute_time();
_torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_torque_setpoint_1->xyz[0] = 0.f;
_torque_setpoint_1->xyz[1] = 0.f;
_torque_setpoint_1->xyz[2] = 0.f;
_thrust_setpoint_0->timestamp = hrt_absolute_time();
_thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_thrust_setpoint_0->xyz[0] = 0.f;
_thrust_setpoint_0->xyz[1] = 0.f;
_thrust_setpoint_0->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;
// Multirotor output
mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
// for the legacy mixing system pubish FW throttle on the MC output
mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
// FW thrust is allocated on mc_thrust_sp[0] for tiltrotor with dynamic control allocation
_thrust_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_THROTTLE];
/* allow differential thrust if enabled */
if (_params->diff_thrust == 1) {
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
}
} else {
mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
_thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
}
// Fixed wing output
@@ -491,6 +527,9 @@ void Tiltrotor::fill_actuator_outputs()
fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL];
fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW];
_torque_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW];
}
_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
src/modules/vtol_att_control/vtol_att_control_main.cpp
+5 -35
View File
@@ -513,8 +513,11 @@ VtolAttitudeControl::Run()
_vtol_type->fill_actuator_outputs();
_actuators_0_pub.publish(_actuators_out_0);
_actuators_1_pub.publish(_actuators_out_1);
publishTorqueSetpoints(0);
publishThrustSetpoints(0);
_vehicle_torque_setpoint0_pub.publish(_torque_setpoint_0);
_vehicle_torque_setpoint1_pub.publish(_torque_setpoint_1);
_vehicle_thrust_setpoint0_pub.publish(_thrust_setpoint_0);
_vehicle_thrust_setpoint1_pub.publish(_thrust_setpoint_1);
// Advertise/Publish vtol vehicle status
_vtol_vehicle_status.timestamp = hrt_absolute_time();
@@ -524,39 +527,6 @@ VtolAttitudeControl::Run()
perf_end(_loop_perf);
}
void VtolAttitudeControl::publishTorqueSetpoints(const hrt_abstime &timestamp_sample)
{
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = timestamp_sample;
v_torque_sp.xyz[0] = _actuators_out_0.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuators_out_0.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuators_out_0.control[actuator_controls_s::INDEX_YAW];
_vehicle_torque_setpoint0_pub.publish(v_torque_sp);
v_torque_sp.xyz[0] = _actuators_out_1.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuators_out_1.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuators_out_1.control[actuator_controls_s::INDEX_YAW];
_vehicle_torque_setpoint1_pub.publish(v_torque_sp);
}
void VtolAttitudeControl::publishThrustSetpoints(const hrt_abstime &timestamp_sample)
{
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = timestamp_sample;
// TODO: separate thrust
v_thrust_sp.xyz[0] = _actuators_out_0.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = -_actuators_out_0.control[actuator_controls_s::INDEX_THROTTLE];
_vehicle_thrust_setpoint0_pub.publish(v_thrust_sp);
v_thrust_sp.xyz[0] = _actuators_out_1.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint1_pub.publish(v_thrust_sp);
}
int
VtolAttitudeControl::task_spawn(int argc, char *argv[])
{
src/modules/vtol_att_control/vtol_att_control_main.h
+10 -3
View File
@@ -142,12 +142,14 @@ public:
struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;}
struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;}
struct vehicle_torque_setpoint_s *get_torque_setpoint_0() {return &_torque_setpoint_0;}
struct vehicle_torque_setpoint_s *get_torque_setpoint_1() {return &_torque_setpoint_1;}
struct vehicle_thrust_setpoint_s *get_thrust_setpoint_0() {return &_thrust_setpoint_0;}
struct vehicle_thrust_setpoint_s *get_thrust_setpoint_1() {return &_thrust_setpoint_1;}
struct Params *get_params() {return &_params;}
private:
void publishTorqueSetpoints(const hrt_abstime &timestamp_sample);
void publishThrustSetpoints(const hrt_abstime &timestamp_sample);
void Run() override;
uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)};
@@ -189,6 +191,11 @@ private:
actuator_controls_s _actuators_out_0{}; //actuator controls going to the mc mixer
actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons)
vehicle_torque_setpoint_s _torque_setpoint_0{};
vehicle_torque_setpoint_s _torque_setpoint_1{};
vehicle_thrust_setpoint_s _thrust_setpoint_0{};
vehicle_thrust_setpoint_s _thrust_setpoint_1{};
airspeed_validated_s _airspeed_validated{}; // airspeed
position_setpoint_triplet_s _pos_sp_triplet{};
tecs_status_s _tecs_status{};
src/modules/vtol_att_control/vtol_type.cpp
+4
View File
@@ -65,6 +65,10 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
_actuators_out_1 = _attc->get_actuators_out1();
_actuators_mc_in = _attc->get_actuators_mc_in();
_actuators_fw_in = _attc->get_actuators_fw_in();
_torque_setpoint_0 = _attc->get_torque_setpoint_0();
_torque_setpoint_1 = _attc->get_torque_setpoint_1();
_thrust_setpoint_0 = _attc->get_thrust_setpoint_0();
_thrust_setpoint_1 = _attc->get_thrust_setpoint_1();
_local_pos = _attc->get_local_pos();
_local_pos_sp = _attc->get_local_pos_sp();
_airspeed_validated = _attc->get_airspeed();
src/modules/vtol_att_control/vtol_type.h
+5
View File
@@ -218,6 +218,11 @@ public:
struct tecs_status_s *_tecs_status;
struct vehicle_land_detected_s *_land_detected;
struct vehicle_torque_setpoint_s *_torque_setpoint_0;
struct vehicle_torque_setpoint_s *_torque_setpoint_1;
struct vehicle_thrust_setpoint_s *_thrust_setpoint_0;
struct vehicle_thrust_setpoint_s *_thrust_setpoint_1;
struct Params *_params;
bool _flag_idle_mc = false; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"