From 8b9b00a16784de0faf7465db2831942d10bd9738 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 28 Aug 2019 21:36:57 -0400 Subject: [PATCH] vtol_att_control fix WQ scheduling - only update FW/MC state when appropriate attitude setpoint updates - always publish actuator_control updates --- boards/px4/fmu-v2/multicopter.cmake | 2 +- .../vtol_att_control_main.cpp | 89 ++++++++++--------- .../vtol_att_control/vtol_att_control_main.h | 2 +- 3 files changed, 49 insertions(+), 44 deletions(-) diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index 0ddefaed1a89..929d27ce2b50 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -29,7 +29,7 @@ px4_add_board( imu/l3gd20 imu/lsm303d imu/mpu6000 - imu/mpu9250 + #imu/mpu9250 irlock lights/rgbled magnetometer/hmc5883 diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index ad6890ca7dea..3462930f2b2b 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -374,67 +374,72 @@ VtolAttitudeControl::Run() } // check in which mode we are in and call mode specific functions - if (_vtol_type->get_mode() == mode::ROTARY_WING) { + switch (_vtol_type->get_mode()) { + case mode::TRANSITION_TO_FW: + case mode::TRANSITION_TO_MC: + // vehicle is doing a transition + _vtol_vehicle_status.vtol_in_trans_mode = true; + _vtol_vehicle_status.vtol_in_rw_mode = true; // making mc attitude controller work during transition + _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW); + + _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); + + if (_mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp)) { - _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); + // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept + if (!_v_control_mode.flag_armed) { + Quatf().copyTo(_mc_virtual_att_sp.q_d); + Vector3f().copyTo(_mc_virtual_att_sp.thrust_body); + Quatf().copyTo(_v_att_sp.q_d); + Vector3f().copyTo(_v_att_sp.thrust_body); + } + + _vtol_type->update_transition_state(); + _v_att_sp_pub.publish(_v_att_sp); + } + break; + + case mode::ROTARY_WING: // vehicle is in rotary wing mode _vtol_vehicle_status.vtol_in_rw_mode = true; _vtol_vehicle_status.vtol_in_trans_mode = false; _vtol_vehicle_status.in_transition_to_fw = false; // got data from mc attitude controller - _vtol_type->update_mc_state(); - - } else if (_vtol_type->get_mode() == mode::FIXED_WING) { + if (_mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp)) { + + // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept + if (!_v_control_mode.flag_armed) { + Quatf().copyTo(_mc_virtual_att_sp.q_d); + Vector3f().copyTo(_mc_virtual_att_sp.thrust_body); + Quatf().copyTo(_v_att_sp.q_d); + Vector3f().copyTo(_v_att_sp.thrust_body); + } + + _vtol_type->update_mc_state(); + _v_att_sp_pub.publish(_v_att_sp); + } - _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); + break; + case mode::FIXED_WING: // vehicle is in fw mode _vtol_vehicle_status.vtol_in_rw_mode = false; _vtol_vehicle_status.vtol_in_trans_mode = false; _vtol_vehicle_status.in_transition_to_fw = false; - _vtol_type->update_fw_state(); - - } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC || _vtol_type->get_mode() == mode::TRANSITION_TO_FW) { - - _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); - _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); - - // vehicle is doing a transition - _vtol_vehicle_status.vtol_in_trans_mode = true; - _vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition - _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW); + if (_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp)) { + _vtol_type->update_fw_state(); + _v_att_sp_pub.publish(_v_att_sp); + } - _vtol_type->update_transition_state(); + break; } _vtol_type->fill_actuator_outputs(); - - // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept - if (!_v_control_mode.flag_armed) { - Quatf().copyTo(_mc_virtual_att_sp.q_d); - Vector3f().copyTo(_mc_virtual_att_sp.thrust_body); - Quatf().copyTo(_v_att_sp.q_d); - Vector3f().copyTo(_v_att_sp.thrust_body); - } - - /* Only publish if the proper mode(s) are enabled */ - if (_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) { - - _v_att_sp_pub.publish(_v_att_sp); - - if (updated_mc_in) { - _actuators_0_pub.publish(_actuators_out_0); - } - - if (updated_fw_in) { - _actuators_1_pub.publish(_actuators_out_1); - } - } + _actuators_0_pub.publish(_actuators_out_0); + _actuators_1_pub.publish(_actuators_out_1); // Advertise/Publish vtol vehicle status _vtol_vehicle_status.timestamp = hrt_absolute_time(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index e3c714083fdc..359fdccdabfb 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -132,7 +132,7 @@ class VtolAttitudeControl : public ModuleBase, public px4:: struct Params *get_params() {return &_params;} private: - /* handlers for subscriptions */ + uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)}; uORB::SubscriptionCallbackWorkItem _actuator_inputs_mc{this, ORB_ID(actuator_controls_virtual_mc)};