From b244c2c878958857cc1af21c670d7d02f6a78126 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 17 Oct 2019 12:20:56 +0200 Subject: [PATCH 001/129] LPF: add sample frequency getter --- src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp | 1 + src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp index 1619d4564db0..44927628ce38 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp @@ -43,6 +43,7 @@ namespace math void LowPassFilter2pVector3f::set_cutoff_frequency(float sample_freq, float cutoff_freq) { _cutoff_freq = cutoff_freq; + _sample_freq = sample_freq; // reset delay elements on filter change _delay_element_1.zero(); diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp index 643cff73aa39..ec8b0aa218ad 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp @@ -74,12 +74,16 @@ class LowPassFilter2pVector3f // Return the cutoff frequency float get_cutoff_freq() const { return _cutoff_freq; } + // Return the sample frequency + float get_sample_freq() const { return _sample_freq; } + // Reset the filter state to this value matrix::Vector3f reset(const matrix::Vector3f &sample); private: float _cutoff_freq{0.0f}; + float _sample_freq{0.0f}; float _a1{0.0f}; float _a2{0.0f}; From 7928b8f06924d411964c9f4ec2f8749ec6ddd9bb Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 17 Oct 2019 12:25:06 +0200 Subject: [PATCH 002/129] uORB: Add angular accel, torque & thrust setpoints --- msg/CMakeLists.txt | 3 +++ msg/tools/uorb_rtps_message_ids.yaml | 6 ++++++ msg/vehicle_angular_acceleration_setpoint.msg | 5 +++++ msg/vehicle_thrust_setpoint.msg | 5 +++++ msg/vehicle_torque_setpoint.msg | 5 +++++ 5 files changed, 24 insertions(+) create mode 100644 msg/vehicle_angular_acceleration_setpoint.msg create mode 100644 msg/vehicle_thrust_setpoint.msg create mode 100644 msg/vehicle_torque_setpoint.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a836c6c4df43..1d0ae0767942 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -140,6 +140,7 @@ set(msg_files vehicle_acceleration.msg vehicle_air_data.msg vehicle_angular_acceleration.msg + vehicle_angular_acceleration_setpoint.msg vehicle_angular_velocity.msg vehicle_attitude.msg vehicle_attitude_setpoint.msg @@ -160,6 +161,8 @@ set(msg_files vehicle_roi.msg vehicle_status.msg vehicle_status_flags.msg + vehicle_thrust_setpoint.msg + vehicle_torque_setpoint.msg vehicle_trajectory_bezier.msg vehicle_trajectory_waypoint.msg vtol_vehicle_status.msg diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index a0031db16a83..4a97ea6fef1b 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -295,6 +295,12 @@ rtps: id: 131 - msg: yaw_estimator_status id: 132 + - msg: vehicle_angular_acceleration_setpoint + id: 133 + - msg: vehicle_torque_setpoint + id: 134 + - msg: vehicle_thrust_setpoint + id: 135 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/msg/vehicle_angular_acceleration_setpoint.msg b/msg/vehicle_angular_acceleration_setpoint.msg new file mode 100644 index 000000000000..69f5b0bae30f --- /dev/null +++ b/msg/vehicle_angular_acceleration_setpoint.msg @@ -0,0 +1,5 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # angular acceleration about X, Y, Z body axis in rad/s^2 diff --git a/msg/vehicle_thrust_setpoint.msg b/msg/vehicle_thrust_setpoint.msg new file mode 100644 index 000000000000..c84bd87a08b2 --- /dev/null +++ b/msg/vehicle_thrust_setpoint.msg @@ -0,0 +1,5 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # thrust setpoint along X, Y, Z body axis (in N) diff --git a/msg/vehicle_torque_setpoint.msg b/msg/vehicle_torque_setpoint.msg new file mode 100644 index 000000000000..83b733dd831f --- /dev/null +++ b/msg/vehicle_torque_setpoint.msg @@ -0,0 +1,5 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # torque setpoint about X, Y, Z body axis (in N.m) From d927c33bb1f1e377193545848aa01e872c8ccb12 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 17 Oct 2019 12:25:58 +0200 Subject: [PATCH 003/129] logger: log angular acceleration, torque and thrust setpoints for system identification --- src/modules/logger/logged_topics.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index a760c7beab2f..9b624d26f077 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -217,6 +217,9 @@ void LoggedTopics::add_system_identification_topics() add_topic("actuator_controls_0"); add_topic("actuator_controls_1"); add_topic("sensor_combined"); + add_topic("vehicle_angular_acceleration"); + add_topic("vehicle_angular_acceleration_setpoint"); + add_topic("vehicle_torque_setpoint"); } int LoggedTopics::add_topics_from_file(const char *fname) From 31d9ed7e7d7875c53821308ee710b3fb39c38680 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 25 Oct 2019 13:54:59 +0200 Subject: [PATCH 004/129] sitl iris: set inertia and update gains --- ROMFS/px4fmu_common/init.d-posix/10016_iris | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/10016_iris b/ROMFS/px4fmu_common/init.d-posix/10016_iris index 9f90ade5bb2a..40e386de8117 100644 --- a/ROMFS/px4fmu_common/init.d-posix/10016_iris +++ b/ROMFS/px4fmu_common/init.d-posix/10016_iris @@ -9,5 +9,18 @@ sh /etc/init.d/rc.mc_defaults +if [ $AUTOCNF = yes ] +then + param set MC_ROLLRATE_P 18.0 + param set MC_ROLLRATE_D 0.36 + param set MC_PITCHRATE_P 18.0 + param set MC_PITCHRATE_D 0.36 + param set MC_YAWRATE_P 7.0 + param set MC_YAWRATE_D 0.0 + + param set VM_INERTIA_XX 0.0083 + param set VM_INERTIA_YY 0.0083 + param set VM_INERTIA_ZZ 0.0286 +fi set MIXER quad_w From 70280510ef8eadd678a2e1254b0e574c9f65ec6a Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sat, 23 Nov 2019 18:59:26 +0100 Subject: [PATCH 005/129] Add topic vehicle_actuator_setpoint --- msg/CMakeLists.txt | 1 + msg/vehicle_actuator_setpoint.msg | 6 ++++++ 2 files changed, 7 insertions(+) create mode 100644 msg/vehicle_actuator_setpoint.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 1d0ae0767942..27fc7d6f3239 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -138,6 +138,7 @@ set(msg_files ulog_stream.msg ulog_stream_ack.msg vehicle_acceleration.msg + vehicle_actuator_setpoint.msg vehicle_air_data.msg vehicle_angular_acceleration.msg vehicle_angular_acceleration_setpoint.msg diff --git a/msg/vehicle_actuator_setpoint.msg b/msg/vehicle_actuator_setpoint.msg new file mode 100644 index 000000000000..5d4781d46e1b --- /dev/null +++ b/msg/vehicle_actuator_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled + +uint8 NUM_ACTUATOR_SETPOINT = 16 + +float32[16] actuator From 88b1298592aeb12ddc6125d25348608850fc2593 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 13:39:50 +0100 Subject: [PATCH 006/129] Add actuator control groups 4 and 5 This is a temporary solution (hack) to integrate the new control allocation topics into the existing mixer system. --- msg/actuator_controls.msg | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index 25ff45f432ed..d323359f3ff9 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -1,6 +1,6 @@ uint64 timestamp # time since system start (microseconds) uint8 NUM_ACTUATOR_CONTROLS = 8 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 7 uint8 INDEX_ROLL = 0 uint8 INDEX_PITCH = 1 uint8 INDEX_YAW = 2 @@ -16,10 +16,13 @@ uint8 GROUP_INDEX_ATTITUDE = 0 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1 uint8 GROUP_INDEX_GIMBAL = 2 uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3 +uint8 GROUP_INDEX_ALLOCATED_PART1 = 4 +uint8 GROUP_INDEX_ALLOCATED_PART2 = 5 uint8 GROUP_INDEX_PAYLOAD = 6 uint64 timestamp_sample # the timestamp the data this control response is based on was sampled float32[8] control # TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3 +# TOPICS actuator_controls_4 actuator_controls_5 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc From 4a68c102d4a38f13139e633976fb0d576b9b64ae Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 13:41:34 +0100 Subject: [PATCH 007/129] PX4IO: adapt to added control groups 4 and 5 --- src/drivers/px4io/px4io.cpp | 14 +++++++++++++- src/modules/px4iofirmware/protocol.h | 4 ++++ src/modules/px4iofirmware/px4io.h | 2 +- 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d0de6a609d58..40afcd893ee9 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -261,6 +261,8 @@ class PX4IO : public cdev::CDev uORB::Subscription _t_actuator_controls_1{ORB_ID(actuator_controls_1)}; ///< actuator controls group 1 topic uORB::Subscription _t_actuator_controls_2{ORB_ID(actuator_controls_2)};; ///< actuator controls group 2 topic uORB::Subscription _t_actuator_controls_3{ORB_ID(actuator_controls_3)};; ///< actuator controls group 3 topic + uORB::Subscription _t_actuator_controls_4{ORB_ID(actuator_controls_4)};; ///< actuator controls group 4 topic + uORB::Subscription _t_actuator_controls_5{ORB_ID(actuator_controls_5)};; ///< actuator controls group 5 topic uORB::Subscription _t_actuator_armed{ORB_ID(actuator_armed)}; ///< system armed control topic uORB::Subscription _t_vehicle_control_mode{ORB_ID(vehicle_control_mode)}; ///< vehicle control mode topic uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; ///< parameter update topic @@ -1261,6 +1263,8 @@ PX4IO::io_set_control_groups() (void)io_set_control_state(1); (void)io_set_control_state(2); (void)io_set_control_state(3); + (void)io_set_control_state(4); + (void)io_set_control_state(5); return ret; } @@ -1295,6 +1299,14 @@ PX4IO::io_set_control_state(unsigned group) case 3: changed = _t_actuator_controls_3.update(&controls); break; + + case 4: + changed = _t_actuator_controls_4.update(&controls); + break; + + case 5: + changed = _t_actuator_controls_5.update(&controls); + break; } if (!changed && (!_in_esc_calibration_mode || group != 0)) { @@ -2377,7 +2389,7 @@ PX4IO::print_status(bool extended_status) io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE)); printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - for (unsigned group = 0; group < 4; group++) { + for (unsigned group = 0; group < 6; group++) { printf("controls %u:", group); for (unsigned i = 0; i < _max_controls; i++) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 3cfcee2d1211..858d958d81a4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -253,12 +253,16 @@ enum { /* DSM bind states */ #define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_4 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 4) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ +#define PX4IO_P_CONTROLS_GROUP_5 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 5) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_VALID 64 #define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ #define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ #define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ #define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP4 (1 << 4) /**< group 4 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP5 (1 << 5) /**< group 5 is valid / received */ /* raw text load to the mixer parser - ignores offset */ #define PX4IO_PAGE_MIXERLOAD 52 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 0ca1819dbfad..e17001740587 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -58,7 +58,7 @@ #define PX4IO_BL_VERSION 3 #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_CONTROL_GROUPS 4 +#define PX4IO_CONTROL_GROUPS 6 #define PX4IO_RC_INPUT_CHANNELS 18 #define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ From b6bf0f165f89b106e4b013537bf54a17cf4e6907 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 13:42:17 +0100 Subject: [PATCH 008/129] mixer_module: adapt to added control groups 4 and 5 --- src/lib/mixer_module/mixer_module.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index f5cb140e1fa6..1104cb914d22 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -49,7 +49,9 @@ MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &inter {&interface, ORB_ID(actuator_controls_0)}, {&interface, ORB_ID(actuator_controls_1)}, {&interface, ORB_ID(actuator_controls_2)}, - {&interface, ORB_ID(actuator_controls_3)} + {&interface, ORB_ID(actuator_controls_3)}, + {&interface, ORB_ID(actuator_controls_4)}, + {&interface, ORB_ID(actuator_controls_5)} }, _scheduling_policy(scheduling_policy), _support_esc_calibration(support_esc_calibration), From 15f0a0352b6c24323ed97f5cf311711d57855091 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 13:46:40 +0100 Subject: [PATCH 009/129] Add mixer type AllocatedActuatorMixer (A:) The mixer is added with the following character string in a *.mix file: `A: X` with X a number between 0 and 15 This indicates that the actuator number X will be written to the output. Ideally, this mixer would use the topic `vehicle_actuator_setpoint`, but for now it receives info on topics `actuator_controls_4` and `actuator_controls_5`. romf_pruner: do not prune A mixer --- Tools/px_romfs_pruner.py | 2 +- .../AllocatedActuatorMixer.cpp | 149 ++++++++++++++++++ .../AllocatedActuatorMixer.hpp | 103 ++++++++++++ .../AllocatedActuatorMixer/CMakeLists.txt | 39 +++++ src/lib/mixer/CMakeLists.txt | 2 + src/lib/mixer/MixerGroup.cpp | 5 + 6 files changed, 299 insertions(+), 1 deletion(-) create mode 100644 src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp create mode 100644 src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp create mode 100644 src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 82e7c452815a..c5688986b3a7 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -96,7 +96,7 @@ def main(): # handle mixer files differently than startup files if file_path.endswith(".mix"): if line.startswith(("Z:", "M:", "R: ", "O:", "S:", - "H:", "T:", "P:")): + "H:", "T:", "P:", "A:")): # reduce multiple consecutive spaces into a # single space line_reduced = re.sub(' +', ' ', line) diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp new file mode 100644 index 000000000000..bdf55e3f3f28 --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_AllocatedActuatorMixer.cpp + * + * Mixer for allocated actuators. + * + * @author Julien Lecoeur + */ + +#include "AllocatedActuatorMixer.hpp" + +#include +#include +#include + +// #define debug(fmt, args...) do { } while(0) +#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) + +AllocatedActuatorMixer::AllocatedActuatorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + uint8_t index) : + Mixer(control_cb, cb_handle) +{ + if (index < 8) { + _control_group = 4; + _control_index = index; + + } else if (index < 16) { + _control_group = 5; + _control_index = index - 8; + + } else { + debug("'A:' invalid index"); + } +} + +unsigned AllocatedActuatorMixer::set_trim(float trim) +{ + return 1; +} + +unsigned AllocatedActuatorMixer::get_trim(float *trim) +{ + *trim = 0.0f; + return 1; +} + +int +AllocatedActuatorMixer::parse(const char *buf, unsigned &buflen, uint8_t &index) +{ + int ret; + int i; + + // enforce that the mixer ends with a new line + if (!string_well_formed(buf, buflen)) { + return -1; + } + + // parse line + if ((ret = sscanf(buf, "A: %d", &i)) != 1) { + debug("'A:' parser: failed on '%s'", buf); + return -1; + } + + buf = skipline(buf, buflen); + + if (buf == nullptr) { + debug("'A:' parser: no line ending, line is incomplete"); + return -1; + } + + // check parsed index + if (i < 16) { + index = i; + + } else { + debug("'A:' parser: invalid index"); + return -1; + } + + return 0; +} + +AllocatedActuatorMixer * +AllocatedActuatorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, + unsigned &buflen) +{ + uint8_t index; + + if (parse(buf, buflen, index) == 0) { + return new AllocatedActuatorMixer(control_cb, cb_handle, index); + + } else { + return nullptr; + } +} + +unsigned +AllocatedActuatorMixer::mix(float *outputs, unsigned space) +{ + if (space < 1) { + return 0; + } + + _control_cb(_cb_handle, + _control_group, + _control_index, + *outputs); + + return 1; +} + +void +AllocatedActuatorMixer::groups_required(uint32_t &groups) +{ + groups |= 1 << _control_group; +} diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp new file mode 100644 index 000000000000..2ee9b66e22a3 --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mixer_AllocatedActuatorMixer.hpp + * + * Mixer for allocated actuators. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include + +/** + * Mixer for allocated actuators. + * + * Copies a single actuator to a single output. + */ +class AllocatedActuatorMixer : public Mixer +{ +public: + /** + * Constructor + * + * @param index Actuator index (0..15) + */ + AllocatedActuatorMixer(ControlCallback control_cb, + uintptr_t cb_handle, + uint8_t index); + virtual ~AllocatedActuatorMixer() = default; + + // no copy, assignment, move, move assignment + AllocatedActuatorMixer(const AllocatedActuatorMixer &) = delete; + AllocatedActuatorMixer &operator=(const AllocatedActuatorMixer &) = delete; + AllocatedActuatorMixer(AllocatedActuatorMixer &&) = delete; + AllocatedActuatorMixer &operator=(AllocatedActuatorMixer &&) = delete; + + /** + * Factory method with full external configuration. + * + * Given a pointer to a buffer containing a text description of the mixer, + * returns a pointer to a new instance of the mixer. + * + * @param control_cb The callback to invoke when fetching a + * control value. + * @param cb_handle Handle passed to the control callback. + * @param buf Buffer containing a text description of + * the mixer. + * @param buflen Length of the buffer in bytes, adjusted + * to reflect the bytes consumed. + * @return A new AllocatedActuatorMixer instance, or nullptr + * if the text format is bad. + */ + static AllocatedActuatorMixer *from_text(Mixer::ControlCallback control_cb, + uintptr_t cb_handle, + const char *buf, + unsigned &buflen); + + unsigned mix(float *outputs, unsigned space) override; + void groups_required(uint32_t &groups) override; + unsigned set_trim(float trim) override; + unsigned get_trim(float *trim) override; + +private: + uint8_t _control_group; /**< group from which the input reads */ + uint8_t _control_index; /**< index within the control group */ + + static int parse(const char *buf, + unsigned &buflen, + uint8_t &index); +}; diff --git a/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt b/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt new file mode 100644 index 000000000000..a9bd1cff6c88 --- /dev/null +++ b/src/lib/mixer/AllocatedActuatorMixer/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_library(AllocatedActuatorMixer + AllocatedActuatorMixer.cpp + AllocatedActuatorMixer.hpp +) +target_link_libraries(AllocatedActuatorMixer PRIVATE Mixer) +add_dependencies(AllocatedActuatorMixer prebuild_targets) diff --git a/src/lib/mixer/CMakeLists.txt b/src/lib/mixer/CMakeLists.txt index 716a99d66a19..61af1d9b6e7d 100644 --- a/src/lib/mixer/CMakeLists.txt +++ b/src/lib/mixer/CMakeLists.txt @@ -34,6 +34,7 @@ # required by other mixers add_subdirectory(Mixer) +add_subdirectory(AllocatedActuatorMixer) add_subdirectory(HelicopterMixer) add_subdirectory(MultirotorMixer) add_subdirectory(NullMixer) @@ -48,6 +49,7 @@ add_library(mixer target_link_libraries(mixer PRIVATE + AllocatedActuatorMixer HelicopterMixer MultirotorMixer NullMixer diff --git a/src/lib/mixer/MixerGroup.cpp b/src/lib/mixer/MixerGroup.cpp index c9575301d959..f87eefd46a26 100644 --- a/src/lib/mixer/MixerGroup.cpp +++ b/src/lib/mixer/MixerGroup.cpp @@ -39,6 +39,7 @@ #include "MixerGroup.hpp" +#include "AllocatedActuatorMixer/AllocatedActuatorMixer.hpp" #include "HelicopterMixer/HelicopterMixer.hpp" #include "MultirotorMixer/MultirotorMixer.hpp" #include "NullMixer/NullMixer.hpp" @@ -192,6 +193,10 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle m = NullMixer::from_text(p, resid); break; + case 'A': + m = AllocatedActuatorMixer::from_text(control_cb, cb_handle, p, resid); + break; + case 'M': m = SimpleMixer::from_text(control_cb, cb_handle, p, resid); break; From 27e34b6346c5611943448e1abb2fdeb2950e8094 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 13:48:42 +0100 Subject: [PATCH 010/129] Add generic quad, hexa and octo mixers --- ROMFS/px4fmu_common/mixers/CMakeLists.txt | 3 +++ ROMFS/px4fmu_common/mixers/hexa.main.mix | 8 ++++++++ ROMFS/px4fmu_common/mixers/octo.main.mix | 10 ++++++++++ ROMFS/px4fmu_common/mixers/quad.main.mix | 20 ++++++++++++++++++++ 4 files changed, 41 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/hexa.main.mix create mode 100644 ROMFS/px4fmu_common/mixers/octo.main.mix create mode 100644 ROMFS/px4fmu_common/mixers/quad.main.mix diff --git a/ROMFS/px4fmu_common/mixers/CMakeLists.txt b/ROMFS/px4fmu_common/mixers/CMakeLists.txt index 9a4e28eb9217..36320d1d1e1c 100644 --- a/ROMFS/px4fmu_common/mixers/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers/CMakeLists.txt @@ -51,6 +51,7 @@ px4_add_romfs_files( fw_generic_wing.main.mix FX79.main.mix generic_diff_rover.main.mix + hexa.main.mix hexa_cox.main.mix hexa_+.main.mix hexa_x.main.mix @@ -58,12 +59,14 @@ px4_add_romfs_files( mount.aux.mix mount_legs.aux.mix ocpoc_quad_x.main.mix + octo.main.mix octo_cox.main.mix octo_cox_w.main.mix octo_+.main.mix octo_x.main.mix pass.aux.mix phantom.main.mix + quad.main.mix quad_dc.main.mix quad_h.main.mix quad_+.main.mix diff --git a/ROMFS/px4fmu_common/mixers/hexa.main.mix b/ROMFS/px4fmu_common/mixers/hexa.main.mix new file mode 100644 index 000000000000..8b96e6f38c0b --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/hexa.main.mix @@ -0,0 +1,8 @@ +# Hexa + +A: 0 +A: 1 +A: 2 +A: 3 +A: 4 +A: 5 diff --git a/ROMFS/px4fmu_common/mixers/octo.main.mix b/ROMFS/px4fmu_common/mixers/octo.main.mix new file mode 100644 index 000000000000..a9220e8f688d --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/octo.main.mix @@ -0,0 +1,10 @@ +# Octo + +A: 0 +A: 1 +A: 2 +A: 3 +A: 4 +A: 5 +A: 6 +A: 7 diff --git a/ROMFS/px4fmu_common/mixers/quad.main.mix b/ROMFS/px4fmu_common/mixers/quad.main.mix new file mode 100644 index 000000000000..747337b2bed4 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/quad.main.mix @@ -0,0 +1,20 @@ +Control outputs +A: 0 +A: 1 +A: 2 +A: 3 + +AUX1 Passthrough +M: 1 +S: 3 5 10000 10000 0 -10000 10000 + +AUX2 Passthrough +M: 1 +S: 3 6 10000 10000 0 -10000 10000 + +Failsafe outputs +The following outputs are set to their disarmed value +during normal operation and to their failsafe falue in case +of flight termination. +Z: +Z: From d0291f074910670d01107f69194fc1d14e89ece9 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 13:51:44 +0100 Subject: [PATCH 011/129] Add ControlAllocation base class --- .../ControlAllocation/CMakeLists.txt | 44 ++++ .../ControlAllocation/ControlAllocation.cpp | 150 +++++++++++++ .../ControlAllocation/ControlAllocation.hpp | 197 ++++++++++++++++++ 3 files changed, 391 insertions(+) create mode 100644 src/modules/control_allocator/ControlAllocation/CMakeLists.txt create mode 100644 src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp create mode 100644 src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt new file mode 100644 index 000000000000..37a6000d8bab --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(ControlAllocation + ControlAllocation.cpp +) + +target_include_directories(ControlAllocation + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(ControlAllocation PRIVATE mathlib) + diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp new file mode 100644 index 000000000000..0b5fee029052 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocation.cpp + * + * Interface for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include "ControlAllocation.hpp" + +const matrix::Vector & +ControlAllocation::getAllocatedActuator() const +{ + return _actuator_sp; +} + +void +ControlAllocation::setControlSetpoint(const matrix::Vector &control) +{ + _control_sp = control; +} + +const matrix::Vector & +ControlAllocation::getControlSetpoint() const +{ + return _control_sp; +} + +const matrix::Vector & +ControlAllocation::getAllocatedControl() const +{ + return _control_allocated; +} + +void +ControlAllocation::setEffectivenessMatrix(const + matrix::Matrix &B) +{ + _B = B; +} + +const matrix::Matrix & +ControlAllocation::getEffectivenessMatrix() const +{ + return _B; +} + +void +ControlAllocation::setActuatorMin(const matrix::Vector + &actuator_min) +{ + _actuator_min = actuator_min; +} + +const matrix::Vector & +ControlAllocation::getActuatorMin() const +{ + return _actuator_min; +} + +void +ControlAllocation::setActuatorMax(const matrix::Vector + &actuator_max) +{ + _actuator_max = actuator_max; +} + +const matrix::Vector & +ControlAllocation::getActuatorMax() const +{ + return _actuator_max; +} + +void +ControlAllocation::setCurrentActuatorSetpoint(const matrix::Vector + &actuator_sp) +{ + _actuator_sp = actuator_sp; +} + +matrix::Vector +ControlAllocation::clipActuatorSetpoint() const +{ + matrix::Vector actuator_clipped = _actuator_sp; + + for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) { + if (_actuator_max(i) < _actuator_min(i)) { + // this should not happen + actuator_clipped(i) = _actuator_min(i); + + } else if (actuator_clipped(i) < _actuator_min(i)) { + actuator_clipped(i) = _actuator_min(i); + + } else if (actuator_clipped(i) > _actuator_max(i)) { + actuator_clipped(i) = _actuator_max(i); + } + } + + return actuator_clipped; +} + +matrix::Vector +ControlAllocation::normalizeActuatorSetpoint() const +{ + matrix::Vector actuator_normalized; + + for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) { + if (_actuator_min(i) < _actuator_max(i)) { + actuator_normalized(i) = -1.0f + 2.0f * (_actuator_sp(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); + + } else { + actuator_normalized(i) = -1.0f; + } + } + + return actuator_normalized; +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp new file mode 100644 index 000000000000..4ef7aff5f052 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocation.hpp + * + * Interface for Control Allocation Algorithms + * + * Implementers of this interface are expected to update the members + * of this base class in the `allocate` method. + * + * Example usage: + * ``` + * [...] + * // Initialization + * ControlAllocationMethodImpl alloc(); + * alloc.setEffectivenessMatrix(B); + * alloc.setActuatorMin(actuator_min); + * alloc.setActuatorMin(actuator_max); + * + * while (1) { + * [...] + * + * // Set control setpoint, allocate actuator setpoint, retrieve actuator setpoint + * alloc.setControlSetpoint(control_sp); + * alloc.allocate(); + * actuator_sp = alloc.getAllocatedActuator(); + * + * // Check if the control setpoint was fully allocated + * unallocated_control = control_sp - alloc.getAllocatedControl() + * + * [...] + * } + * ``` + * + * + * @author Julien Lecoeur + */ + +#pragma once + +#include +#include + +class ControlAllocation +{ +public: + ControlAllocation() = default; + virtual ~ControlAllocation() = default; + + static const uint8_t NUM_ACTUATORS = vehicle_actuator_setpoint_s::NUM_ACTUATOR_SETPOINT; + static const uint8_t NUM_AXES = 6; + + /** + * Allocate control setpoint to actuators + * + * @param control_setpoint Desired control setpoint vector (input) + */ + virtual void allocate() = 0; + + /** + * Set the control effectiveness matrix + * + * @param B Effectiveness matrix + */ + virtual void setEffectivenessMatrix(const matrix::Matrix &B); + + /** + * Get the allocated actuator vector + * + * @return Actuator vector + */ + const matrix::Vector &getAllocatedActuator() const; + + /** + * Set the desired control vector + * + * @param Control vector + */ + void setControlSetpoint(const matrix::Vector &control); + + /** + * Set the desired control vector + * + * @param Control vector + */ + const matrix::Vector &getControlSetpoint() const; + + /** + * Get the allocated control vector + * + * @return Control vector + */ + const matrix::Vector &getAllocatedControl() const; + + /** + * Get the control effectiveness matrix + * + * @return Effectiveness matrix + */ + const matrix::Matrix &getEffectivenessMatrix() const; + + /** + * Set the minimum actuator values + * + * @param actuator_min Minimum actuator values + */ + void setActuatorMin(const matrix::Vector &actuator_min); + + /** + * Get the minimum actuator values + * + * @return Minimum actuator values + */ + const matrix::Vector &getActuatorMin() const; + + /** + * Set the maximum actuator values + * + * @param actuator_max Maximum actuator values + */ + void setActuatorMax(const matrix::Vector &actuator_max); + + /** + * Get the maximum actuator values + * + * @return Maximum actuator values + */ + const matrix::Vector &getActuatorMax() const; + + /** + * Set the current actuator setpoint. + * + * Use this when a new allocation method is started to initialize it properly. + * In most cases, it is not needed to call this method before `allocate()`. + * Indeed the previous actuator setpoint is expected to be stored during calls to `allocate()`. + * + * @param actuator_sp Actuator setpoint + */ + void setCurrentActuatorSetpoint(const matrix::Vector &actuator_sp); + + /** + * Clip the actuator setpoint between minimum and maximum values. + * + * The output is in the range [min; max] + * + * @return Clipped actuator setpoint + */ + matrix::Vector clipActuatorSetpoint() const; + + /** + * Normalize the actuator setpoint between minimum and maximum values. + * + * The output is in the range [-1; +1] + * + * @return Clipped actuator setpoint + */ + matrix::Vector normalizeActuatorSetpoint() const; + +protected: + matrix::Matrix _B; //< Effectiveness matrix + matrix::Vector _actuator_min; //< Minimum actuator values + matrix::Vector _actuator_max; //< Maximum actuator values + matrix::Vector _actuator_sp; //< Actuator setpoint + matrix::Vector _control_sp; //< Control setpoint + matrix::Vector _control_allocated; //< Allocated control +}; From 38cadb735d7fab45922c137295f89d35aa7adde3 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 13:56:23 +0100 Subject: [PATCH 012/129] control_allocation: introduced ControlAllocationPseudoInverse class which serves as base for allocation algorithms using pseudo inverse Add "Simple" control allocation method based on pseudoinverse Add test for ControlAllocationSimple ControlAllocationSimple: pull generic code into base class Signed-off-by: RomanBapst Signed-off-by: RomanBapst --- boards/px4/sitl/test.cmake | 1 + .../ControlAllocation/CMakeLists.txt | 2 + .../ControlAllocationPseudoInverse.cpp | 75 +++++++++++++++++++ .../ControlAllocationPseudoInverse.hpp | 69 +++++++++++++++++ .../ControlAllocationPseudoInverseTest.cpp | 66 ++++++++++++++++ 5 files changed, 213 insertions(+) create mode 100644 src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp create mode 100644 src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp create mode 100644 src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index f36effca6bf8..186d0707b7a4 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -24,6 +24,7 @@ px4_add_board( attitude_estimator_q camera_feedback commander + control_allocator dataman ekf2 events diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt index 37a6000d8bab..8409dd6c2622 100644 --- a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt +++ b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_library(ControlAllocation ControlAllocation.cpp + ControlAllocationPseudoInverse.cpp ) target_include_directories(ControlAllocation @@ -42,3 +43,4 @@ target_include_directories(ControlAllocation target_link_libraries(ControlAllocation PRIVATE mathlib) +px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp new file mode 100644 index 000000000000..78355e8f2640 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocationPseudoInverse.hpp + * + * Simple Control Allocation Algorithm + * + * @author Julien Lecoeur + */ + +#include "ControlAllocationPseudoInverse.hpp" + +void +ControlAllocationPseudoInverse::setEffectivenessMatrix(const + matrix::Matrix &B) +{ + _B = B; + _A_update_needed = true; +} + +void +ControlAllocationPseudoInverse::updatePseudoInverse() +{ + if (_A_update_needed) { + _A = matrix::geninv(_B); + _A_update_needed = false; + } +} + +void +ControlAllocationPseudoInverse::allocate() +{ + //Compute new gains if needed + updatePseudoInverse(); + + // Allocate + _actuator_sp = _A * _control_sp; + + // Clip + _actuator_sp = clipActuatorSetpoint(); + + // Compute achieved control + _control_allocated = _B * _actuator_sp; +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp new file mode 100644 index 000000000000..41403aa09ead --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocationPseudoInverse.hpp + * + * Simple Control Allocation Algorithm + * + * It computes the pseudo-inverse of the effectiveness matrix + * Actuator saturation is handled by simple clipping, do not + * expect good performance in case of actuator saturation. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ControlAllocation.hpp" + +class ControlAllocationPseudoInverse: public ControlAllocation +{ +public: + ControlAllocationPseudoInverse() = default; + virtual ~ControlAllocationPseudoInverse() = default; + + virtual void allocate() override; + virtual void setEffectivenessMatrix(const matrix::Matrix &B) override; + +protected: + matrix::Matrix _A; + + bool _A_update_needed{false}; + + /** + * Recalculate pseudo inverse if required. + * + */ + void updatePseudoInverse(); +}; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp new file mode 100644 index 000000000000..ecf888759830 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocationTest.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include + +using namespace matrix; + +TEST(ControlAllocationTest, AllZeroCase) +{ + ControlAllocationPseudoInverse method; + + matrix::Vector control_sp; + matrix::Vector control_allocated; + matrix::Vector control_allocated_expected; + matrix::Matrix B; + matrix::Vector actuator_sp; + matrix::Vector actuator_sp_expected; + + method.setEffectivenessMatrix(B); + method.setControlSetpoint(control_sp); + method.allocate(); + actuator_sp = method.getAllocatedActuator(); + control_allocated_expected = method.getAllocatedControl(); + + EXPECT_EQ(actuator_sp, actuator_sp_expected); + EXPECT_EQ(control_allocated, control_allocated_expected); +} From 9648ef64ddf501bbb4c0f1ceb81790321a588bf6 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 13:59:16 +0100 Subject: [PATCH 013/129] Add control_allocator module Add topic control_allocator_status Increase wq:rate_ctrl stack size --- msg/CMakeLists.txt | 1 + msg/control_allocator_status.msg | 21 + msg/tools/uorb_rtps_message_ids.yaml | 4 + .../px4_work_queue/WorkQueueManager.hpp | 3 +- src/modules/control_allocator/CMakeLists.txt | 48 ++ .../control_allocator/ControlAllocator.cpp | 528 ++++++++++++++++++ .../control_allocator/ControlAllocator.hpp | 179 ++++++ .../control_allocator_params.c | 312 +++++++++++ 8 files changed, 1095 insertions(+), 1 deletion(-) create mode 100644 msg/control_allocator_status.msg create mode 100644 src/modules/control_allocator/CMakeLists.txt create mode 100644 src/modules/control_allocator/ControlAllocator.cpp create mode 100644 src/modules/control_allocator/ControlAllocator.hpp create mode 100644 src/modules/control_allocator/control_allocator_params.c diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 27fc7d6f3239..450677b02b30 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -48,6 +48,7 @@ set(msg_files collision_constraints.msg collision_report.msg commander_state.msg + control_allocator_status.msg cpuload.msg debug_array.msg debug_key_value.msg diff --git a/msg/control_allocator_status.msg b/msg/control_allocator_status.msg new file mode 100644 index 000000000000..25ae4600f201 --- /dev/null +++ b/msg/control_allocator_status.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 torque_setpoint_achieved # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. +float32[3] allocated_torque # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved. +float32[3] unallocated_torque # Unallocated torque. Equal to 0 if the setpoint was achieved. + # Computed as: unallocated_torque = torque_setpoint - allocated_torque + +uint8 thrust_setpoint_achieved # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. +float32[3] allocated_thrust # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved. +float32[3] unallocated_thrust # Unallocated thrust. Equal to 0 if the setpoint was achieved. + # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust + +int8 ACTUATOR_SATURATION_OK = 0 # The actuator is not saturated +int8 ACTUATOR_SATURATION_UPPER_DYN = 1 # The actuator is saturated (with a value <= the desired value) because it cannot increase its value faster +int8 ACTUATOR_SATURATION_UPPER = 2 # The actuator is saturated (with a value <= the desired value) because it has reached its maximum value +int8 ACTUATOR_SATURATION_LOWER_DYN = -1 # The actuator is saturated (with a value >= the desired value) because it cannot decrease its value faster +int8 ACTUATOR_SATURATION_LOWER = -2 # The actuator is saturated (with a value >= the desired value) because it has reached its minimum value + +int8[16] actuator_saturation # Indicates actuator saturation status. + # Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved. + # Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value. diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 4a97ea6fef1b..b201fe0707ca 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -301,6 +301,10 @@ rtps: id: 134 - msg: vehicle_thrust_setpoint id: 135 + - msg: vehicle_actuator_setpoint + id: 136 + - msg: control_allocator_status + id: 137 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 949c19495aa8..82c34ae88d50 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -48,7 +48,8 @@ struct wq_config_t { namespace wq_configurations { -static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1664, 0}; // PX4 inner loop highest priority +// static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1664, 0}; // PX4 inner loop highest priority +static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 9500, 0}; // PX4 inner loop highest priority static constexpr wq_config_t SPI0{"wq:SPI0", 2336, -1}; static constexpr wq_config_t SPI1{"wq:SPI1", 2336, -2}; diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt new file mode 100644 index 000000000000..6628862bd802 --- /dev/null +++ b/src/modules/control_allocator/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(ControlAllocation) + +px4_add_module( + MODULE modules__control_allocator + MAIN control_allocator + COMPILE_FLAGS + SRCS + ControlAllocator.cpp + ControlAllocator.hpp + DEPENDS + mathlib + ControlAllocation + mixer + px4_work_queue +) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp new file mode 100644 index 000000000000..0a0f5f678fc3 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -0,0 +1,528 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocator.cpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#include "ControlAllocator.hpp" + +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; + +ControlAllocator::ControlAllocator() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + parameters_updated(); +} + +ControlAllocator::~ControlAllocator() +{ + free(_control_allocation); + perf_free(_loop_perf); +} + +bool +ControlAllocator::init() +{ + if (!_vehicle_torque_setpoint_sub.registerCallback()) { + PX4_ERR("vehicle_torque_setpoint callback registration failed!"); + return false; + } + + if (!_vehicle_thrust_setpoint_sub.registerCallback()) { + PX4_ERR("vehicle_thrust_setpoint callback registration failed!"); + return false; + } + + return true; +} + +void +ControlAllocator::parameters_updated() +{ + // Allocation method + // Do this first: in case a new method is loaded, it will be configured below + update_allocation_method(); + + // Minimum actuator values + matrix::Vector actuator_min; + actuator_min(0) = _param_ca_act0_min.get(); + actuator_min(1) = _param_ca_act1_min.get(); + actuator_min(2) = _param_ca_act2_min.get(); + actuator_min(3) = _param_ca_act3_min.get(); + actuator_min(4) = _param_ca_act4_min.get(); + actuator_min(5) = _param_ca_act5_min.get(); + actuator_min(6) = _param_ca_act6_min.get(); + actuator_min(7) = _param_ca_act7_min.get(); + actuator_min(8) = _param_ca_act8_min.get(); + actuator_min(9) = _param_ca_act9_min.get(); + actuator_min(10) = _param_ca_act10_min.get(); + actuator_min(11) = _param_ca_act11_min.get(); + actuator_min(12) = _param_ca_act12_min.get(); + actuator_min(13) = _param_ca_act13_min.get(); + actuator_min(14) = _param_ca_act14_min.get(); + actuator_min(15) = _param_ca_act15_min.get(); + _control_allocation->setActuatorMin(actuator_min); + + // Maximum actuator values + matrix::Vector actuator_max; + actuator_max(0) = _param_ca_act0_max.get(); + actuator_max(1) = _param_ca_act1_max.get(); + actuator_max(2) = _param_ca_act2_max.get(); + actuator_max(3) = _param_ca_act3_max.get(); + actuator_max(4) = _param_ca_act4_max.get(); + actuator_max(5) = _param_ca_act5_max.get(); + actuator_max(6) = _param_ca_act6_max.get(); + actuator_max(7) = _param_ca_act7_max.get(); + actuator_max(8) = _param_ca_act8_max.get(); + actuator_max(9) = _param_ca_act9_max.get(); + actuator_max(10) = _param_ca_act10_max.get(); + actuator_max(11) = _param_ca_act11_max.get(); + actuator_max(12) = _param_ca_act12_max.get(); + actuator_max(13) = _param_ca_act13_max.get(); + actuator_max(14) = _param_ca_act14_max.get(); + actuator_max(15) = _param_ca_act15_max.get(); + _control_allocation->setActuatorMax(actuator_max); + + // Control effectiveness + matrix::Matrix B; + + switch (_param_ca_airframe.get()) { + case 0: { + // quad_w + const float B_quad_w[NUM_AXES][NUM_ACTUATORS] = { + {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + B = matrix::Matrix(B_quad_w); + break; + } + + case 1: { + // hexa_x + const float B_hexa_x[NUM_AXES][NUM_ACTUATORS] = { + {-0.333333f, 0.333333f, 0.166667f, -0.166667f, -0.166667f, 0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.288675f, -0.288675f, 0.288675f, -0.288675f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.166667f, 0.166667f, -0.166667f, 0.166667f, 0.166667f, -0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f }, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f }, + {-0.166667f, -0.166667f, -0.166667f, -0.166667f, -0.166667f, -0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + }; + B = matrix::Matrix(B_hexa_x); + break; + } + + default: + // none + break; + } + + // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) + for (size_t j = 0; j < NUM_ACTUATORS; j++) { + if (actuator_min(j) >= actuator_max(j)) { + for (size_t i = 0; i < NUM_AXES; i++) { + B(i, j) = 0.0f; + } + + } + } + + // Assign control effectiveness matrix + _control_allocation->setEffectivenessMatrix(B); +} + +void +ControlAllocator::update_allocation_method() +{ + int method = _param_ca_method.get(); + + if (_allocation_method_id != method) { + + // Save current state + matrix::Vector actuator_sp; + + if (_control_allocation != nullptr) { + actuator_sp = _control_allocation->getAllocatedActuator(); + } + + // try to instanciate new allocation method + ControlAllocation *tmp = nullptr; + + switch (method) { + case 0: + tmp = new ControlAllocationPseudoInverse(); + break; + + default: + PX4_ERR("Unknown allocation method"); + tmp = nullptr; + break; + } + + // Replace previous method with new one + if (tmp == nullptr) { + // It did not work, forget about it + PX4_ERR("Control allocation init failed"); + _param_ca_method.set(_allocation_method_id); + + } else if (_control_allocation == tmp) { + // Nothing has changed, this should not happen + PX4_ERR("Control allocation init error"); + _allocation_method_id = method; + + } else { + // Successful update + PX4_INFO("Control allocation init success"); + + // Swap allocation methods + if (_control_allocation != nullptr) { + free(_control_allocation); + } + + _control_allocation = tmp; + + // Save method id + _allocation_method_id = method; + + // Configure new allocation method + _control_allocation->setCurrentActuatorSetpoint(actuator_sp); + } + } + + // Guard against bad initialization + if (_control_allocation == nullptr) { + PX4_ERR("Falling back to ControlAllocationPseudoInverse"); + _control_allocation = new ControlAllocationPseudoInverse(); + _allocation_method_id = 0; + } +} + +void +ControlAllocator::Run() +{ + if (should_exit()) { + _vehicle_torque_setpoint_sub.unregisterCallback(); + _vehicle_thrust_setpoint_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); + _last_run = now; + + bool do_update = false; + vehicle_torque_setpoint_s vehicle_torque_setpoint; + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + + // Run allocator on torque changes + if (_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)) { + _torque_sp = matrix::Vector3f(vehicle_torque_setpoint.xyz); + + do_update = true; + _timestamp_sample = vehicle_torque_setpoint.timestamp_sample; + + } + + // Also run allocator on thrust setpoint changes if the torque setpoint + // has not been updated for more than 5ms + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)) { + _thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz); + + if (dt > 5_ms) { + do_update = true; + _timestamp_sample = vehicle_thrust_setpoint.timestamp_sample; + } + } + + if (do_update) { + + // Set control setpoint vector + matrix::Vector c; + c(0) = _torque_sp(0); + c(1) = _torque_sp(1); + c(2) = _torque_sp(2); + c(3) = _thrust_sp(0); + c(4) = _thrust_sp(1); + c(5) = _thrust_sp(2); + _control_allocation->setControlSetpoint(c); + + // Do allocation + _control_allocation->allocate(); + + // Publish actuator setpoint and allocator status + publish_actuator_setpoint(); + publish_control_allocator_status(); + + // Publish on legacy topics for compatibility with + // the current mixer system and multicopter controller + // TODO: remove + publish_legacy_actuator_controls(); + publish_legacy_multirotor_motor_limits(); + } + + perf_end(_loop_perf); +} + +void +ControlAllocator::publish_actuator_setpoint() +{ + matrix::Vector actuator_sp = _control_allocation->getAllocatedActuator(); + + vehicle_actuator_setpoint_s vehicle_actuator_setpoint{}; + vehicle_actuator_setpoint.timestamp = hrt_absolute_time(); + vehicle_actuator_setpoint.timestamp_sample = _timestamp_sample; + actuator_sp.copyTo(vehicle_actuator_setpoint.actuator); + + _vehicle_actuator_setpoint_pub.publish(vehicle_actuator_setpoint); +} + +void +ControlAllocator::publish_control_allocator_status() +{ + control_allocator_status_s control_allocator_status{}; + control_allocator_status.timestamp = hrt_absolute_time(); + + // Allocated control + matrix::Vector allocated_control = _control_allocation->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 + matrix::Vector unallocated_control = _control_allocation->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); + + // Actuator saturation + matrix::Vector actuator_sp = _control_allocation->getAllocatedActuator(); + matrix::Vector actuator_min = _control_allocation->getActuatorMin(); + matrix::Vector actuator_max = _control_allocation->getActuatorMax(); + + for (size_t i = 0; i < NUM_ACTUATORS; i++) { + if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) { + control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_UPPER; + + } else if (actuator_sp(i) < (actuator_min(i) + FLT_EPSILON)) { + control_allocator_status.actuator_saturation[i] = control_allocator_status_s::ACTUATOR_SATURATION_LOWER; + } + } + + _control_allocator_status_pub.publish(control_allocator_status); +} + +void +ControlAllocator::publish_legacy_actuator_controls() +{ + // For compatibility with the current mixer system, + // publish normalized version on actuator_controls_4/5 + actuator_controls_s actuator_controls_4{}; + actuator_controls_s actuator_controls_5{}; + actuator_controls_4.timestamp = hrt_absolute_time(); + actuator_controls_5.timestamp = hrt_absolute_time(); + actuator_controls_4.timestamp_sample = _timestamp_sample; + actuator_controls_5.timestamp_sample = _timestamp_sample; + + matrix::Vector actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint(); + + for (size_t i = 0; i < 8; i++) { + actuator_controls_4.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i))) ? actuator_sp_normalized(i) : 0.0f; + actuator_controls_5.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i + 8))) ? actuator_sp_normalized(i + 8) : 0.0f; + } + + _actuator_controls_4_pub.publish(actuator_controls_4); + _actuator_controls_5_pub.publish(actuator_controls_5); +} + +void +ControlAllocator::publish_legacy_multirotor_motor_limits() +{ + multirotor_motor_limits_s multirotor_motor_limits{}; + multirotor_motor_limits.timestamp = hrt_absolute_time(); + multirotor_motor_limits.saturation_status = 1; + + // Motor saturation + matrix::Vector actuator_norm = _control_allocation->normalizeActuatorSetpoint(); + + for (size_t i = 0; i < NUM_ACTUATORS; i++) { + if (actuator_norm(i) > (1.0f - FLT_EPSILON)) { + multirotor_motor_limits.saturation_status |= (1 << 1); + + } else if (actuator_norm(i) < (-1.0f + FLT_EPSILON)) { + multirotor_motor_limits.saturation_status |= (1 << 2); + } + } + + // Control saturation + matrix::Vector unallocated_control = _control_allocation->getControlSetpoint() - + _control_allocation->getAllocatedControl(); + + // roll + if (unallocated_control(0) > FLT_EPSILON) { + multirotor_motor_limits.saturation_status |= (1 << 3); + + } else if (unallocated_control(0) < FLT_EPSILON) { + multirotor_motor_limits.saturation_status |= (1 << 4); + } + + // pitch + if (unallocated_control(1) > FLT_EPSILON) { + multirotor_motor_limits.saturation_status |= (1 << 5); + + } else if (unallocated_control(1) < FLT_EPSILON) { + multirotor_motor_limits.saturation_status |= (1 << 6); + } + + // yaw + if (unallocated_control(2) > FLT_EPSILON) { + multirotor_motor_limits.saturation_status |= (1 << 7); + + } else if (unallocated_control(2) < FLT_EPSILON) { + multirotor_motor_limits.saturation_status |= (1 << 8); + } + + // z thrust + if (unallocated_control(5) > FLT_EPSILON) { + multirotor_motor_limits.saturation_status |= (1 << 10); // 10 because Z points downward + + } else if (unallocated_control(5) < FLT_EPSILON) { + multirotor_motor_limits.saturation_status |= (1 << 9); // 9 because Z points downward + } + + _multirotor_motor_limits_pub.publish(multirotor_motor_limits); +} + +int ControlAllocator::task_spawn(int argc, char *argv[]) +{ + ControlAllocator *instance = new ControlAllocator(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int ControlAllocator::print_status() +{ + PX4_INFO("Running"); + PX4_INFO("Allocation method: %d", _allocation_method_id); + + perf_print_counter(_loop_perf); + + return 0; +} + +int ControlAllocator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int ControlAllocator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements control allocation. It takes torque and thrust setpoints +as inputs and outputs actuator setpoint messages. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +/** + * Control Allocator app start / stop handling function + */ +extern "C" __EXPORT int control_allocator_main(int argc, char *argv[]); + +int control_allocator_main(int argc, char *argv[]) +{ + return ControlAllocator::main(argc, argv); +} diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp new file mode 100644 index 000000000000..b62cc6c4e1b3 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocator.hpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class ControlAllocator : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + ControlAllocator(); + + virtual ~ControlAllocator(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + bool init(); + +private: + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + void update_allocation_method(); + + void publish_actuator_setpoint(); + void publish_control_allocator_status(); + + void publish_legacy_actuator_controls(); + void publish_legacy_multirotor_motor_limits(); + + static const uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; + static const uint8_t NUM_AXES = ControlAllocation::NUM_AXES; + + int _allocation_method_id{-1}; + ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations + + // Inputs + uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)}; /**< vehicle torque setpoint subscription */ + uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle thrust setpoint subscription */ + + // Outputs + uORB::Publication _vehicle_actuator_setpoint_pub{ORB_ID(vehicle_actuator_setpoint)}; /**< actuator setpoint publication */ + uORB::Publication _control_allocator_status_pub{ORB_ID(control_allocator_status)}; /**< actuator setpoint publication */ + + // actuator_controls publication handles (temporary hack to plug actuator_setpoint into the mixer system) + uORB::Publication _actuator_controls_4_pub{ORB_ID(actuator_controls_4)}; /**< actuator controls 4 publication */ + uORB::Publication _actuator_controls_5_pub{ORB_ID(actuator_controls_5)}; /**< actuator controls 5 publication */ + + // multirotor limits, should be replaced by something more generic + uORB::Publication _multirotor_motor_limits_pub{ORB_ID(multirotor_motor_limits)}; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ + uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; /**< airspeed subscription */ + + matrix::Vector3f _torque_sp; + matrix::Vector3f _thrust_sp; + + // float _battery_scale_factor{1.0f}; + // float _airspeed_scale_factor{1.0f}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + hrt_abstime _task_start{hrt_absolute_time()}; + hrt_abstime _last_run{0}; + hrt_abstime _timestamp_sample{0}; + + DEFINE_PARAMETERS( + (ParamInt) _param_ca_airframe, + (ParamInt) _param_ca_method, + (ParamBool) _param_ca_bat_scale_en, + (ParamBool) _param_ca_air_scale_en, + (ParamFloat) _param_ca_act0_min, + (ParamFloat) _param_ca_act1_min, + (ParamFloat) _param_ca_act2_min, + (ParamFloat) _param_ca_act3_min, + (ParamFloat) _param_ca_act4_min, + (ParamFloat) _param_ca_act5_min, + (ParamFloat) _param_ca_act6_min, + (ParamFloat) _param_ca_act7_min, + (ParamFloat) _param_ca_act8_min, + (ParamFloat) _param_ca_act9_min, + (ParamFloat) _param_ca_act10_min, + (ParamFloat) _param_ca_act11_min, + (ParamFloat) _param_ca_act12_min, + (ParamFloat) _param_ca_act13_min, + (ParamFloat) _param_ca_act14_min, + (ParamFloat) _param_ca_act15_min, + (ParamFloat) _param_ca_act0_max, + (ParamFloat) _param_ca_act1_max, + (ParamFloat) _param_ca_act2_max, + (ParamFloat) _param_ca_act3_max, + (ParamFloat) _param_ca_act4_max, + (ParamFloat) _param_ca_act5_max, + (ParamFloat) _param_ca_act6_max, + (ParamFloat) _param_ca_act7_max, + (ParamFloat) _param_ca_act8_max, + (ParamFloat) _param_ca_act9_max, + (ParamFloat) _param_ca_act10_max, + (ParamFloat) _param_ca_act11_max, + (ParamFloat) _param_ca_act12_max, + (ParamFloat) _param_ca_act13_max, + (ParamFloat) _param_ca_act14_max, + (ParamFloat) _param_ca_act15_max + ) + +}; diff --git a/src/modules/control_allocator/control_allocator_params.c b/src/modules/control_allocator/control_allocator_params.c new file mode 100644 index 000000000000..dae36e5e97e6 --- /dev/null +++ b/src/modules/control_allocator/control_allocator_params.c @@ -0,0 +1,312 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file control_allocator_params.c + * + * Parameters for the control allocator. + * + * @author Julien Lecoeur + */ + + +/** + * Airframe ID + * + * This is used to retrieve pre-computed control effectiveness matrix + * + * @value 0 Test + * @min 0 + * @max 0 + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_AIRFRAME, 0); + +/** + * Control allocation method + * + * @value 0 Pseudo-inverse with output clipping (default) + * @value 1 Pseudo-inverse with sequential desaturation technique + * @min 0 + * @max 1 + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_METHOD, 0); + +/** + * Battery power level scaler + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The copter + * should constantly behave as if it was fully charged with reduced max acceleration + * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_BAT_SCALE_EN, 0); + +/** + * Airspeed scaler + * + * This compensates for the variation of flap effectiveness with airspeed. + * + * @boolean + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_AIR_SCALE_EN, 0); + +/** + * Minimum value for actuator 0 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT0_MIN, 0.0); + +/** + * Minimum value for actuator 1 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT1_MIN, 0.0); + +/** + * Minimum value for actuator 2 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT2_MIN, 0.0); + +/** + * Minimum value for actuator 3 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT3_MIN, 0.0); + +/** + * Minimum value for actuator 4 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT4_MIN, 0.0); + +/** + * Minimum value for actuator 5 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT5_MIN, 0.0); + +/** + * Minimum value for actuator 6 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT6_MIN, 0.0); + +/** + * Minimum value for actuator 7 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT7_MIN, 0.0); + +/** + * Minimum value for actuator 8 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT8_MIN, 0.0); + +/** + * Minimum value for actuator 9 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT9_MIN, 0.0); + +/** + * Minimum value for actuator 10 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT10_MIN, 0.0); + +/** + * Minimum value for actuator 11 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT11_MIN, 0.0); + +/** + * Minimum value for actuator 12 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT12_MIN, 0.0); + +/** + * Minimum value for actuator 13 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT13_MIN, 0.0); + +/** + * Minimum value for actuator 14 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT14_MIN, 0.0); + +/** + * Minimum value for actuator 15 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT15_MIN, 0.0); + +/** + * Maximum value for actuator 0 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT0_MAX, 0.0); + +/** + * Maximum value for actuator 1 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT1_MAX, 0.0); + +/** + * Maximum value for actuator 2 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT2_MAX, 0.0); + +/** + * Maximum value for actuator 3 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT3_MAX, 0.0); + +/** + * Maximum value for actuator 4 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT4_MAX, 0.0); + +/** + * Maximum value for actuator 5 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT5_MAX, 0.0); + +/** + * Maximum value for actuator 6 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT6_MAX, 0.0); + +/** + * Maximum value for actuator 7 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT7_MAX, 0.0); + +/** + * Maximum value for actuator 8 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT8_MAX, 0.0); + +/** + * Maximum value for actuator 9 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT9_MAX, 0.0); + +/** + * Maximum value for actuator 10 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT10_MAX, 0.0); + +/** + * Maximum value for actuator 11 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT11_MAX, 0.0); + +/** + * Maximum value for actuator 12 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT12_MAX, 0.0); + +/** + * Maximum value for actuator 13 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT13_MAX, 0.0); + +/** + * Maximum value for actuator 14 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT14_MAX, 0.0); + +/** + * Maximum value for actuator 15 + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_ACT15_MAX, 0.0); From b952370470a39c74b023ddb3f049292d28683057 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 19 Nov 2019 15:17:45 +0100 Subject: [PATCH 014/129] control_allocation: implemented sequential desaturation class Signed-off-by: RomanBapst --- .../ControlAllocation/CMakeLists.txt | 1 + .../ControlAllocation/ControlAllocation.hpp | 11 ++ ...ontrolAllocationSequentialDesaturation.cpp | 123 ++++++++++++++++++ ...ontrolAllocationSequentialDesaturation.hpp | 89 +++++++++++++ .../control_allocator/ControlAllocator.cpp | 4 + .../control_allocator/ControlAllocator.hpp | 2 + .../control_allocator_params.c | 2 +- 7 files changed, 231 insertions(+), 1 deletion(-) create mode 100644 src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp create mode 100644 src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt index 8409dd6c2622..36a9239859b7 100644 --- a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt +++ b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt @@ -34,6 +34,7 @@ px4_add_library(ControlAllocation ControlAllocation.cpp ControlAllocationPseudoInverse.cpp + ControlAllocationSequentialDesaturation.cpp ) target_include_directories(ControlAllocation diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 4ef7aff5f052..8bfbf621df78 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -81,6 +81,17 @@ class ControlAllocation static const uint8_t NUM_ACTUATORS = vehicle_actuator_setpoint_s::NUM_ACTUATOR_SETPOINT; static const uint8_t NUM_AXES = 6; + typedef matrix::Vector ActuatorVector; + + enum ControlAxis { + ROLL = 0, + PITCH, + YAW, + THRUST_X, + THRUST_Y, + THRUST_Z + }; + /** * Allocate control setpoint to actuators * diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp new file mode 100644 index 000000000000..fa4362c61d23 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocationSequentialDesaturation.cpp + * + * @author Roman Bapst + */ + +#include "ControlAllocationSequentialDesaturation.hpp" + + + +void +ControlAllocationSequentialDesaturation::allocate() +{ + //Compute new gains if needed + updatePseudoInverse(); + + // Allocate + _actuator_sp = _A * _control_sp; + + // go through control axes from lowest to highest priority and unsaturate the actuators + for (unsigned i = 0; i < NUM_AXES; i++) { + desaturateActuators(_actuator_sp, _axis_prio_increasing[i]); + } + + // Clip + _actuator_sp = clipActuatorSetpoint(); + + // Compute achieved control + _control_allocated = _B * _actuator_sp; +} + +void ControlAllocationSequentialDesaturation::desaturateActuators( + ActuatorVector &actuator_sp, + const ControlAxis &axis) +{ + ActuatorVector desaturation_vector = getDesaturationVector(axis); + + float gain = computeDesaturationGain(desaturation_vector, actuator_sp); + + actuator_sp = actuator_sp + gain * desaturation_vector; + + gain = computeDesaturationGain(desaturation_vector, actuator_sp); + + actuator_sp = actuator_sp + 0.5f * gain * desaturation_vector; +} + +ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDesaturationVector(const ControlAxis &axis) +{ + ActuatorVector ret; + + for (unsigned i = 0; i < NUM_ACTUATORS; i++) { + ret(i) = _A(i, axis); + } + + return ret; +} + + +float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector, + const ActuatorVector &actuator_sp) +{ + float k_min = 0.f; + float k_max = 0.f; + + for (unsigned i = 0; i < NUM_ACTUATORS; i++) { + // Avoid division by zero. If desaturation_vector(i) is zero, there's nothing we can do to unsaturate anyway + if (fabsf(desaturation_vector(i)) < FLT_EPSILON) { + continue; + } + + if (actuator_sp(i) < _actuator_min(i)) { + float k = (_actuator_min(i) - actuator_sp(i)) / desaturation_vector(i); + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + } + + if (actuator_sp(i) > _actuator_max(i)) { + float k = (_actuator_max(i) - actuator_sp(i)) / desaturation_vector(i); + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + } + } + + // Reduce the saturation as much as possible + return k_min + k_max; +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp new file mode 100644 index 000000000000..6765a2532288 --- /dev/null +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocationSequentialDesaturation.hpp + * + * Control Allocation Algorithm which sequentially modifies control demands in order to + * eliminate the saturation of the actuator setpoint vector. + * + * + * @author Roman Bapst + */ + +#pragma once + +#include "ControlAllocationPseudoInverse.hpp" + +class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse +{ +public: + + ControlAllocationSequentialDesaturation() = default; + virtual ~ControlAllocationSequentialDesaturation() = default; + + void allocate() override; + +private: + + /** + * List of control axis used for desaturating the actuator vector. The desaturation logic will sequentially + * go through this list and if needed apply corrections to the demand of the corresponding axis in order to desaturate + * the actuator vector. + */ + ControlAxis _axis_prio_increasing [NUM_AXES] = {ControlAxis::YAW, ControlAxis::THRUST_Y, ControlAxis::THRUST_X, ControlAxis::THRUST_Z, ControlAxis::PITCH, ControlAxis::ROLL}; + + /** + * Desaturate actuator setpoint vector. + * + * @return Desaturated actuator setpoint vector. + */ + void desaturateActuators(ActuatorVector &actuator_sp, const ControlAxis &axis); + + /** + * Get desaturation vector. + * + * @param axis Control axis + * @return ActuatorVector Column of the pseudo-inverse matrix corresponding to the given control axis. + */ + ActuatorVector getDesaturationVector(const ControlAxis &axis); + + /** + * Compute desaturation gain. + * + * @param desaturation_vector Column of the pseudo-inverse matrix corresponding to a given control axis. + * @param Actuator setpoint vector. + * @return Gain which eliminates the saturation of the highest saturated actuator. + */ + float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp); +}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 0a0f5f678fc3..53b927d2de7a 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -199,6 +199,10 @@ ControlAllocator::update_allocation_method() tmp = new ControlAllocationPseudoInverse(); break; + case 1: + tmp = new ControlAllocationSequentialDesaturation(); + break; + default: PX4_ERR("Unknown allocation method"); tmp = nullptr; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index b62cc6c4e1b3..3758bf31cb31 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -43,6 +43,8 @@ #include #include +#include + #include #include #include diff --git a/src/modules/control_allocator/control_allocator_params.c b/src/modules/control_allocator/control_allocator_params.c index dae36e5e97e6..5706b2e79595 100644 --- a/src/modules/control_allocator/control_allocator_params.c +++ b/src/modules/control_allocator/control_allocator_params.c @@ -58,7 +58,7 @@ PARAM_DEFINE_INT32(CA_AIRFRAME, 0); * @value 0 Pseudo-inverse with output clipping (default) * @value 1 Pseudo-inverse with sequential desaturation technique * @min 0 - * @max 1 + * @max 2 * @group Control Allocation */ PARAM_DEFINE_INT32(CA_METHOD, 0); From 170a7ed379f3725e9a8fed3b60b358b25234af19 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 13:59:59 +0100 Subject: [PATCH 015/129] px4_sitl_default: build control_allocator --- boards/px4/sitl/default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 142b04c1b9c2..5b00a4189e7a 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -25,6 +25,7 @@ px4_add_board( attitude_estimator_q camera_feedback commander + control_allocator dataman ekf2 events From c316a19d558264011506a6ad245dd72ff1a91118 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 14:00:21 +0100 Subject: [PATCH 016/129] px4_fmu-v5_default: build control_allocator --- boards/px4/fmu-v5/default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 9bede18cfd6e..a39c199e4a06 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -65,6 +65,7 @@ px4_add_board( battery_status camera_feedback commander + control_allocator dataman ekf2 esc_battery From e87a98a889de1c7c8022d8aecf5e1931cbb4a382 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 14:00:56 +0100 Subject: [PATCH 017/129] px4_fmu-v2_default: build control_allocator --- boards/px4/fmu-v2/default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index b169e5c70739..5465c34266b9 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -72,6 +72,7 @@ px4_add_board( battery_status #camera_feedback commander + control_allocator dataman ekf2 #esc_battery From 71fc9fbc9acac5779a7d2abded30b53a77b6aecd Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 14:02:08 +0100 Subject: [PATCH 018/129] mc/fw/vtol startup scripts: start control_allocator --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 6 ++++++ ROMFS/px4fmu_common/init.d/rc.mc_apps | 5 +++++ ROMFS/px4fmu_common/init.d/rc.vtol_apps | 6 ++++++ 3 files changed, 17 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index c565aebf460c..2b9561e66972 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -16,6 +16,12 @@ ekf2 start fw_att_control start fw_pos_control_l1 start airspeed_selector start + +# +# Start Control Allocator +# +control_allocator start + # # Start Land Detector. # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 32f2f18df084..baf2d974a9ae 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -64,6 +64,11 @@ mc_att_control start mc_hover_thrust_estimator start mc_pos_control start +# +# Start Control Allocator +# +control_allocator start + # # Start Multicopter Land Detector. # diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 40a1e3df23e1..99234ee98681 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -27,6 +27,12 @@ mc_hover_thrust_estimator start fw_att_control start vtol fw_pos_control_l1 start vtol +# +# Start Control Allocator +# +control_allocator start + +# # Start Land Detector # Multicopter for now until we have something for VTOL # From 9a2afb37bc2c4daaff9174167ba5e0dac19a7252 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 1 Nov 2019 14:04:36 +0100 Subject: [PATCH 019/129] sitl iris: use control allocation module --- ROMFS/px4fmu_common/init.d-posix/10016_iris | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/10016_iris b/ROMFS/px4fmu_common/init.d-posix/10016_iris index 40e386de8117..9dd0fd417de5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/10016_iris +++ b/ROMFS/px4fmu_common/init.d-posix/10016_iris @@ -21,6 +21,17 @@ then param set VM_INERTIA_XX 0.0083 param set VM_INERTIA_YY 0.0083 param set VM_INERTIA_ZZ 0.0286 + + param set CA_AIRFRAME 0 + param set CA_ACT0_MIN 0.0 + param set CA_ACT1_MIN 0.0 + param set CA_ACT2_MIN 0.0 + param set CA_ACT3_MIN 0.0 + param set CA_ACT0_MAX 1.0 + param set CA_ACT1_MAX 1.0 + param set CA_ACT2_MAX 1.0 + param set CA_ACT3_MAX 1.0 fi -set MIXER quad_w +# set MIXER quad_w +set MIXER quad From b39e5b6bfea6968b7fa2660f28747f45cc770004 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 18 Nov 2019 18:32:37 +0100 Subject: [PATCH 020/129] sitl_typhoon: use control allocator module --- .../init.d-posix/6011_typhoon_h480 | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 index 4955f3b578a9..57ad3e2b49dd 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 @@ -25,8 +25,23 @@ then param set TRIG_MODE 4 param set MNT_MODE_IN 0 param set MAV_PROTO_VER 2 + + param set CA_AIRFRAME 1 + param set CA_ACT0_MIN 0.0 + param set CA_ACT1_MIN 0.0 + param set CA_ACT2_MIN 0.0 + param set CA_ACT3_MIN 0.0 + param set CA_ACT4_MIN 0.0 + param set CA_ACT5_MIN 0.0 + param set CA_ACT0_MAX 1.0 + param set CA_ACT1_MAX 1.0 + param set CA_ACT2_MAX 1.0 + param set CA_ACT3_MAX 1.0 + param set CA_ACT4_MAX 1.0 + param set CA_ACT5_MAX 1.0 fi set MAV_TYPE 13 -set MIXER hexa_x +# set MIXER hexa_x +set MIXER hexa From ba91f12950bf4e7033a065b8680f966e6f75f4ec Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sat, 16 Nov 2019 17:50:33 +0100 Subject: [PATCH 021/129] [WIP] test on PX4IO --- ROMFS/px4fmu_common/init.d/airframes/4001_quad_x | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x index 709e8197ef5a..70be5a3f433a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x @@ -23,5 +23,6 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_x +# set MIXER quad set PWM_OUT 1234 From c1df71509fb0fc7e4c2f88001fda829e0cc8902b Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 28 Nov 2019 09:35:44 +0100 Subject: [PATCH 022/129] control_allocator: fixed negative saturation status reporting Signed-off-by: RomanBapst --- src/modules/control_allocator/ControlAllocator.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 53b927d2de7a..d8aa469df875 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -432,7 +432,7 @@ ControlAllocator::publish_legacy_multirotor_motor_limits() if (unallocated_control(0) > FLT_EPSILON) { multirotor_motor_limits.saturation_status |= (1 << 3); - } else if (unallocated_control(0) < FLT_EPSILON) { + } else if (unallocated_control(0) < -FLT_EPSILON) { multirotor_motor_limits.saturation_status |= (1 << 4); } @@ -440,7 +440,7 @@ ControlAllocator::publish_legacy_multirotor_motor_limits() if (unallocated_control(1) > FLT_EPSILON) { multirotor_motor_limits.saturation_status |= (1 << 5); - } else if (unallocated_control(1) < FLT_EPSILON) { + } else if (unallocated_control(1) < -FLT_EPSILON) { multirotor_motor_limits.saturation_status |= (1 << 6); } @@ -448,7 +448,7 @@ ControlAllocator::publish_legacy_multirotor_motor_limits() if (unallocated_control(2) > FLT_EPSILON) { multirotor_motor_limits.saturation_status |= (1 << 7); - } else if (unallocated_control(2) < FLT_EPSILON) { + } else if (unallocated_control(2) < -FLT_EPSILON) { multirotor_motor_limits.saturation_status |= (1 << 8); } @@ -456,10 +456,11 @@ ControlAllocator::publish_legacy_multirotor_motor_limits() if (unallocated_control(5) > FLT_EPSILON) { multirotor_motor_limits.saturation_status |= (1 << 10); // 10 because Z points downward - } else if (unallocated_control(5) < FLT_EPSILON) { + } else if (unallocated_control(5) < -FLT_EPSILON) { multirotor_motor_limits.saturation_status |= (1 << 9); // 9 because Z points downward } + _multirotor_motor_limits_pub.publish(multirotor_motor_limits); } From 568492d25788e6f9e84fef12a84b5d4d59f9a4ed Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 25 Nov 2019 09:31:22 +0100 Subject: [PATCH 023/129] standard.cpp: set pusher thrust setpoint Signed-off-by: RomanBapst --- src/modules/vtol_att_control/standard.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 9f7a0b327e08..475d4e76bc40 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -311,6 +311,9 @@ void Standard::update_transition_state() } } + + _v_att_sp->thrust_body[0] = _pusher_throttle; + mc_weight = math::constrain(mc_weight, 0.0f, 1.0f); _mc_roll_weight = mc_weight; From 908d09b296cd28cb63848b657eb76f2fa063d505 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 25 Nov 2019 09:30:51 +0100 Subject: [PATCH 024/129] sitl standard vtol: set parameters required for CA module Signed-off-by: RomanBapst --- .../init.d-posix/1040_standard_vtol | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index 0de4a44facd4..05133c56a047 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -41,6 +41,30 @@ then param set VT_TYPE 2 param set VT_B_TRANS_DUR 8 + + param set MC_INERTIA_XX 1.0 + param set MC_INERTIA_YY 1.0 + param set MC_INERTIA_ZZ 1.0 + + param set CA_ACT0_MIN 0.0 + param set CA_ACT1_MIN 0.0 + param set CA_ACT2_MIN 0.0 + param set CA_ACT3_MIN 0.0 + param set CA_ACT0_MAX 1.0 + param set CA_ACT1_MAX 1.0 + param set CA_ACT2_MAX 1.0 + param set CA_ACT3_MAX 1.0 + + param set CA_ACT4_MAX 1.0 + param set CA_ACT4_MIN 0.0 + + param set CA_ACT5_MAX 1.0 + param set CA_ACT5_MIN -1.0 + param set CA_ACT6_MAX 1.0 + param set CA_ACT6_MIN -1.0 + param set CA_ACT7_MAX 1.0 + param set CA_ACT7_MIN -1.0 + fi set MAV_TYPE 22 From fdcf144d2e7b1db72cd72c44b788a58f42ab7240 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 25 Nov 2019 09:56:08 +0100 Subject: [PATCH 025/129] added direct mixer to fly stanard vtol with control allocation module Signed-off-by: RomanBapst --- ROMFS/px4fmu_common/mixers/standard_vtol.main.mix | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/standard_vtol.main.mix diff --git a/ROMFS/px4fmu_common/mixers/standard_vtol.main.mix b/ROMFS/px4fmu_common/mixers/standard_vtol.main.mix new file mode 100644 index 000000000000..cacc9f7fefeb --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/standard_vtol.main.mix @@ -0,0 +1,9 @@ +Control outputs +A: 0 +A: 1 +A: 2 +A: 3 +A: 4 +A: 5 +A: 6 +A: 7 From 961311cebcc6d73a82573b809231646bfd2dbc3b Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 25 Nov 2019 09:56:24 +0100 Subject: [PATCH 026/129] SITL: use direct mixer for standard vtol Signed-off-by: RomanBapst --- ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index 05133c56a047..6ed7f8739655 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -69,5 +69,4 @@ fi set MAV_TYPE 22 -set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix -set MIXER custom +set MIXER standard_vtol From 63ee029f9fa986f9a04b8b9a6500193830d1c111 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 28 Nov 2019 13:24:51 +0100 Subject: [PATCH 027/129] control_allocator: added control effectiveness matrix for fixed wing flight - monitor transition status and re-compute pseudo-inverse if necessary Signed-off-by: RomanBapst --- .../control_allocator/ControlAllocator.cpp | 64 +++++++++++++++---- .../control_allocator/ControlAllocator.hpp | 7 ++ 2 files changed, 59 insertions(+), 12 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index d8aa469df875..c637ce58ac98 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -126,21 +126,45 @@ ControlAllocator::parameters_updated() actuator_max(15) = _param_ca_act15_max.get(); _control_allocation->setActuatorMax(actuator_max); + matrix::Matrix B = getEffectinvenessMatrix(); + + // Assign control effectiveness matrix + _control_allocation->setEffectivenessMatrix(B); +} + +const matrix::Matrix +ControlAllocator::getEffectinvenessMatrix() +{ // Control effectiveness matrix::Matrix B; switch (_param_ca_airframe.get()) { case 0: { - // quad_w - const float B_quad_w[NUM_AXES][NUM_ACTUATORS] = { - {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; - B = matrix::Matrix(B_quad_w); + if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + // quad_w + const float B_quad_w[NUM_AXES][NUM_ACTUATORS] = { + {-0.5, 0.5, 0.5, -0.5, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5, -0.5, 0.5, -0.5, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + B = matrix::Matrix(B_quad_w); + + } else { + // fixed wing + const float B_fixed_wing[NUM_AXES][NUM_ACTUATORS] = { + { 0.0, 0.0, 0.0, 0.0, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.0, 0.0, 0.0, 0.0, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.0, 0.0, 0.0, 0.0, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + B = matrix::Matrix(B_fixed_wing); + } + break; } @@ -163,6 +187,10 @@ ControlAllocator::parameters_updated() break; } + + matrix::Vector actuator_max = _control_allocation->getActuatorMax(); + matrix::Vector actuator_min = _control_allocation->getActuatorMin(); + // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) for (size_t j = 0; j < NUM_ACTUATORS; j++) { if (actuator_min(j) >= actuator_max(j)) { @@ -173,8 +201,7 @@ ControlAllocator::parameters_updated() } } - // Assign control effectiveness matrix - _control_allocation->setEffectivenessMatrix(B); + return B; } void @@ -269,6 +296,19 @@ ControlAllocator::Run() parameters_updated(); } + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status = {}; + _vehicle_status_sub.update(&vehicle_status); + + if (_vehicle_type != vehicle_status.vehicle_type) { + _vehicle_type = vehicle_status.vehicle_type; + matrix::Matrix B = getEffectinvenessMatrix(); + + // Assign control effectiveness matrix + _control_allocation->setEffectivenessMatrix(B); + } + } + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const hrt_abstime now = hrt_absolute_time(); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 3758bf31cb31..0b07df8d1f3e 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -63,6 +63,7 @@ #include #include #include +#include class ControlAllocator : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -102,6 +103,9 @@ class ControlAllocator : public ModuleBase, public ModuleParam void publish_legacy_actuator_controls(); void publish_legacy_multirotor_motor_limits(); + const matrix::Matrix + getEffectinvenessMatrix(); + static const uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; static const uint8_t NUM_AXES = ControlAllocation::NUM_AXES; @@ -126,6 +130,7 @@ class ControlAllocator : public ModuleBase, public ModuleParam uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; /**< airspeed subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; matrix::Vector3f _torque_sp; matrix::Vector3f _thrust_sp; @@ -133,6 +138,8 @@ class ControlAllocator : public ModuleBase, public ModuleParam // float _battery_scale_factor{1.0f}; // float _airspeed_scale_factor{1.0f}; + uint8_t _vehicle_type{0}; /**< see vehicle_status_s::vehicle_type */ + perf_counter_t _loop_perf; /**< loop duration performance counter */ hrt_abstime _task_start{hrt_absolute_time()}; From 1155f2ece30fd85b598af4421bc7eba9cb413479 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 28 Nov 2019 13:25:40 +0100 Subject: [PATCH 028/129] control allocation: fomatting fix Signed-off-by: RomanBapst --- .../ControlAllocationSequentialDesaturation.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index fa4362c61d23..8af658fb70c3 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -77,7 +77,8 @@ void ControlAllocationSequentialDesaturation::desaturateActuators( actuator_sp = actuator_sp + 0.5f * gain * desaturation_vector; } -ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDesaturationVector(const ControlAxis &axis) +ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDesaturationVector( + const ControlAxis &axis) { ActuatorVector ret; From 73fd294ec4c45ee9886bf2b1ef5c7c0f3ed3e800 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 14:53:19 +0100 Subject: [PATCH 029/129] control_allocator: maintain quad_w effectiveness --- .../control_allocator/ControlAllocator.cpp | 56 ++++++++++++------- 1 file changed, 36 insertions(+), 20 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index c637ce58ac98..ecd4e43eb9f6 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -139,10 +139,40 @@ ControlAllocator::getEffectinvenessMatrix() matrix::Matrix B; switch (_param_ca_airframe.get()) { + case 0: { - if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + // quad_w + const float B_quad_w[NUM_AXES][NUM_ACTUATORS] = { // quad_w - const float B_quad_w[NUM_AXES][NUM_ACTUATORS] = { + {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + B = matrix::Matrix(B_quad_w); + break; + } + + case 1: { + // hexa_x + const float B_hexa_x[NUM_AXES][NUM_ACTUATORS] = { + {-0.333333f, 0.333333f, 0.166667f, -0.166667f, -0.166667f, 0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.288675f, -0.288675f, 0.288675f, -0.288675f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.166667f, 0.166667f, -0.166667f, 0.166667f, 0.166667f, -0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f }, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f }, + {-0.166667f, -0.166667f, -0.166667f, -0.166667f, -0.166667f, -0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + }; + B = matrix::Matrix(B_hexa_x); + break; + } + + case 2: { + if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + // standard_vtol_hover + const float B_standard_vtol_hover[NUM_AXES][NUM_ACTUATORS] = { {-0.5, 0.5, 0.5, -0.5, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.5, -0.5, 0.5, -0.5, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, @@ -150,11 +180,11 @@ ControlAllocator::getEffectinvenessMatrix() { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; - B = matrix::Matrix(B_quad_w); + B = matrix::Matrix(B_standard_vtol_hover); } else { - // fixed wing - const float B_fixed_wing[NUM_AXES][NUM_ACTUATORS] = { + // standard_vtol_fixed_wing + const float B_standard_vtol_fixed_wing[NUM_AXES][NUM_ACTUATORS] = { { 0.0, 0.0, 0.0, 0.0, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.0, 0.0, 0.0, 0.0, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.0, 0.0, 0.0, 0.0, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, @@ -162,26 +192,12 @@ ControlAllocator::getEffectinvenessMatrix() { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.0f, 0.0f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; - B = matrix::Matrix(B_fixed_wing); + B = matrix::Matrix(B_standard_vtol_fixed_wing); } break; } - case 1: { - // hexa_x - const float B_hexa_x[NUM_AXES][NUM_ACTUATORS] = { - {-0.333333f, 0.333333f, 0.166667f, -0.166667f, -0.166667f, 0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.288675f, -0.288675f, 0.288675f, -0.288675f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.166667f, 0.166667f, -0.166667f, 0.166667f, 0.166667f, -0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f }, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f }, - {-0.166667f, -0.166667f, -0.166667f, -0.166667f, -0.166667f, -0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - }; - B = matrix::Matrix(B_hexa_x); - break; - } - default: // none break; From bc30f49c82c06e44b680f6fc85a27112ffeba77e Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 14:54:30 +0100 Subject: [PATCH 030/129] sitl standard_vtol: set param CA_AIRFRAME --- ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index 6ed7f8739655..f4db1130b7dc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -46,6 +46,8 @@ then param set MC_INERTIA_YY 1.0 param set MC_INERTIA_ZZ 1.0 + param set CA_AIRFRAME 2 + param set CA_ACT0_MIN 0.0 param set CA_ACT1_MIN 0.0 param set CA_ACT2_MIN 0.0 From 236a2896cc122ad6c0130c68d7d5cc254e420aaa Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 15:40:35 +0100 Subject: [PATCH 031/129] Fix standard_vtol param names --- ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index f4db1130b7dc..08493535210f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -42,9 +42,9 @@ then param set VT_B_TRANS_DUR 8 - param set MC_INERTIA_XX 1.0 - param set MC_INERTIA_YY 1.0 - param set MC_INERTIA_ZZ 1.0 + param set VM_INERTIA_XX 1.0 + param set VM_INERTIA_YY 1.0 + param set VM_INERTIA_ZZ 1.0 param set CA_AIRFRAME 2 From ed822acac76b3a1be2c7f123e3107e9b95441f45 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 19:31:50 +0100 Subject: [PATCH 032/129] fmu-v2: do not build control_allocator --- boards/px4/fmu-v2/default.cmake | 1 - 1 file changed, 1 deletion(-) diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 5465c34266b9..b169e5c70739 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -72,7 +72,6 @@ px4_add_board( battery_status #camera_feedback commander - control_allocator dataman ekf2 #esc_battery From 266b819dade43b26bc9970b33127ecb02915d9a6 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 19:32:56 +0100 Subject: [PATCH 033/129] Add angular_velocity_controller module --- .../AngularVelocityControl.cpp | 117 ++++++ .../AngularVelocityControl.hpp | 147 ++++++++ .../AngularVelocityControlTest.cpp | 45 +++ .../AngularVelocityControl/CMakeLists.txt | 44 +++ .../AngularVelocityController.cpp | 341 ++++++++++++++++++ .../AngularVelocityController.hpp | 169 +++++++++ .../CMakeLists.txt | 48 +++ .../angular_velocity_controller_params.c | 303 ++++++++++++++++ .../vehicle_model_params.c | 109 ++++++ 9 files changed, 1323 insertions(+) create mode 100644 src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp create mode 100644 src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp create mode 100644 src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp create mode 100644 src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt create mode 100644 src/modules/angular_velocity_controller/AngularVelocityController.cpp create mode 100644 src/modules/angular_velocity_controller/AngularVelocityController.hpp create mode 100644 src/modules/angular_velocity_controller/CMakeLists.txt create mode 100644 src/modules/angular_velocity_controller/angular_velocity_controller_params.c create mode 100644 src/modules/angular_velocity_controller/vehicle_model_params.c diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp new file mode 100644 index 000000000000..15f403d2e03e --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file AngularVelocityControl.cpp + */ + +#include +#include + +using namespace matrix; + +void AngularVelocityControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +{ + _gain_p = P; + _gain_i = I; + _gain_d = D; +} + +void AngularVelocityControl::setSaturationStatus(const matrix::Vector &saturation_positive, const matrix::Vector &saturation_negative) +{ + _saturation_positive = saturation_positive; + _saturation_negative = saturation_negative; +} + +void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_acceleration, const Vector3f &angular_velocity_sp, const float dt, const bool landed) +{ + // angular rates error + Vector3f angular_velocity_error = angular_velocity_sp - angular_velocity; + + // P + D control + _angular_accel_sp = _gain_p.emult(angular_velocity_error) - _gain_d.emult(angular_acceleration); + + // I + FF control + Vector3f torque_feedforward = _angular_velocity_int + _gain_ff.emult(angular_velocity_sp); + + // compute torque setpoint + _torque_sp = ( + _inertia * _angular_accel_sp + + torque_feedforward + + angular_velocity.cross(_inertia * angular_velocity) + ); + + // update integral only if we are not landed + if (!landed) { + updateIntegral(angular_velocity_error, dt); + } +} + +void AngularVelocityControl::updateIntegral(Vector3f &angular_velocity_error, const float dt) +{ + for (int i = 0; i < 3; i++) { + // prevent further positive control saturation + if (_saturation_positive(i)) { + angular_velocity_error(i) = math::min(angular_velocity_error(i), 0.f); + } + + // prevent further negative control saturation + if (_saturation_negative(i)) { + angular_velocity_error(i) = math::max(angular_velocity_error(i), 0.f); + } + + // I term factor: reduce the I gain with increasing rate error. + // This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint + // change (noticeable in a bounce-back effect after a flip). + // The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should: + // with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect), + // and up to 200 deg error leads to <25% reduction of I. + float i_factor = angular_velocity_error(i) / math::radians(400.f); + i_factor = math::max(0.0f, 1.f - i_factor * i_factor); + + // Perform the integration using a first order method + float angular_velocity_i = _angular_velocity_int(i) + i_factor * _gain_i(i) * angular_velocity_error(i) * dt; + + // do not propagate the result if out of range or invalid + if (PX4_ISFINITE(angular_velocity_i)) { + _angular_velocity_int(i) = math::constrain(angular_velocity_i, -_lim_int(i), _lim_int(i)); + } + } +} + +void AngularVelocityControl::reset() +{ + _angular_velocity_int.zero(); + _torque_sp.zero(); + _angular_accel_sp.zero(); +} diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp new file mode 100644 index 000000000000..54e6cb41bca0 --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file AngularVelocityControl.hpp + * + * PID 3 axis angular velocity control. + */ + +#pragma once + +#include + +#include + +class AngularVelocityControl +{ +public: + AngularVelocityControl() = default; + ~AngularVelocityControl() = default; + + /** + * Set the control gains + * @param P 3D vector of proportional gains for body x,y,z axis + * @param I 3D vector of integral gains + * @param D 3D vector of derivative gains + */ + void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); + + /** + * Set the mximum absolute value of the integrator for all axes + * @param integrator_limit limit value for all axes x, y, z + */ + void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; }; + + /** + * Set direct angular velocity setpoint to torque feed forward gain + * @see _gain_ff + * @param FF 3D vector of feed forward gains for body x,y,z axis + */ + void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; }; + + /** + * Set inertia matrix + * @see _inertia + * @param inertia inertia matrix + */ + void setInertiaMatrix(const matrix::Matrix3f &inertia) { _inertia = inertia; }; + + /** + * Set saturation status + * @see _saturation_positive + * @see _saturation_negative + * @param saturation_positive positive saturation + * @param saturation_negative negative saturation + */ + void setSaturationStatus(const matrix::Vector &saturation_positive, const matrix::Vector &saturation_negative); + + /** + * Run one control loop cycle calculation + * @param angular_velocity estimation of the current vehicle angular velocity + * @param angular_acceleration estimation of the current vehicle angular acceleration + * @param angular_velocity_sp desired vehicle angular velocity setpoint + * @param dt elapsed time since last update + * @param landed whether the vehicle is on the ground + */ + void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration, const matrix::Vector3f &angular_velocity_sp, const float dt, const bool landed); + + /** + * Get the desired angular acceleration + * @see _angular_accel_sp + */ + const matrix::Vector3f &getAngularAccelerationSetpoint() {return _angular_accel_sp;}; + + /** + * Get the torque vector to apply to the vehicle + * @see _torque_sp + */ + const matrix::Vector3f &getTorqueSetpoint() {return _torque_sp;}; + + /** + * Get the integral term + * @see _torque_sp + */ + const matrix::Vector3f &getIntegral() {return _angular_velocity_int;}; + + /** + * Set the integral term to 0 to prevent windup + * @see _angular_velocity_int + */ + void resetIntegral() { _angular_velocity_int.zero(); } + + /** + * ReSet the controller state + */ + void reset(); + +private: + void updateIntegral(matrix::Vector3f &angular_velocity_error, const float dt); + + // Gains + matrix::Vector3f _gain_p; ///< proportional gain for all axes x, y, z + matrix::Vector3f _gain_i; ///< integral gain + matrix::Vector3f _gain_d; ///< derivative gain + matrix::Vector3f _lim_int; ///< integrator term maximum absolute value + matrix::Vector3f _gain_ff; ///< direct angular velocity to torque feed forward gain + matrix::Matrix3f _inertia{matrix::eye()}; ///< inertia matrix + + // States + matrix::Vector3f _angular_velocity_int; + matrix::Vector _saturation_positive; + matrix::Vector _saturation_negative; + + // Output + matrix::Vector3f _angular_accel_sp; //< Angular acceleration setpoint computed using P and D gains + matrix::Vector3f _torque_sp; //< Torque setpoint to apply to the vehicle +}; diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp new file mode 100644 index 000000000000..9f4c85af469e --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControlTest.cpp @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +using namespace matrix; + +TEST(AngularVelocityControlTest, AllZeroCase) +{ + AngularVelocityControl control; + control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false); + Vector3f torque = control.getTorqueSetpoint(); + EXPECT_EQ(torque, Vector3f()); +} diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt b/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt new file mode 100644 index 000000000000..bf129205eb67 --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(AngularVelocityControl + AngularVelocityControl.cpp +) +target_include_directories(AngularVelocityControl + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(AngularVelocityControl PRIVATE mathlib) + +px4_add_unit_gtest(SRC AngularVelocityControlTest.cpp LINKLIBS AngularVelocityControl) diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp new file mode 100644 index 000000000000..12a4b1c1dcd7 --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -0,0 +1,341 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "AngularVelocityController.hpp" + +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +AngularVelocityController::AngularVelocityController() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + parameters_updated(); +} + +AngularVelocityController::~AngularVelocityController() +{ + perf_free(_loop_perf); +} + +bool +AngularVelocityController::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle_angular_velocity callback registration failed!"); + return false; + } + + return true; +} + +void +AngularVelocityController::parameters_updated() +{ + // Control parameters + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const Vector3f k_gains = Vector3f(_param_avc_x_k.get(), _param_avc_y_k.get(), _param_avc_z_k.get()); + + _control.setGains( + k_gains.emult(Vector3f(_param_avc_x_p.get(), _param_avc_y_p.get(), _param_avc_z_p.get())), + k_gains.emult(Vector3f(_param_avc_x_i.get(), _param_avc_y_i.get(), _param_avc_z_i.get())), + k_gains.emult(Vector3f(_param_avc_x_d.get(), _param_avc_y_d.get(), _param_avc_z_d.get()))); + + _control.setIntegratorLimit( + Vector3f(_param_avc_x_i_lim.get(), _param_avc_y_i_lim.get(), _param_avc_z_i_lim.get())); + + _control.setFeedForwardGain( + Vector3f(_param_avc_x_ff.get(), _param_avc_y_ff.get(), _param_avc_z_ff.get())); + + // inertia matrix + const float inertia[3][3] = { + {_param_vm_inertia_xx.get(), _param_vm_inertia_xy.get(), _param_vm_inertia_xz.get()}, + {_param_vm_inertia_xy.get(), _param_vm_inertia_yy.get(), _param_vm_inertia_yz.get()}, + {_param_vm_inertia_xz.get(), _param_vm_inertia_yz.get(), _param_vm_inertia_zz.get()} + }; + _control.setInertiaMatrix(matrix::Matrix3f(inertia)); + +} + +void +AngularVelocityController::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + /* run controller on gyro changes */ + vehicle_angular_velocity_s vehicle_angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) { + const hrt_abstime now = hrt_absolute_time(); + _timestamp_sample = vehicle_angular_velocity.timestamp_sample; + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); + _last_run = now; + + const Vector3f angular_velocity{vehicle_angular_velocity.xyz}; + + /* check for updates in other topics */ + _vehicle_status_sub.update(&_vehicle_status); + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + } + } + + // check angular acceleration topic + vehicle_angular_acceleration_s vehicle_angular_acceleration; + + if (_vehicle_angular_acceleration_sub.update(&vehicle_angular_acceleration)) { + _angular_acceleration = Vector3f(vehicle_angular_acceleration.xyz); + } + + // check rates setpoint topic + vehicle_rates_setpoint_s vehicle_rates_setpoint; + + if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + _angular_velocity_sp(0) = vehicle_rates_setpoint.roll; + _angular_velocity_sp(1) = vehicle_rates_setpoint.pitch; + _angular_velocity_sp(2) = vehicle_rates_setpoint.yaw; + _thrust_sp = Vector3f(vehicle_rates_setpoint.thrust_body); + } + + // run the controller + bool run_control = ((_vehicle_control_mode.flag_control_rates_enabled + || _vehicle_control_mode.flag_control_termination_enabled) + && (!_vehicle_status.is_vtol + || (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) + || (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode))); + + if (run_control) { + + if (_vehicle_control_mode.flag_control_termination_enabled) { + // reset controller + _control.reset(); + + } else { + // reset integral if disarmed + if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _control.resetIntegral(); + } + + // update saturation status from mixer feedback + if (_control_allocator_status_sub.updated()) { + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_sub.copy(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (not control_allocator_status.torque_setpoint_achieved) { + for (size_t i=0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } + } + + _control.setSaturationStatus(saturation_positive, saturation_negative); + } + } + + // run rate controller + _control.update(angular_velocity, _angular_acceleration, _angular_velocity_sp, dt, _maybe_landed || _landed); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + Vector3f integral = _control.getIntegral(); + rate_ctrl_status.timestamp = hrt_absolute_time(); + rate_ctrl_status.rollspeed_integ = integral(0); + rate_ctrl_status.pitchspeed_integ = integral(1); + rate_ctrl_status.yawspeed_integ = integral(2); + _rate_ctrl_status_pub.publish(rate_ctrl_status); + } + + // publish controller output + publish_angular_acceleration_setpoint(); + publish_torque_setpoint(); + publish_thrust_setpoint(); + } + } + + perf_end(_loop_perf); +} + +void +AngularVelocityController::publish_angular_acceleration_setpoint() +{ + Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint(); + + vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {}; + v_angular_accel_sp.timestamp = hrt_absolute_time(); + v_angular_accel_sp.timestamp_sample = _timestamp_sample; + v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f; + v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f; + v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f; + + _vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp); +} + +void +AngularVelocityController::publish_torque_setpoint() +{ + Vector3f torque_sp = _control.getTorqueSetpoint(); + + 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] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f; + v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f; + v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f; + + _vehicle_torque_setpoint_pub.publish(v_torque_sp); +} + +void +AngularVelocityController::publish_thrust_setpoint() +{ + vehicle_thrust_setpoint_s v_thrust_sp = {}; + v_thrust_sp.timestamp = hrt_absolute_time(); + v_thrust_sp.timestamp_sample = _timestamp_sample; + v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f; + v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f; + v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f; + + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); +} + +int AngularVelocityController::task_spawn(int argc, char *argv[]) +{ + AngularVelocityController *instance = new AngularVelocityController(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int AngularVelocityController::print_status() +{ + PX4_INFO("Running"); + + perf_print_counter(_loop_perf); + + return 0; +} + +int AngularVelocityController::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int AngularVelocityController::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the angular velocity controller. +It takes angular velocity setpoints and measured angular +velocity as inputs and outputs actuator setpoints. + +The controller has a PID loop for angular rate error. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +/** + * Angular velocity controller app start / stop handling function + */ +extern "C" __EXPORT int angular_velocity_controller_main(int argc, char *argv[]); + +int angular_velocity_controller_main(int argc, char *argv[]) +{ + return AngularVelocityController::main(argc, argv); +} diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.hpp b/src/modules/angular_velocity_controller/AngularVelocityController.hpp new file mode 100644 index 000000000000..c62002899453 --- /dev/null +++ b/src/modules/angular_velocity_controller/AngularVelocityController.hpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +class AngularVelocityController : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + AngularVelocityController(); + + virtual ~AngularVelocityController(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + bool init(); + +private: + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + void vehicle_status_poll(); + + void publish_angular_acceleration_setpoint(); + void publish_torque_setpoint(); + void publish_thrust_setpoint(); + void publish_actuator_controls(); + + AngularVelocityControl _control; ///< class for control calculations + + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; /**< motor limits subscription */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; /**< vehicle angular acceleration subscription */ + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::Publication _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */ + uORB::Publication _vehicle_angular_acceleration_setpoint_pub{ORB_ID(vehicle_angular_acceleration_setpoint)}; /**< angular acceleration setpoint publication */ + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; /**< thrust setpoint publication */ + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; /**< torque setpoint publication */ + + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_status_s _vehicle_status{}; + + bool _landed{true}; + bool _maybe_landed{true}; + + float _battery_status_scale{0.0f}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + matrix::Vector3f _angular_velocity_sp; /**< angular velocity setpoint */ + matrix::Vector3f _angular_acceleration; /**< angular acceleration (estimated) */ + matrix::Vector3f _thrust_sp; /**< thrust setpoint */ + + bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ + + hrt_abstime _task_start{hrt_absolute_time()}; + hrt_abstime _last_run{0}; + hrt_abstime _timestamp_sample{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_avc_x_p, + (ParamFloat) _param_avc_x_i, + (ParamFloat) _param_avc_x_i_lim, + (ParamFloat) _param_avc_x_d, + (ParamFloat) _param_avc_x_ff, + (ParamFloat) _param_avc_x_k, + + (ParamFloat) _param_avc_y_p, + (ParamFloat) _param_avc_y_i, + (ParamFloat) _param_avc_y_i_lim, + (ParamFloat) _param_avc_y_d, + (ParamFloat) _param_avc_y_ff, + (ParamFloat) _param_avc_y_k, + + (ParamFloat) _param_avc_z_p, + (ParamFloat) _param_avc_z_i, + (ParamFloat) _param_avc_z_i_lim, + (ParamFloat) _param_avc_z_d, + (ParamFloat) _param_avc_z_ff, + (ParamFloat) _param_avc_z_k, + + (ParamFloat) _param_vm_inertia_xx, + (ParamFloat) _param_vm_inertia_yy, + (ParamFloat) _param_vm_inertia_zz, + (ParamFloat) _param_vm_inertia_xy, + (ParamFloat) _param_vm_inertia_xz, + (ParamFloat) _param_vm_inertia_yz + ) + +}; + diff --git a/src/modules/angular_velocity_controller/CMakeLists.txt b/src/modules/angular_velocity_controller/CMakeLists.txt new file mode 100644 index 000000000000..d99caeb0cc4e --- /dev/null +++ b/src/modules/angular_velocity_controller/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(AngularVelocityControl) + +px4_add_module( + MODULE modules__angular_velocity_control + MAIN angular_velocity_controller + COMPILE_FLAGS + SRCS + AngularVelocityController.cpp + AngularVelocityController.hpp + DEPENDS + circuit_breaker + mathlib + AngularVelocityControl + px4_work_queue + ) diff --git a/src/modules/angular_velocity_controller/angular_velocity_controller_params.c b/src/modules/angular_velocity_controller/angular_velocity_controller_params.c new file mode 100644 index 000000000000..112ffdc765cc --- /dev/null +++ b/src/modules/angular_velocity_controller/angular_velocity_controller_params.c @@ -0,0 +1,303 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file angular_velocity_controller_params.c + * Parameters for angular velocity controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + * @author Julien Lecoeur + */ + +/** + * Body X axis angular velocity P gain + * + * Body X axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @unit 1/s + * @min 0.0 + * @max 20.0 + * @decimal 3 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_X_P, 18.f); + +/** + * Body X axis angular velocity I gain + * + * Body X axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @unit N.m/rad + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_X_I, 0.2f); + +/** + * Body X axis angular velocity integrator limit + * + * Body X axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes. + * + * @unit N.m + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_X_I_LIM, 0.3f); + +/** + * Body X axis angular velocity D gain + * + * Body X axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @unit 1 + * @min 0.0 + * @max 2.0 + * @decimal 4 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_X_D, 0.36f); + +/** + * Body X axis angular velocity feedforward gain + * + * Improves tracking performance. + * + * @unit N.m.s/rad + * @min 0.0 + * @decimal 4 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_X_FF, 0.0f); + +/** + * Body X axis angular velocity controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = AVC_X_K * (AVC_X_P * error + * + AVC_X_I * error_integral + * + AVC_X_D * error_derivative) + * Set AVC_X_P=1 to implement a PID in the ideal form. + * Set AVC_X_K=1 to implement a PID in the parallel form. + * + * @unit 1 + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_X_K, 1.0f); + +/** + * Body Y axis angular velocity P gain + * + * Body Y axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @unit 1/s + * @min 0.0 + * @max 20.0 + * @decimal 3 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Y_P, 18.f); + +/** + * Body Y axis angular velocity I gain + * + * Body Y axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @unit N.m/rad + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Y_I, 0.2f); + +/** + * Body Y axis angular velocity integrator limit + * + * Body Y axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes. + * + * @unit N.m + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Y_I_LIM, 0.3f); + +/** + * Body Y axis angular velocity D gain + * + * Body Y axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @unit 1 + * @min 0.0 + * @max 2.0 + * @decimal 4 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Y_D, 0.36f); + +/** + * Body Y axis angular velocity feedforward + * + * Improves tracking performance. + * + * @unit N.m.s/rad + * @min 0.0 + * @decimal 4 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Y_FF, 0.0f); + +/** + * Body Y axis angular velocity controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = AVC_Y_K * (AVC_Y_P * error + * + AVC_Y_I * error_integral + * + AVC_Y_D * error_derivative) + * Set AVC_Y_P=1 to implement a PID in the ideal form. + * Set AVC_Y_K=1 to implement a PID in the parallel form. + * + * @unit 1 + * @min 0.0 + * @max 20.0 + * @decimal 4 + * @increment 0.0005 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Y_K, 1.0f); + +/** + * Body Z axis angular velocity P gain + * + * Body Z axis angular velocity proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @unit 1/s + * @min 0.0 + * @max 20.0 + * @decimal 2 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Z_P, 7.f); + +/** + * Body Z axis angular velocity I gain + * + * Body Z axis angular velocity integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @unit N.m/rad + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Z_I, 0.1f); + +/** + * Body Z axis angular velocity integrator limit + * + * Body Z axis angular velocity integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes. + * + * @unit N.m + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Z_I_LIM, 0.30f); + +/** + * Body Z axis angular velocity D gain + * + * Body Z axis angular velocity differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @unit 1 + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Z_D, 0.0f); + +/** + * Body Z axis angular velocity feedforward + * + * Improves tracking performance. + * + * @unit N.m.s/rad + * @min 0.0 + * @decimal 4 + * @increment 0.01 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Z_FF, 0.0f); + +/** + * Body Z axis angular velocity controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = AVC_Z_K * (AVC_Z_P * error + * + AVC_Z_I * error_integral + * + AVC_Z_D * error_derivative) + * Set AVC_Z_P=1 to implement a PID in the ideal form. + * Set AVC_Z_K=1 to implement a PID in the parallel form. + * + * @unit 1 + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Angular Velocity Control + */ +PARAM_DEFINE_FLOAT(AVC_Z_K, 1.0f); diff --git a/src/modules/angular_velocity_controller/vehicle_model_params.c b/src/modules/angular_velocity_controller/vehicle_model_params.c new file mode 100644 index 000000000000..df133a8b5ceb --- /dev/null +++ b/src/modules/angular_velocity_controller/vehicle_model_params.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_model_params.c + * Parameters for vehicle model. + * + * @author Julien Lecoeur + */ + +/** + * Mass + * + * @unit kg + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_MASS, 1.f); + +/** + * Inertia matrix, XX component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_XX, 0.01f); + +/** + * Inertia matrix, YY component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_YY, 0.01f); + +/** + * Inertia matrix, ZZ component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_ZZ, 0.01f); + +/** + * Inertia matrix, XY component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_XY, 0.f); + +/** + * Inertia matrix, XZ component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_XZ, 0.f); + +/** + * Inertia matrix, YZ component + * + * @unit kg.m^2 + * @decimal 5 + * @increment 0.00001 + * @group Vehicle Model + */ +PARAM_DEFINE_FLOAT(VM_INERTIA_YZ, 0.f); From 836f0c726bc7ef7745bfc98416c81d607d1d15eb Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 19:33:25 +0100 Subject: [PATCH 034/129] sitl iris: revert default gains --- ROMFS/px4fmu_common/init.d-posix/10016_iris | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/10016_iris b/ROMFS/px4fmu_common/init.d-posix/10016_iris index 9dd0fd417de5..a801c9feb3f8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/10016_iris +++ b/ROMFS/px4fmu_common/init.d-posix/10016_iris @@ -11,13 +11,6 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF = yes ] then - param set MC_ROLLRATE_P 18.0 - param set MC_ROLLRATE_D 0.36 - param set MC_PITCHRATE_P 18.0 - param set MC_PITCHRATE_D 0.36 - param set MC_YAWRATE_P 7.0 - param set MC_YAWRATE_D 0.0 - param set VM_INERTIA_XX 0.0083 param set VM_INERTIA_YY 0.0083 param set VM_INERTIA_ZZ 0.0286 @@ -32,6 +25,7 @@ then param set CA_ACT2_MAX 1.0 param set CA_ACT3_MAX 1.0 fi + # set MIXER quad_w set MIXER quad From 09b0f9a8ba9b7666f5468341f66883910159f554 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 19:34:09 +0100 Subject: [PATCH 035/129] sitl standard_vtol: use default inertia --- ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index 08493535210f..b62b42f532f7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -41,11 +41,6 @@ then param set VT_TYPE 2 param set VT_B_TRANS_DUR 8 - - param set VM_INERTIA_XX 1.0 - param set VM_INERTIA_YY 1.0 - param set VM_INERTIA_ZZ 1.0 - param set CA_AIRFRAME 2 param set CA_ACT0_MIN 0.0 @@ -57,15 +52,15 @@ then param set CA_ACT2_MAX 1.0 param set CA_ACT3_MAX 1.0 - param set CA_ACT4_MAX 1.0 param set CA_ACT4_MIN 0.0 + param set CA_ACT4_MAX 1.0 - param set CA_ACT5_MAX 1.0 param set CA_ACT5_MIN -1.0 - param set CA_ACT6_MAX 1.0 + param set CA_ACT5_MAX 1.0 param set CA_ACT6_MIN -1.0 - param set CA_ACT7_MAX 1.0 + param set CA_ACT6_MAX 1.0 param set CA_ACT7_MIN -1.0 + param set CA_ACT7_MAX 1.0 fi From 2556c7106ac25d59651d00bf3d3477a0e209961f Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 19:34:54 +0100 Subject: [PATCH 036/129] mc/fw/vtol: revert control_allocator autostart --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 5 ----- ROMFS/px4fmu_common/init.d/rc.mc_apps | 5 ----- ROMFS/px4fmu_common/init.d/rc.vtol_apps | 5 ----- 3 files changed, 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 2b9561e66972..8f3dedd01613 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -17,11 +17,6 @@ fw_att_control start fw_pos_control_l1 start airspeed_selector start -# -# Start Control Allocator -# -control_allocator start - # # Start Land Detector. # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index baf2d974a9ae..32f2f18df084 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -64,11 +64,6 @@ mc_att_control start mc_hover_thrust_estimator start mc_pos_control start -# -# Start Control Allocator -# -control_allocator start - # # Start Multicopter Land Detector. # diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 99234ee98681..2a12d878aafc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -27,11 +27,6 @@ mc_hover_thrust_estimator start fw_att_control start vtol fw_pos_control_l1 start vtol -# -# Start Control Allocator -# -control_allocator start - # # Start Land Detector # Multicopter for now until we have something for VTOL From b86535e53958530008fd92aa9175e95a276e6466 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 19:35:43 +0100 Subject: [PATCH 037/129] fmu-v5: build angular_velocity_controller --- boards/px4/fmu-v5/default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index a39c199e4a06..0ab083df6574 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -61,6 +61,7 @@ px4_add_board( uavcan MODULES airspeed_selector + angular_velocity_controller attitude_estimator_q battery_status camera_feedback From d2668f75f9502a1d1a403ae52baffd998abbb82d Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 19:35:53 +0100 Subject: [PATCH 038/129] sitl: build angular_velocity_controller --- boards/px4/sitl/default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 5b00a4189e7a..42e83abcc345 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -22,6 +22,7 @@ px4_add_board( #uavcan MODULES airspeed_selector + angular_velocity_controller attitude_estimator_q camera_feedback commander From 766d615854a8eb3df20e6f3370070f53aebd5b05 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 19:36:40 +0100 Subject: [PATCH 039/129] Add rc.new_apps script to test new controllers and control allocator --- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 1 + ROMFS/px4fmu_common/init.d/rc.new_apps | 16 ++++++++++++++++ 2 files changed, 17 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/rc.new_apps diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 4446c6a658bc..0786a4ea6e3a 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -51,4 +51,5 @@ px4_add_romfs_files( rc.vehicle_setup rc.vtol_apps rc.vtol_defaults + rc.new_apps ) diff --git a/ROMFS/px4fmu_common/init.d/rc.new_apps b/ROMFS/px4fmu_common/init.d/rc.new_apps new file mode 100644 index 000000000000..f5b3dff12ecb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.new_apps @@ -0,0 +1,16 @@ +#!/bin/sh +# +# Standard apps for new control allocation and controllers +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +# +# Start angular velocity controller +# +angular_velocity_controller start + +# +# Start Control Allocator +# +control_allocator start From e3b5b6ddc860fdc4c6264138bb14f511eb15ab55 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 19:37:03 +0100 Subject: [PATCH 040/129] sitl: autostart new apps --- ROMFS/px4fmu_common/init.d-posix/rcS | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 2222e5e0c3e5..dd0c27577b1c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -300,5 +300,8 @@ else fi sh etc/init.d/rc.logging +# Start new apps +sh etc/init.d/rc.new_apps + mavlink boot_complete replay trystart From 988916404d7607a675a77e9efea5cb971fb609a9 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 19:59:11 +0100 Subject: [PATCH 041/129] sitl typhoon_h480: set inertia and gains --- ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 index 57ad3e2b49dd..d28c95f3c645 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 @@ -26,6 +26,14 @@ then param set MNT_MODE_IN 0 param set MAV_PROTO_VER 2 + param set VM_INERTIA_XX 0.005 + param set VM_INERTIA_YY 0.005 + param set VM_INERTIA_ZZ 0.01 + + param set AVC_X_D 0.0 + param set AVC_Y_D 0.0 + param set AVC_Z_D 0.0 + param set CA_AIRFRAME 1 param set CA_ACT0_MIN 0.0 param set CA_ACT1_MIN 0.0 From fb4c9d50642bba82e66ddf4175319ea996c23473 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 20:16:10 +0100 Subject: [PATCH 042/129] format --- .../AngularVelocityControl/AngularVelocityControl.cpp | 6 ++++-- .../AngularVelocityControl/AngularVelocityControl.hpp | 6 ++++-- .../AngularVelocityController.cpp | 10 +++++----- .../AngularVelocityController.hpp | 3 ++- 4 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp index 15f403d2e03e..56c49c2bf10d 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp @@ -47,13 +47,15 @@ void AngularVelocityControl::setGains(const Vector3f &P, const Vector3f &I, cons _gain_d = D; } -void AngularVelocityControl::setSaturationStatus(const matrix::Vector &saturation_positive, const matrix::Vector &saturation_negative) +void AngularVelocityControl::setSaturationStatus(const matrix::Vector &saturation_positive, + const matrix::Vector &saturation_negative) { _saturation_positive = saturation_positive; _saturation_negative = saturation_negative; } -void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_acceleration, const Vector3f &angular_velocity_sp, const float dt, const bool landed) +void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_acceleration, + const Vector3f &angular_velocity_sp, const float dt, const bool landed) { // angular rates error Vector3f angular_velocity_error = angular_velocity_sp - angular_velocity; diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp index 54e6cb41bca0..e91dde775930 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp @@ -84,7 +84,8 @@ class AngularVelocityControl * @param saturation_positive positive saturation * @param saturation_negative negative saturation */ - void setSaturationStatus(const matrix::Vector &saturation_positive, const matrix::Vector &saturation_negative); + void setSaturationStatus(const matrix::Vector &saturation_positive, + const matrix::Vector &saturation_negative); /** * Run one control loop cycle calculation @@ -94,7 +95,8 @@ class AngularVelocityControl * @param dt elapsed time since last update * @param landed whether the vehicle is on the ground */ - void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration, const matrix::Vector3f &angular_velocity_sp, const float dt, const bool landed); + void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration, + const matrix::Vector3f &angular_velocity_sp, const float dt, const bool landed); /** * Get the desired angular acceleration diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index 12a4b1c1dcd7..babcc9f5bc98 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -163,10 +163,10 @@ AngularVelocityController::Run() // run the controller bool run_control = ((_vehicle_control_mode.flag_control_rates_enabled - || _vehicle_control_mode.flag_control_termination_enabled) - && (!_vehicle_status.is_vtol - || (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) - || (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode))); + || _vehicle_control_mode.flag_control_termination_enabled) + && (!_vehicle_status.is_vtol + || (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) + || (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode))); if (run_control) { @@ -189,7 +189,7 @@ AngularVelocityController::Run() Vector saturation_negative; if (not control_allocator_status.torque_setpoint_achieved) { - for (size_t i=0; i < 3; i++) { + for (size_t i = 0; i < 3; i++) { if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { saturation_positive(i) = true; diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.hpp b/src/modules/angular_velocity_controller/AngularVelocityController.hpp index c62002899453..e58e19f2c337 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.hpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.hpp @@ -61,7 +61,8 @@ #include -class AngularVelocityController : public ModuleBase, public ModuleParams, public px4::WorkItem +class AngularVelocityController : public ModuleBase, public ModuleParams, + public px4::WorkItem { public: AngularVelocityController(); From 97130d7098df0f29f3d21bda4722f774617a8379 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 21:39:35 +0100 Subject: [PATCH 043/129] sitl standard_vtol: disable control allocation (CI fails) --- ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index b62b42f532f7..1d5604361ef5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -66,4 +66,6 @@ fi set MAV_TYPE 22 -set MIXER standard_vtol +set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix +set MIXER custom +# set MIXER standard_vtol From e87dd7b4565b403891429b5324b1214c23750a65 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 21:40:25 +0100 Subject: [PATCH 044/129] AngularVelocityController: fixup --- .../angular_velocity_controller/AngularVelocityController.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index babcc9f5bc98..03c710c67104 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -40,7 +40,6 @@ using namespace matrix; using namespace time_literals; -using math::radians; AngularVelocityController::AngularVelocityController() : ModuleParams(nullptr), From e7e787205ca777e097523adcaab163796a99b883 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 21:41:14 +0100 Subject: [PATCH 045/129] ControlAllocation: fix appveyor build --- .../control_allocator/ControlAllocation/ControlAllocation.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 8bfbf621df78..73ec73412968 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -78,7 +78,7 @@ class ControlAllocation ControlAllocation() = default; virtual ~ControlAllocation() = default; - static const uint8_t NUM_ACTUATORS = vehicle_actuator_setpoint_s::NUM_ACTUATOR_SETPOINT; + static const uint8_t NUM_ACTUATORS = 16; static const uint8_t NUM_AXES = 6; typedef matrix::Vector ActuatorVector; From 3a53b7da4df387b768725d27ca5d445533e3fa43 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 22:59:05 +0100 Subject: [PATCH 046/129] angular_velocity_controller: run in FW mode --- .../AngularVelocityController.cpp | 75 ++++++++----------- 1 file changed, 31 insertions(+), 44 deletions(-) diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index 03c710c67104..33db84b30c75 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -161,60 +161,47 @@ AngularVelocityController::Run() } // run the controller - bool run_control = ((_vehicle_control_mode.flag_control_rates_enabled - || _vehicle_control_mode.flag_control_termination_enabled) - && (!_vehicle_status.is_vtol - || (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) - || (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode))); - - if (run_control) { - - if (_vehicle_control_mode.flag_control_termination_enabled) { - // reset controller - _control.reset(); - - } else { - // reset integral if disarmed - if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _control.resetIntegral(); - } + if (_vehicle_control_mode.flag_control_rates_enabled) { + // reset integral if disarmed + if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _control.resetIntegral(); + } - // update saturation status from mixer feedback - if (_control_allocator_status_sub.updated()) { - control_allocator_status_s control_allocator_status; + // update saturation status from mixer feedback + if (_control_allocator_status_sub.updated()) { + control_allocator_status_s control_allocator_status; - if (_control_allocator_status_sub.copy(&control_allocator_status)) { - Vector saturation_positive; - Vector saturation_negative; + if (_control_allocator_status_sub.copy(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; - if (not control_allocator_status.torque_setpoint_achieved) { - for (size_t i = 0; i < 3; i++) { - if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { - saturation_positive(i) = true; + if (not control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; - } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { - saturation_negative(i) = true; - } + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; } } - - _control.setSaturationStatus(saturation_positive, saturation_negative); } - } - // run rate controller - _control.update(angular_velocity, _angular_acceleration, _angular_velocity_sp, dt, _maybe_landed || _landed); - - // publish rate controller status - rate_ctrl_status_s rate_ctrl_status{}; - Vector3f integral = _control.getIntegral(); - rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.rollspeed_integ = integral(0); - rate_ctrl_status.pitchspeed_integ = integral(1); - rate_ctrl_status.yawspeed_integ = integral(2); - _rate_ctrl_status_pub.publish(rate_ctrl_status); + _control.setSaturationStatus(saturation_positive, saturation_negative); + } } + // run rate controller + _control.update(angular_velocity, _angular_acceleration, _angular_velocity_sp, dt, _maybe_landed || _landed); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + Vector3f integral = _control.getIntegral(); + rate_ctrl_status.timestamp = hrt_absolute_time(); + rate_ctrl_status.rollspeed_integ = integral(0); + rate_ctrl_status.pitchspeed_integ = integral(1); + rate_ctrl_status.yawspeed_integ = integral(2); + _rate_ctrl_status_pub.publish(rate_ctrl_status); + // publish controller output publish_angular_acceleration_setpoint(); publish_torque_setpoint(); From af72fc838863d7f4aadfc3644b168cba84919fbe Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 23:00:56 +0100 Subject: [PATCH 047/129] control_allocator: do not publish multirotor_motor_limits --- .../control_allocator/ControlAllocator.cpp | 60 ------------------- .../control_allocator/ControlAllocator.hpp | 4 -- 2 files changed, 64 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index ecd4e43eb9f6..b39179be9dc8 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -378,7 +378,6 @@ ControlAllocator::Run() // the current mixer system and multicopter controller // TODO: remove publish_legacy_actuator_controls(); - publish_legacy_multirotor_motor_limits(); } perf_end(_loop_perf); @@ -461,65 +460,6 @@ ControlAllocator::publish_legacy_actuator_controls() _actuator_controls_5_pub.publish(actuator_controls_5); } -void -ControlAllocator::publish_legacy_multirotor_motor_limits() -{ - multirotor_motor_limits_s multirotor_motor_limits{}; - multirotor_motor_limits.timestamp = hrt_absolute_time(); - multirotor_motor_limits.saturation_status = 1; - - // Motor saturation - matrix::Vector actuator_norm = _control_allocation->normalizeActuatorSetpoint(); - - for (size_t i = 0; i < NUM_ACTUATORS; i++) { - if (actuator_norm(i) > (1.0f - FLT_EPSILON)) { - multirotor_motor_limits.saturation_status |= (1 << 1); - - } else if (actuator_norm(i) < (-1.0f + FLT_EPSILON)) { - multirotor_motor_limits.saturation_status |= (1 << 2); - } - } - - // Control saturation - matrix::Vector unallocated_control = _control_allocation->getControlSetpoint() - - _control_allocation->getAllocatedControl(); - - // roll - if (unallocated_control(0) > FLT_EPSILON) { - multirotor_motor_limits.saturation_status |= (1 << 3); - - } else if (unallocated_control(0) < -FLT_EPSILON) { - multirotor_motor_limits.saturation_status |= (1 << 4); - } - - // pitch - if (unallocated_control(1) > FLT_EPSILON) { - multirotor_motor_limits.saturation_status |= (1 << 5); - - } else if (unallocated_control(1) < -FLT_EPSILON) { - multirotor_motor_limits.saturation_status |= (1 << 6); - } - - // yaw - if (unallocated_control(2) > FLT_EPSILON) { - multirotor_motor_limits.saturation_status |= (1 << 7); - - } else if (unallocated_control(2) < -FLT_EPSILON) { - multirotor_motor_limits.saturation_status |= (1 << 8); - } - - // z thrust - if (unallocated_control(5) > FLT_EPSILON) { - multirotor_motor_limits.saturation_status |= (1 << 10); // 10 because Z points downward - - } else if (unallocated_control(5) < -FLT_EPSILON) { - multirotor_motor_limits.saturation_status |= (1 << 9); // 9 because Z points downward - } - - - _multirotor_motor_limits_pub.publish(multirotor_motor_limits); -} - int ControlAllocator::task_spawn(int argc, char *argv[]) { ControlAllocator *instance = new ControlAllocator(); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 0b07df8d1f3e..693ad05d2880 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -58,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -124,9 +123,6 @@ class ControlAllocator : public ModuleBase, public ModuleParam uORB::Publication _actuator_controls_4_pub{ORB_ID(actuator_controls_4)}; /**< actuator controls 4 publication */ uORB::Publication _actuator_controls_5_pub{ORB_ID(actuator_controls_5)}; /**< actuator controls 5 publication */ - // multirotor limits, should be replaced by something more generic - uORB::Publication _multirotor_motor_limits_pub{ORB_ID(multirotor_motor_limits)}; - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; /**< airspeed subscription */ From cddf12b681c1acf3c28ae87a9e62ec7740552099 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 23:01:32 +0100 Subject: [PATCH 048/129] control_allocator: fix param metadata --- src/modules/control_allocator/control_allocator_params.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/control_allocator/control_allocator_params.c b/src/modules/control_allocator/control_allocator_params.c index 5706b2e79595..0fc6a969306f 100644 --- a/src/modules/control_allocator/control_allocator_params.c +++ b/src/modules/control_allocator/control_allocator_params.c @@ -47,7 +47,10 @@ * * @value 0 Test * @min 0 - * @max 0 + * @max 2 + * @value 0 quad wide + * @value 1 hexa x + * @value 2 standard vtol * @group Control Allocation */ PARAM_DEFINE_INT32(CA_AIRFRAME, 0); @@ -58,7 +61,7 @@ PARAM_DEFINE_INT32(CA_AIRFRAME, 0); * @value 0 Pseudo-inverse with output clipping (default) * @value 1 Pseudo-inverse with sequential desaturation technique * @min 0 - * @max 2 + * @max 1 * @group Control Allocation */ PARAM_DEFINE_INT32(CA_METHOD, 0); From 544b9a34ed1c8e67438854a7f9fd0b9bea21a42a Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 23:03:19 +0100 Subject: [PATCH 049/129] standard_vtol: activate both motors and flaps during transitions --- .../control_allocator/ControlAllocator.cpp | 78 ++++++++++++++----- .../control_allocator/ControlAllocator.hpp | 8 +- 2 files changed, 65 insertions(+), 21 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index b39179be9dc8..54b729edfc77 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -170,29 +170,46 @@ ControlAllocator::getEffectinvenessMatrix() } case 2: { - if (_vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - // standard_vtol_hover - const float B_standard_vtol_hover[NUM_AXES][NUM_ACTUATORS] = { - {-0.5, 0.5, 0.5, -0.5, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.5, -0.5, 0.5, -0.5, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + switch (_flight_phase) { + case FlightPhase::HOVER_FLIGHT: { + const float B_standard_vtol[NUM_AXES][NUM_ACTUATORS] = { + {-0.5f, 0.5f, 0.5f, -0.5f, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; - B = matrix::Matrix(B_standard_vtol_hover); - - } else { - // standard_vtol_fixed_wing - const float B_standard_vtol_fixed_wing[NUM_AXES][NUM_ACTUATORS] = { - { 0.0, 0.0, 0.0, 0.0, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.0, 0.0, 0.0, 0.0, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.0, 0.0, 0.0, 0.0, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.0f, 0.0f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + B = matrix::Matrix(B_standard_vtol); + break; + } + + case FlightPhase::FORWARD_FLIGHT: { + const float B_standard_vtol[NUM_AXES][NUM_ACTUATORS] = { + { 0.f, 0.f, 0.f, 0.f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + B = matrix::Matrix(B_standard_vtol); + break; + } + + case FlightPhase::TRANSITION_HF_TO_FF: + case FlightPhase::TRANSITION_FF_TO_HF: { + const float B_standard_vtol[NUM_AXES][NUM_ACTUATORS] = { + { -0.5f, 0.5f, 0.5f, -0.5f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; - B = matrix::Matrix(B_standard_vtol_fixed_wing); + B = matrix::Matrix(B_standard_vtol); + break; + } } break; @@ -316,8 +333,29 @@ ControlAllocator::Run() vehicle_status_s vehicle_status = {}; _vehicle_status_sub.update(&vehicle_status); - if (_vehicle_type != vehicle_status.vehicle_type) { - _vehicle_type = vehicle_status.vehicle_type; + FlightPhase new_flight_phase = FlightPhase::HOVER_FLIGHT; + + // Check if the current flight phase is HOVER or FIXED_WING + if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + new_flight_phase = FlightPhase::HOVER_FLIGHT; + + } else { + new_flight_phase = FlightPhase::FORWARD_FLIGHT; + } + + // Special cases for VTOL in transition + if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) { + if (vehicle_status.in_transition_to_fw) { + new_flight_phase = FlightPhase::TRANSITION_HF_TO_FF; + + } else { + new_flight_phase = FlightPhase::TRANSITION_FF_TO_HF; + } + } + + // Update effectiveness matrix if needed + if (_flight_phase != new_flight_phase) { + _flight_phase = new_flight_phase; matrix::Matrix B = getEffectinvenessMatrix(); // Assign control effectiveness matrix diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 693ad05d2880..b369c8dd469f 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -134,7 +134,13 @@ class ControlAllocator : public ModuleBase, public ModuleParam // float _battery_scale_factor{1.0f}; // float _airspeed_scale_factor{1.0f}; - uint8_t _vehicle_type{0}; /**< see vehicle_status_s::vehicle_type */ + enum class FlightPhase { + HOVER_FLIGHT=0, + FORWARD_FLIGHT=1, + TRANSITION_HF_TO_FF=2, + TRANSITION_FF_TO_HF=3 + }; + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; perf_counter_t _loop_perf; /**< loop duration performance counter */ From 736dfa1aeb864ab1a8e55ceece2b3eb004265b9e Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 23:03:39 +0100 Subject: [PATCH 050/129] standard_vtol: reactivate control allocation --- ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index 1d5604361ef5..8d288fe36a7f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -66,6 +66,6 @@ fi set MAV_TYPE 22 -set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix -set MIXER custom -# set MIXER standard_vtol +# set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix +# set MIXER custom +set MIXER standard_vtol From 57bab34fe15a06f56b10cfd0e99b58db80879f1f Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 23:08:26 +0100 Subject: [PATCH 051/129] control_allocator: enumerate airframes --- .../control_allocator/ControlAllocator.cpp | 22 ++++++++----------- .../control_allocator/ControlAllocator.hpp | 6 +++++ 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 54b729edfc77..caf27a170165 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -138,10 +138,8 @@ ControlAllocator::getEffectinvenessMatrix() // Control effectiveness matrix::Matrix B; - switch (_param_ca_airframe.get()) { - - case 0: { - // quad_w + switch ((Airframe)_param_ca_airframe.get()) { + case Airframe::QUAD_W: { const float B_quad_w[NUM_AXES][NUM_ACTUATORS] = { // quad_w {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, @@ -155,8 +153,7 @@ ControlAllocator::getEffectinvenessMatrix() break; } - case 1: { - // hexa_x + case Airframe::HEXA_X: { const float B_hexa_x[NUM_AXES][NUM_ACTUATORS] = { {-0.333333f, 0.333333f, 0.166667f, -0.166667f, -0.166667f, 0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.f, 0.f, 0.288675f, -0.288675f, 0.288675f, -0.288675f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, @@ -169,17 +166,17 @@ ControlAllocator::getEffectinvenessMatrix() break; } - case 2: { + case Airframe::STANDARD_VTOL: { switch (_flight_phase) { case FlightPhase::HOVER_FLIGHT: { const float B_standard_vtol[NUM_AXES][NUM_ACTUATORS] = { {-0.5f, 0.5f, 0.5f, -0.5f, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; B = matrix::Matrix(B_standard_vtol); break; } @@ -206,7 +203,7 @@ ControlAllocator::getEffectinvenessMatrix() { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; + }; B = matrix::Matrix(B_standard_vtol); break; } @@ -220,7 +217,6 @@ ControlAllocator::getEffectinvenessMatrix() break; } - matrix::Vector actuator_max = _control_allocation->getActuatorMax(); matrix::Vector actuator_min = _control_allocation->getActuatorMin(); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index b369c8dd469f..34e9502c6e5c 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -111,6 +111,12 @@ class ControlAllocator : public ModuleBase, public ModuleParam int _allocation_method_id{-1}; ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations + enum class Airframe { + QUAD_W=0, + HEXA_X=1, + STANDARD_VTOL=2 + }; + // Inputs uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)}; /**< vehicle torque setpoint subscription */ uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle thrust setpoint subscription */ From 29fe598178610f302e1d58832a524258ce78c168 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 28 Nov 2019 23:13:17 +0100 Subject: [PATCH 052/129] format --- src/modules/control_allocator/ControlAllocator.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 34e9502c6e5c..7a78578da09e 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -112,9 +112,9 @@ class ControlAllocator : public ModuleBase, public ModuleParam ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations enum class Airframe { - QUAD_W=0, - HEXA_X=1, - STANDARD_VTOL=2 + QUAD_W = 0, + HEXA_X = 1, + STANDARD_VTOL = 2 }; // Inputs @@ -141,10 +141,10 @@ class ControlAllocator : public ModuleBase, public ModuleParam // float _airspeed_scale_factor{1.0f}; enum class FlightPhase { - HOVER_FLIGHT=0, - FORWARD_FLIGHT=1, - TRANSITION_HF_TO_FF=2, - TRANSITION_FF_TO_HF=3 + HOVER_FLIGHT = 0, + FORWARD_FLIGHT = 1, + TRANSITION_HF_TO_FF = 2, + TRANSITION_FF_TO_HF = 3 }; FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; From 212dd59434661bd99628acbea79f7c247a03e102 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 29 Nov 2019 13:37:44 +0100 Subject: [PATCH 053/129] ControlAllocation: fix global defined macro interfering with local member variable The symbol _B is already defined on certain platforms as a macro e.g. in newlib the c library cygwin uses or on the board lpc31eventrtr in NuttX. --- .../ControlAllocation/ControlAllocation.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 73ec73412968..3260871d5a48 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -72,14 +72,16 @@ #include #include +#undef _B // This symbol is a macro on certain platforms, it interferes with local scope variables + class ControlAllocation { public: ControlAllocation() = default; virtual ~ControlAllocation() = default; - static const uint8_t NUM_ACTUATORS = 16; - static const uint8_t NUM_AXES = 6; + static constexpr uint8_t NUM_ACTUATORS = 16; + static constexpr uint8_t NUM_AXES = 6; typedef matrix::Vector ActuatorVector; From e136a4294431df2d9c2d248abb420c6363930db8 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 29 Nov 2019 20:16:28 +0100 Subject: [PATCH 054/129] control_allocation: rename _B -> _effectiveness --- .../ControlAllocation/ControlAllocation.cpp | 4 ++-- .../ControlAllocation/ControlAllocation.hpp | 4 +--- .../ControlAllocation/ControlAllocationPseudoInverse.cpp | 6 +++--- .../ControlAllocationSequentialDesaturation.cpp | 2 +- src/modules/control_allocator/ControlAllocator.cpp | 2 +- 5 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index 0b5fee029052..8e2a8696c101 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -69,13 +69,13 @@ void ControlAllocation::setEffectivenessMatrix(const matrix::Matrix &B) { - _B = B; + _effectiveness = B; } const matrix::Matrix & ControlAllocation::getEffectivenessMatrix() const { - return _B; + return _effectiveness; } void diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 3260871d5a48..e05a8e969f9d 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -72,8 +72,6 @@ #include #include -#undef _B // This symbol is a macro on certain platforms, it interferes with local scope variables - class ControlAllocation { public: @@ -201,7 +199,7 @@ class ControlAllocation matrix::Vector normalizeActuatorSetpoint() const; protected: - matrix::Matrix _B; //< Effectiveness matrix + matrix::Matrix _effectiveness; //< Effectiveness matrix matrix::Vector _actuator_min; //< Minimum actuator values matrix::Vector _actuator_max; //< Maximum actuator values matrix::Vector _actuator_sp; //< Actuator setpoint diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 78355e8f2640..7d746cfcd691 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -45,7 +45,7 @@ void ControlAllocationPseudoInverse::setEffectivenessMatrix(const matrix::Matrix &B) { - _B = B; + _effectiveness = B; _A_update_needed = true; } @@ -53,7 +53,7 @@ void ControlAllocationPseudoInverse::updatePseudoInverse() { if (_A_update_needed) { - _A = matrix::geninv(_B); + _A = matrix::geninv(_effectiveness); _A_update_needed = false; } } @@ -71,5 +71,5 @@ ControlAllocationPseudoInverse::allocate() _actuator_sp = clipActuatorSetpoint(); // Compute achieved control - _control_allocated = _B * _actuator_sp; + _control_allocated = _effectiveness * _actuator_sp; } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 8af658fb70c3..8c24b1d08c80 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -59,7 +59,7 @@ ControlAllocationSequentialDesaturation::allocate() _actuator_sp = clipActuatorSetpoint(); // Compute achieved control - _control_allocated = _B * _actuator_sp; + _control_allocated = _effectiveness * _actuator_sp; } void ControlAllocationSequentialDesaturation::desaturateActuators( diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index caf27a170165..d3575f481e61 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -126,7 +126,7 @@ ControlAllocator::parameters_updated() actuator_max(15) = _param_ca_act15_max.get(); _control_allocation->setActuatorMax(actuator_max); - matrix::Matrix B = getEffectinvenessMatrix(); + matrix::Matrix B = getEffectivenessMatrix(); // Assign control effectiveness matrix _control_allocation->setEffectivenessMatrix(B); From bac71d61e544fde79077319e6af8e36a184dabe7 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 29 Nov 2019 20:19:24 +0100 Subject: [PATCH 055/129] control_allocation: rename _A -> _mix --- .../ControlAllocation/ControlAllocationPseudoInverse.cpp | 4 ++-- .../ControlAllocation/ControlAllocationPseudoInverse.hpp | 2 +- .../ControlAllocationSequentialDesaturation.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 7d746cfcd691..697773e40df3 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -53,7 +53,7 @@ void ControlAllocationPseudoInverse::updatePseudoInverse() { if (_A_update_needed) { - _A = matrix::geninv(_effectiveness); + _mix = matrix::geninv(_effectiveness); _A_update_needed = false; } } @@ -65,7 +65,7 @@ ControlAllocationPseudoInverse::allocate() updatePseudoInverse(); // Allocate - _actuator_sp = _A * _control_sp; + _actuator_sp = _mix * _control_sp; // Clip _actuator_sp = clipActuatorSetpoint(); diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp index 41403aa09ead..f948810b658d 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -57,7 +57,7 @@ class ControlAllocationPseudoInverse: public ControlAllocation virtual void setEffectivenessMatrix(const matrix::Matrix &B) override; protected: - matrix::Matrix _A; + matrix::Matrix _mix; bool _A_update_needed{false}; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 8c24b1d08c80..f64ddf397f7f 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -48,7 +48,7 @@ ControlAllocationSequentialDesaturation::allocate() updatePseudoInverse(); // Allocate - _actuator_sp = _A * _control_sp; + _actuator_sp = _mix * _control_sp; // go through control axes from lowest to highest priority and unsaturate the actuators for (unsigned i = 0; i < NUM_AXES; i++) { @@ -83,7 +83,7 @@ ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDe ActuatorVector ret; for (unsigned i = 0; i < NUM_ACTUATORS; i++) { - ret(i) = _A(i, axis); + ret(i) = _mix(i, axis); } return ret; From 17946d22ea0ec29142224a87c4d038e64a46f610 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 29 Nov 2019 20:21:04 +0100 Subject: [PATCH 056/129] control_allocation: fix method names --- .../ControlAllocation/ControlAllocation.cpp | 5 +++-- .../ControlAllocation/ControlAllocation.hpp | 6 +++--- .../ControlAllocationPseudoInverseTest.cpp | 2 +- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index 8e2a8696c101..13bcf0110d9e 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -42,7 +42,7 @@ #include "ControlAllocation.hpp" const matrix::Vector & -ControlAllocation::getAllocatedActuator() const +ControlAllocation::getActuatorSetpoint() const { return _actuator_sp; } @@ -105,9 +105,10 @@ ControlAllocation::getActuatorMax() const } void -ControlAllocation::setCurrentActuatorSetpoint(const matrix::Vector +ControlAllocation::setActuatorSetpoint(const matrix::Vector &actuator_sp) { + // Set actuator setpoint _actuator_sp = actuator_sp; } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index e05a8e969f9d..75837cbd64ee 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -54,7 +54,7 @@ * // Set control setpoint, allocate actuator setpoint, retrieve actuator setpoint * alloc.setControlSetpoint(control_sp); * alloc.allocate(); - * actuator_sp = alloc.getAllocatedActuator(); + * actuator_sp = alloc.getActuatorSetpoint(); * * // Check if the control setpoint was fully allocated * unallocated_control = control_sp - alloc.getAllocatedControl() @@ -111,7 +111,7 @@ class ControlAllocation * * @return Actuator vector */ - const matrix::Vector &getAllocatedActuator() const; + const matrix::Vector &getActuatorSetpoint() const; /** * Set the desired control vector @@ -178,7 +178,7 @@ class ControlAllocation * * @param actuator_sp Actuator setpoint */ - void setCurrentActuatorSetpoint(const matrix::Vector &actuator_sp); + void setActuatorSetpoint(const matrix::Vector &actuator_sp); /** * Clip the actuator setpoint between minimum and maximum values. diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp index ecf888759830..2736d3fa240b 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp @@ -58,7 +58,7 @@ TEST(ControlAllocationTest, AllZeroCase) method.setEffectivenessMatrix(B); method.setControlSetpoint(control_sp); method.allocate(); - actuator_sp = method.getAllocatedActuator(); + actuator_sp = method.getActuatorSetpoint(); control_allocated_expected = method.getAllocatedControl(); EXPECT_EQ(actuator_sp, actuator_sp_expected); From 11a87a5442e5ebe117a865f5e0e678a439bcf65f Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 29 Nov 2019 20:22:14 +0100 Subject: [PATCH 057/129] control_allocation: fix state in setActuatorSetpoint --- .../ControlAllocation/ControlAllocation.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index 13bcf0110d9e..f49044792df2 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -110,6 +110,13 @@ ControlAllocation::setActuatorSetpoint(const matrix::Vector From 81827aeb5574810cc346851fb89720f8a38bc3b2 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 29 Nov 2019 20:24:39 +0100 Subject: [PATCH 058/129] control_allocation: fixup --- src/modules/control_allocator/ControlAllocator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index d3575f481e61..91dbb992d9a1 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -244,7 +244,7 @@ ControlAllocator::update_allocation_method() matrix::Vector actuator_sp; if (_control_allocation != nullptr) { - actuator_sp = _control_allocation->getAllocatedActuator(); + actuator_sp = _control_allocation->getActuatorSetpoint(); } // try to instanciate new allocation method @@ -291,7 +291,7 @@ ControlAllocator::update_allocation_method() _allocation_method_id = method; // Configure new allocation method - _control_allocation->setCurrentActuatorSetpoint(actuator_sp); + _control_allocation->setActuatorSetpoint(actuator_sp); } } @@ -420,7 +420,7 @@ ControlAllocator::Run() void ControlAllocator::publish_actuator_setpoint() { - matrix::Vector actuator_sp = _control_allocation->getAllocatedActuator(); + matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); vehicle_actuator_setpoint_s vehicle_actuator_setpoint{}; vehicle_actuator_setpoint.timestamp = hrt_absolute_time(); @@ -455,7 +455,7 @@ ControlAllocator::publish_control_allocator_status() control_allocator_status.unallocated_thrust[2] = unallocated_control(5); // Actuator saturation - matrix::Vector actuator_sp = _control_allocation->getAllocatedActuator(); + matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); matrix::Vector actuator_min = _control_allocation->getActuatorMin(); matrix::Vector actuator_max = _control_allocation->getActuatorMax(); From 32c524a9adce7b7c27981ef9d22fd6e59ea3109b Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 29 Nov 2019 20:26:51 +0100 Subject: [PATCH 059/129] control_allocator: update effectiveness when changed --- .../control_allocator/ControlAllocator.cpp | 29 +++++++++---------- .../control_allocator/ControlAllocator.hpp | 9 +++--- 2 files changed, 17 insertions(+), 21 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 91dbb992d9a1..662cc6955d90 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -133,7 +133,7 @@ ControlAllocator::parameters_updated() } const matrix::Matrix -ControlAllocator::getEffectinvenessMatrix() +ControlAllocator::getEffectivenessMatrix() { // Control effectiveness matrix::Matrix B; @@ -329,37 +329,26 @@ ControlAllocator::Run() vehicle_status_s vehicle_status = {}; _vehicle_status_sub.update(&vehicle_status); - FlightPhase new_flight_phase = FlightPhase::HOVER_FLIGHT; - // Check if the current flight phase is HOVER or FIXED_WING if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - new_flight_phase = FlightPhase::HOVER_FLIGHT; + _flight_phase = FlightPhase::HOVER_FLIGHT; } else { - new_flight_phase = FlightPhase::FORWARD_FLIGHT; + _flight_phase = FlightPhase::FORWARD_FLIGHT; } // Special cases for VTOL in transition if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) { if (vehicle_status.in_transition_to_fw) { - new_flight_phase = FlightPhase::TRANSITION_HF_TO_FF; + _flight_phase = FlightPhase::TRANSITION_HF_TO_FF; } else { - new_flight_phase = FlightPhase::TRANSITION_FF_TO_HF; + _flight_phase = FlightPhase::TRANSITION_FF_TO_HF; } } - // Update effectiveness matrix if needed - if (_flight_phase != new_flight_phase) { - _flight_phase = new_flight_phase; - matrix::Matrix B = getEffectinvenessMatrix(); - - // Assign control effectiveness matrix - _control_allocation->setEffectivenessMatrix(B); - } } - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const hrt_abstime now = hrt_absolute_time(); const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); @@ -391,6 +380,14 @@ ControlAllocator::Run() if (do_update) { + // Update effectiveness matrix if needed + matrix::Matrix B = getEffectivenessMatrix(); + + // Assign control effectiveness matrix + if ((B - _control_allocation->getEffectivenessMatrix()).abs().max() > 1e-5f) { + _control_allocation->setEffectivenessMatrix(B); + } + // Set control setpoint vector matrix::Vector c; c(0) = _torque_sp(0); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 7a78578da09e..8d8576202772 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -87,6 +87,9 @@ class ControlAllocator : public ModuleBase, public ModuleParam bool init(); + static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; + static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES; + private: /** @@ -102,11 +105,7 @@ class ControlAllocator : public ModuleBase, public ModuleParam void publish_legacy_actuator_controls(); void publish_legacy_multirotor_motor_limits(); - const matrix::Matrix - getEffectinvenessMatrix(); - - static const uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; - static const uint8_t NUM_AXES = ControlAllocation::NUM_AXES; + const matrix::Matrix getEffectivenessMatrix(); int _allocation_method_id{-1}; ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations From 28bedf9104ee27852271df222c9b4fef38c6983f Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 29 Nov 2019 20:28:02 +0100 Subject: [PATCH 060/129] Add tiltrotor effectiveness (WIP) --- .../control_allocator/ControlAllocator.cpp | 15 +++++++++++++++ .../control_allocator/ControlAllocator.hpp | 3 ++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 662cc6955d90..f899bd903a68 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -212,6 +212,21 @@ ControlAllocator::getEffectivenessMatrix() break; } + case Airframe::TILTROTOR_VTOL: { + matrix::Vector act = _control_allocation->getActuatorSetpoint(); + + const float B_tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = { + {-0.5f * cosf(act(4)), 0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), 0.5f * act(0) * sinf(act(4)), -0.5f * act(1) * sinf(act(5)), -0.5f * act(2) * sinf(act(6)), 0.5f * act(3) * sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f * cosf(act(4)), -0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), -0.5f * act(0) * sinf(act(4)), 0.5f * act(1) * sinf(act(5)), -0.5f * act(2) * sinf(act(6)), 0.5f * act(3) * sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f * sinf(act(4)), -0.5f * sinf(act(5)), -0.5f * sinf(act(6)), 0.5f * sinf(act(7)), 0.5f * act(0) * cosf(act(4)), -0.5f * act(1) * cosf(act(5)), -0.5f * act(2) * cosf(act(6)), 0.5f * act(3) * cosf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.25f * sinf(act(4)), 0.25f * sinf(act(5)), -0.25f * sinf(act(6)), -0.25f * sinf(act(7)), 0.25f * act(0) * cosf(act(4)), 0.25f * act(1) * cosf(act(5)), -0.25f * act(2) * cosf(act(6)), -0.25f * act(3) * cosf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f * cosf(act(4)), -0.25f * cosf(act(5)), -0.25f * cosf(act(6)), -0.25f * cosf(act(7)), 0.25f * act(0) * sinf(act(4)), 0.25f * act(1) * sinf(act(5)), 0.25f * act(2) * sinf(act(6)), 0.25f * act(3) * sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + B = matrix::Matrix(B_tiltrotor_vtol); + break; + } + default: // none break; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 8d8576202772..91af93206e84 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -113,7 +113,8 @@ class ControlAllocator : public ModuleBase, public ModuleParam enum class Airframe { QUAD_W = 0, HEXA_X = 1, - STANDARD_VTOL = 2 + STANDARD_VTOL = 2, + TILTROTOR_VTOL = 3, }; // Inputs From 1cc5baba02e9f83d6ff019d34a5fbfbb1344b188 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 29 Nov 2019 20:29:14 +0100 Subject: [PATCH 061/129] sitl tiltrotor: add direct mixer --- .../px4fmu_common/init.d-posix/1042_tiltrotor | 25 ++++++++++++++++++- .../px4fmu_common/mixers-sitl/CMakeLists.txt | 1 + .../tiltrotor_sitl_direct.main.mix | 14 +++++++++++ 3 files changed, 39 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix diff --git a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor index dd64031f9222..dc3ac51f9856 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor @@ -41,12 +41,35 @@ then param set VT_TILT_FW 1 param set VT_TILT_TRANS 0.6 param set VT_ELEV_MC_LOCK 0 - param set VT_TYPE 1 param set VT_B_TRANS_DUR 8 + # param set VT_TYPE 1 + param set VT_TYPE 2 + + param set CA_AIRFRAME 3 + + param set CA_ACT0_MIN 0 + param set CA_ACT0_MAX 1 + param set CA_ACT1_MIN 0 + param set CA_ACT1_MAX 1 + param set CA_ACT2_MIN 0 + param set CA_ACT2_MAX 1 + param set CA_ACT3_MIN 0 + param set CA_ACT3_MAX 1 + + param set CA_ACT4_MIN -0.1 + param set CA_ACT4_MAX 1.5 + param set CA_ACT5_MIN -0.1 + param set CA_ACT5_MAX 1.5 + param set CA_ACT6_MIN -0.1 + param set CA_ACT6_MAX 1.5 + param set CA_ACT7_MIN -0.1 + param set CA_ACT7_MAX 1.5 + fi set MAV_TYPE 21 set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl.main.mix +# set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl_direct.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt index 18d536981b36..8f52ef94bd46 100644 --- a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt @@ -39,4 +39,5 @@ px4_add_romfs_files( standard_vtol_sitl.main.mix tiltrotor_sitl.main.mix uuv_x_sitl.main.mix + tiltrotor_sitl_direct.main.mix ) diff --git a/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix b/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix new file mode 100644 index 000000000000..ea3adaa0b77d --- /dev/null +++ b/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix @@ -0,0 +1,14 @@ +Mixer for quad tiltrotor (x motor configuration) +================================================ + +A: 0 +A: 1 +A: 2 +A: 3 +A: 4 +A: 5 +A: 6 +A: 7 +A: 8 +A: 9 +A: 10 From 16976b12319eaff44347743b0ce4ea2c0e441be3 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Fri, 29 Nov 2019 20:29:52 +0100 Subject: [PATCH 062/129] control_allocation: make allocation incremental --- .../ControlAllocation/ControlAllocationPseudoInverse.cpp | 2 +- .../ControlAllocationSequentialDesaturation.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 697773e40df3..e2bbdb10364f 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -65,7 +65,7 @@ ControlAllocationPseudoInverse::allocate() updatePseudoInverse(); // Allocate - _actuator_sp = _mix * _control_sp; + _actuator_sp += _mix * (_control_sp - _control_allocated); // Clip _actuator_sp = clipActuatorSetpoint(); diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index f64ddf397f7f..278419b73042 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -48,7 +48,7 @@ ControlAllocationSequentialDesaturation::allocate() updatePseudoInverse(); // Allocate - _actuator_sp = _mix * _control_sp; + _actuator_sp += _mix * (_control_sp - _control_allocated); // go through control axes from lowest to highest priority and unsaturate the actuators for (unsigned i = 0; i < NUM_AXES; i++) { From e07d49fcce2965fe987f2624ca4628e27dfb2c9b Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sun, 1 Dec 2019 17:56:55 +0100 Subject: [PATCH 063/129] control_allocation: update method signature --- .../ControlAllocation/ControlAllocation.cpp | 16 ++++++++++------ .../ControlAllocation/ControlAllocation.hpp | 9 +++++++-- .../ControlAllocationPseudoInverse.cpp | 2 +- .../ControlAllocationSequentialDesaturation.cpp | 2 +- .../control_allocator/ControlAllocator.cpp | 4 +++- 5 files changed, 22 insertions(+), 11 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index f49044792df2..169a8e5cf7dd 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -112,7 +112,7 @@ ControlAllocation::setActuatorSetpoint(const matrix::Vector -ControlAllocation::clipActuatorSetpoint() const +ControlAllocation::clipActuatorSetpoint(const matrix::Vector &actuator) const { - matrix::Vector actuator_clipped = _actuator_sp; + matrix::Vector actuator_clipped; for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) { if (_actuator_max(i) < _actuator_min(i)) { @@ -134,6 +134,9 @@ ControlAllocation::clipActuatorSetpoint() const } else if (actuator_clipped(i) > _actuator_max(i)) { actuator_clipped(i) = _actuator_max(i); + + } else { + actuator_clipped(i) = actuator(i); } } @@ -141,16 +144,17 @@ ControlAllocation::clipActuatorSetpoint() const } matrix::Vector -ControlAllocation::normalizeActuatorSetpoint() const +ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector &actuator) +const { matrix::Vector actuator_normalized; for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) { if (_actuator_min(i) < _actuator_max(i)) { - actuator_normalized(i) = -1.0f + 2.0f * (_actuator_sp(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); + actuator_normalized(i) = -1.0f + 2.0f * (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); } else { - actuator_normalized(i) = -1.0f; + actuator_normalized(i) = -1.0f + 2.0f * (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); } } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 75837cbd64ee..b033f5d3e765 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -185,18 +185,23 @@ class ControlAllocation * * The output is in the range [min; max] * + * @param actuator Actuator vector to clip + * * @return Clipped actuator setpoint */ - matrix::Vector clipActuatorSetpoint() const; + matrix::Vector clipActuatorSetpoint(const matrix::Vector &actuator) const; /** * Normalize the actuator setpoint between minimum and maximum values. * * The output is in the range [-1; +1] * + * @param actuator Actuator vector to normalize + * * @return Clipped actuator setpoint */ - matrix::Vector normalizeActuatorSetpoint() const; + matrix::Vector normalizeActuatorSetpoint(const matrix::Vector &actuator) + const; protected: matrix::Matrix _effectiveness; //< Effectiveness matrix diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index e2bbdb10364f..67bfd327c384 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -68,7 +68,7 @@ ControlAllocationPseudoInverse::allocate() _actuator_sp += _mix * (_control_sp - _control_allocated); // Clip - _actuator_sp = clipActuatorSetpoint(); + _actuator_sp = clipActuatorSetpoint(_actuator_sp); // Compute achieved control _control_allocated = _effectiveness * _actuator_sp; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 278419b73042..49826c450450 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -56,7 +56,7 @@ ControlAllocationSequentialDesaturation::allocate() } // Clip - _actuator_sp = clipActuatorSetpoint(); + _actuator_sp = clipActuatorSetpoint(_actuator_sp); // Compute achieved control _control_allocated = _effectiveness * _actuator_sp; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index f899bd903a68..f8b20944b9d8 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -495,7 +495,9 @@ ControlAllocator::publish_legacy_actuator_controls() actuator_controls_4.timestamp_sample = _timestamp_sample; actuator_controls_5.timestamp_sample = _timestamp_sample; - matrix::Vector actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint(); + matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); + matrix::Vector actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint( + actuator_sp); for (size_t i = 0; i < 8; i++) { actuator_controls_4.control[i] = (PX4_ISFINITE(actuator_sp_normalized(i))) ? actuator_sp_normalized(i) : 0.0f; From 8afc742160ac4368efd4fbdf75fc01b6f6880d6c Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sun, 1 Dec 2019 17:58:22 +0100 Subject: [PATCH 064/129] ControlAllocationPseudoInverse: rename _A_update_needed -> _mix_update_needed --- .../ControlAllocation/ControlAllocationPseudoInverse.cpp | 7 +++---- .../ControlAllocation/ControlAllocationPseudoInverse.hpp | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 67bfd327c384..502ae4589a3c 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -45,16 +45,15 @@ void ControlAllocationPseudoInverse::setEffectivenessMatrix(const matrix::Matrix &B) { - _effectiveness = B; - _A_update_needed = true; + _mix_update_needed = true; } void ControlAllocationPseudoInverse::updatePseudoInverse() { - if (_A_update_needed) { + if (_mix_update_needed) { _mix = matrix::geninv(_effectiveness); - _A_update_needed = false; + _mix_update_needed = false; } } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp index f948810b658d..1695eb93068c 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -59,7 +59,7 @@ class ControlAllocationPseudoInverse: public ControlAllocation protected: matrix::Matrix _mix; - bool _A_update_needed{false}; + bool _mix_update_needed{false}; /** * Recalculate pseudo inverse if required. From fd8a8abd8adb8684fc3c6821adcff2f221f4c003 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sun, 1 Dec 2019 17:58:56 +0100 Subject: [PATCH 065/129] ControlAllocation: set trim with effectiveness --- .../ControlAllocation/ControlAllocation.cpp | 16 +++++++++------- .../ControlAllocation/ControlAllocation.hpp | 7 +++++-- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index 169a8e5cf7dd..4242f99cb54a 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -66,10 +66,13 @@ ControlAllocation::getAllocatedControl() const } void -ControlAllocation::setEffectivenessMatrix(const - matrix::Matrix &B) +ControlAllocation::setEffectivenessMatrix( + const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim) { - _effectiveness = B; + _effectiveness = effectiveness; + _actuator_trim = clipActuatorSetpoint(actuator_trim); + _control_trim = _effectiveness * _actuator_trim; } const matrix::Matrix & @@ -105,8 +108,8 @@ ControlAllocation::getActuatorMax() const } void -ControlAllocation::setActuatorSetpoint(const matrix::Vector - &actuator_sp) +ControlAllocation::setActuatorSetpoint( + const matrix::Vector &actuator_sp) { // Set actuator setpoint _actuator_sp = actuator_sp; @@ -126,8 +129,7 @@ ControlAllocation::clipActuatorSetpoint(const matrix::Vector &B); + virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim); /** * Get the allocated actuator vector @@ -205,9 +206,11 @@ class ControlAllocation protected: matrix::Matrix _effectiveness; //< Effectiveness matrix + matrix::Vector _actuator_trim; //< Neutral actuator values matrix::Vector _actuator_min; //< Minimum actuator values matrix::Vector _actuator_max; //< Maximum actuator values matrix::Vector _actuator_sp; //< Actuator setpoint matrix::Vector _control_sp; //< Control setpoint matrix::Vector _control_allocated; //< Allocated control + matrix::Vector _control_trim; //< Control at trim actuator values }; From 9ccddf5204a8e3188639a0c32a7e73ed7171fab1 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sun, 1 Dec 2019 17:59:21 +0100 Subject: [PATCH 066/129] ControlAllocationPseudoInverse: set trim with effectiveness --- .../ControlAllocation/ControlAllocationPseudoInverse.cpp | 8 +++++--- .../ControlAllocation/ControlAllocationPseudoInverse.hpp | 3 ++- .../ControlAllocationPseudoInverseTest.cpp | 5 +++-- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 502ae4589a3c..c4a57ffb9c6a 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -42,9 +42,11 @@ #include "ControlAllocationPseudoInverse.hpp" void -ControlAllocationPseudoInverse::setEffectivenessMatrix(const - matrix::Matrix &B) +ControlAllocationPseudoInverse::setEffectivenessMatrix( + const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim) { + ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim); _mix_update_needed = true; } @@ -64,7 +66,7 @@ ControlAllocationPseudoInverse::allocate() updatePseudoInverse(); // Allocate - _actuator_sp += _mix * (_control_sp - _control_allocated); + _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); // Clip _actuator_sp = clipActuatorSetpoint(_actuator_sp); diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp index 1695eb93068c..c39c6c19f2b3 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -54,7 +54,8 @@ class ControlAllocationPseudoInverse: public ControlAllocation virtual ~ControlAllocationPseudoInverse() = default; virtual void allocate() override; - virtual void setEffectivenessMatrix(const matrix::Matrix &B) override; + virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, + const matrix::Vector &actuator_trim) override; protected: matrix::Matrix _mix; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp index 2736d3fa240b..fe04cca748bc 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp @@ -51,11 +51,12 @@ TEST(ControlAllocationTest, AllZeroCase) matrix::Vector control_sp; matrix::Vector control_allocated; matrix::Vector control_allocated_expected; - matrix::Matrix B; + matrix::Matrix effectiveness; matrix::Vector actuator_sp; + matrix::Vector actuator_trim; matrix::Vector actuator_sp_expected; - method.setEffectivenessMatrix(B); + method.setEffectivenessMatrix(effectiveness, actuator_trim); method.setControlSetpoint(control_sp); method.allocate(); actuator_sp = method.getActuatorSetpoint(); From 8b3dba70f97ea82f032956cf5344590bbfec30e9 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sun, 1 Dec 2019 18:00:02 +0100 Subject: [PATCH 067/129] ControlAllocationSequentialDesaturation: set trim with effectiveness --- .../ControlAllocationSequentialDesaturation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 49826c450450..6af2bd472c08 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -48,7 +48,7 @@ ControlAllocationSequentialDesaturation::allocate() updatePseudoInverse(); // Allocate - _actuator_sp += _mix * (_control_sp - _control_allocated); + _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); // go through control axes from lowest to highest priority and unsaturate the actuators for (unsigned i = 0; i < NUM_AXES; i++) { From 6a50223f40caee72fba800b621b223e4d59cbc34 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sun, 1 Dec 2019 18:01:35 +0100 Subject: [PATCH 068/129] ControlAllocator: set trim with effectiveness --- .../control_allocator/ControlAllocator.cpp | 90 ++++++++++++++++--- .../control_allocator/ControlAllocator.hpp | 1 + 2 files changed, 81 insertions(+), 10 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index f8b20944b9d8..e70cf760b853 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -126,13 +126,14 @@ ControlAllocator::parameters_updated() actuator_max(15) = _param_ca_act15_max.get(); _control_allocation->setActuatorMax(actuator_max); - matrix::Matrix B = getEffectivenessMatrix(); + matrix::Matrix effectiveness = getEffectivenessMatrix(); + matrix::Vector trim = getActuatorTrim(); // Assign control effectiveness matrix - _control_allocation->setEffectivenessMatrix(B); + _control_allocation->setEffectivenessMatrix(effectiveness, trim); } -const matrix::Matrix +const matrix::Matrix ControlAllocator::getEffectivenessMatrix() { // Control effectiveness @@ -213,17 +214,29 @@ ControlAllocator::getEffectivenessMatrix() } case Airframe::TILTROTOR_VTOL: { - matrix::Vector act = _control_allocation->getActuatorSetpoint(); + matrix::Vector act = getActuatorTrim(); const float B_tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = { - {-0.5f * cosf(act(4)), 0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), 0.5f * act(0) * sinf(act(4)), -0.5f * act(1) * sinf(act(5)), -0.5f * act(2) * sinf(act(6)), 0.5f * act(3) * sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.5f * cosf(act(4)), -0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), -0.5f * act(0) * sinf(act(4)), 0.5f * act(1) * sinf(act(5)), -0.5f * act(2) * sinf(act(6)), 0.5f * act(3) * sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.5f * sinf(act(4)), -0.5f * sinf(act(5)), -0.5f * sinf(act(6)), 0.5f * sinf(act(7)), 0.5f * act(0) * cosf(act(4)), -0.5f * act(1) * cosf(act(5)), -0.5f * act(2) * cosf(act(6)), 0.5f * act(3) * cosf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.25f * sinf(act(4)), 0.25f * sinf(act(5)), -0.25f * sinf(act(6)), -0.25f * sinf(act(7)), 0.25f * act(0) * cosf(act(4)), 0.25f * act(1) * cosf(act(5)), -0.25f * act(2) * cosf(act(6)), -0.25f * act(3) * cosf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.5f * cosf(act(4)), 0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), 0.5f * act(0) *sinf(act(4)), -0.5f * act(1) *sinf(act(5)), -0.5f * act(2) *sinf(act(6)), 0.5f * act(3) *sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f * cosf(act(4)), -0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), -0.5f * act(0) *sinf(act(4)), 0.5f * act(1) *sinf(act(5)), -0.5f * act(2) *sinf(act(6)), 0.5f * act(3) *sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.5f * sinf(act(4)), 0.5f * sinf(act(5)), 0.5f * sinf(act(6)), -0.5f * sinf(act(7)), -0.5f * act(0) *cosf(act(4)), 0.5f * act(1) *cosf(act(5)), 0.5f * act(2) *cosf(act(6)), -0.5f * act(3) *cosf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.25f * sinf(act(4)), 0.25f * sinf(act(5)), 0.25f * sinf(act(6)), 0.25f * sinf(act(7)), 0.25f * act(0) *cosf(act(4)), 0.25f * act(1) *cosf(act(5)), 0.25f * act(2) *cosf(act(6)), 0.25f * act(3) *cosf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.25f * cosf(act(4)), -0.25f * cosf(act(5)), -0.25f * cosf(act(6)), -0.25f * cosf(act(7)), 0.25f * act(0) * sinf(act(4)), 0.25f * act(1) * sinf(act(5)), 0.25f * act(2) * sinf(act(6)), 0.25f * act(3) * sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + {-0.25f * cosf(act(4)), -0.25f * cosf(act(5)), -0.25f * cosf(act(6)), -0.25f * cosf(act(7)), 0.25f * act(0) *sinf(act(4)), 0.25f * act(1) *sinf(act(5)), 0.25f * act(2) *sinf(act(6)), 0.25f * act(3) *sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; B = matrix::Matrix(B_tiltrotor_vtol); + + // Temporarily disable a few controls (WIP) + for (size_t j = 4; j < 8; j++) { + // B(0, j) = 0.0f; + // B(1, j) = 0.0f; + B(2, j) = 0.0f; + + B(3, j) = 0.0f; + B(4, j) = 0.0f; + B(5, j) = 0.0f; + } + break; } @@ -248,6 +261,62 @@ ControlAllocator::getEffectivenessMatrix() return B; } +const matrix::Vector +ControlAllocator::getActuatorTrim() +{ + matrix::Vector act_trim; + + // Current actuator + // matrix::Vector act = _control_allocation->getActuatorSetpoint(); + + switch ((Airframe)_param_ca_airframe.get()) { + case Airframe::QUAD_W: + case Airframe::HEXA_X: + case Airframe::STANDARD_VTOL: { + // zero trim + act_trim = matrix::Vector(); + break; + } + + case Airframe::TILTROTOR_VTOL: { + float tilt = 0.0f; + + switch (_flight_phase) { + case FlightPhase::HOVER_FLIGHT: { + tilt = 0.0f; + break; + } + + case FlightPhase::FORWARD_FLIGHT: { + tilt = 1.5f; + break; + } + + case FlightPhase::TRANSITION_FF_TO_HF: + case FlightPhase::TRANSITION_HF_TO_FF: { + tilt = 0.75f; + break; + } + } + + // Trim: half throttle, tilted motors + act_trim(0) = 0.5f; + act_trim(1) = 0.5f; + act_trim(2) = 0.5f; + act_trim(3) = 0.5f; + act_trim(4) = tilt; + act_trim(5) = tilt; + act_trim(6) = tilt; + act_trim(7) = tilt; + + break; + } + } + + return act_trim; +} + + void ControlAllocator::update_allocation_method() { @@ -397,10 +466,11 @@ ControlAllocator::Run() // Update effectiveness matrix if needed matrix::Matrix B = getEffectivenessMatrix(); + matrix::Vector trim = getActuatorTrim(); // Assign control effectiveness matrix if ((B - _control_allocation->getEffectivenessMatrix()).abs().max() > 1e-5f) { - _control_allocation->setEffectivenessMatrix(B); + _control_allocation->setEffectivenessMatrix(B, trim); } // Set control setpoint vector diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 91af93206e84..e8e711bb2552 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -106,6 +106,7 @@ class ControlAllocator : public ModuleBase, public ModuleParam void publish_legacy_multirotor_motor_limits(); const matrix::Matrix getEffectivenessMatrix(); + const matrix::Vector getActuatorTrim(); int _allocation_method_id{-1}; ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations From 4d6199b6c2865845399df1e8ec6c3878ccfaf8b0 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sun, 1 Dec 2019 18:01:50 +0100 Subject: [PATCH 069/129] sitl tiltrotor: type --- ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor index dc3ac51f9856..e52a8b8e4d1e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor @@ -43,8 +43,8 @@ then param set VT_ELEV_MC_LOCK 0 param set VT_B_TRANS_DUR 8 - # param set VT_TYPE 1 - param set VT_TYPE 2 + param set VT_TYPE 1 + # param set VT_TYPE 2 param set CA_AIRFRAME 3 From 7b19e810b7667fea987678dd18c1ed88259576a3 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sun, 1 Dec 2019 18:25:29 +0100 Subject: [PATCH 070/129] sitl tiltrotor: use control allocation --- ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor | 8 ++++---- src/modules/control_allocator/ControlAllocator.cpp | 10 +++------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor index e52a8b8e4d1e..49cad96f9b1b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor @@ -43,8 +43,8 @@ then param set VT_ELEV_MC_LOCK 0 param set VT_B_TRANS_DUR 8 - param set VT_TYPE 1 - # param set VT_TYPE 2 + # param set VT_TYPE 1 + param set VT_TYPE 2 param set CA_AIRFRAME 3 @@ -70,6 +70,6 @@ fi set MAV_TYPE 21 -set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl.main.mix -# set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl_direct.main.mix +# set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl.main.mix +set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl_direct.main.mix set MIXER custom diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index e70cf760b853..56b61ce7d317 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -217,8 +217,8 @@ ControlAllocator::getEffectivenessMatrix() matrix::Vector act = getActuatorTrim(); const float B_tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = { - {-0.5f * cosf(act(4)), 0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), 0.5f * act(0) *sinf(act(4)), -0.5f * act(1) *sinf(act(5)), -0.5f * act(2) *sinf(act(6)), 0.5f * act(3) *sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.5f * cosf(act(4)), -0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), -0.5f * act(0) *sinf(act(4)), 0.5f * act(1) *sinf(act(5)), -0.5f * act(2) *sinf(act(6)), 0.5f * act(3) *sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.5f * cosf(act(4)), 0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), 0.5f * act(0) *sinf(act(4)), -0.5f * act(1) *sinf(act(5)), -0.5f * act(2) *sinf(act(6)), 0.5f * act(3) *sinf(act(7)), -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f * cosf(act(4)), -0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), -0.5f * act(0) *sinf(act(4)), 0.5f * act(1) *sinf(act(5)), -0.5f * act(2) *sinf(act(6)), 0.5f * act(3) *sinf(act(7)), 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f}, {-0.5f * sinf(act(4)), 0.5f * sinf(act(5)), 0.5f * sinf(act(6)), -0.5f * sinf(act(7)), -0.5f * act(0) *cosf(act(4)), 0.5f * act(1) *cosf(act(5)), 0.5f * act(2) *cosf(act(6)), -0.5f * act(3) *cosf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.25f * sinf(act(4)), 0.25f * sinf(act(5)), 0.25f * sinf(act(6)), 0.25f * sinf(act(7)), 0.25f * act(0) *cosf(act(4)), 0.25f * act(1) *cosf(act(5)), 0.25f * act(2) *cosf(act(6)), 0.25f * act(3) *cosf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, @@ -228,10 +228,6 @@ ControlAllocator::getEffectivenessMatrix() // Temporarily disable a few controls (WIP) for (size_t j = 4; j < 8; j++) { - // B(0, j) = 0.0f; - // B(1, j) = 0.0f; - B(2, j) = 0.0f; - B(3, j) = 0.0f; B(4, j) = 0.0f; B(5, j) = 0.0f; @@ -294,7 +290,7 @@ ControlAllocator::getActuatorTrim() case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: { - tilt = 0.75f; + tilt = 1.0f; break; } } From 5bab9d9861f68e16c1e2ea383683f833eb8437e2 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 9 Jun 2020 08:13:37 +0200 Subject: [PATCH 071/129] Fix rebase conflicts --- msg/actuator_controls.msg | 2 +- src/lib/mixer_module/mixer_module.cpp | 24 ++++++++++++------------ 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index d323359f3ff9..c4b52ea48dfe 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -1,6 +1,6 @@ uint64 timestamp # time since system start (microseconds) uint8 NUM_ACTUATOR_CONTROLS = 8 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 7 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 6 uint8 INDEX_ROLL = 0 uint8 INDEX_PITCH = 1 uint8 INDEX_YAW = 2 diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 1104cb914d22..7527e6190ac5 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -46,18 +46,18 @@ MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &inter bool support_esc_calibration, bool ramp_up) : ModuleParams(&interface), _control_subs{ - {&interface, ORB_ID(actuator_controls_0)}, - {&interface, ORB_ID(actuator_controls_1)}, - {&interface, ORB_ID(actuator_controls_2)}, - {&interface, ORB_ID(actuator_controls_3)}, - {&interface, ORB_ID(actuator_controls_4)}, - {&interface, ORB_ID(actuator_controls_5)} -}, -_scheduling_policy(scheduling_policy), -_support_esc_calibration(support_esc_calibration), -_max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS), -_interface(interface), -_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")) + {&interface, ORB_ID(actuator_controls_0)}, + {&interface, ORB_ID(actuator_controls_1)}, + {&interface, ORB_ID(actuator_controls_2)}, + {&interface, ORB_ID(actuator_controls_3)}, + {&interface, ORB_ID(actuator_controls_4)}, + {&interface, ORB_ID(actuator_controls_5)}, + }, + _scheduling_policy(scheduling_policy), + _support_esc_calibration(support_esc_calibration), + _max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS), + _interface(interface), + _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")) { output_limit_init(&_output_limit); _output_limit.ramp_up = ramp_up; From e3c61c0350a370043e28799ccc8b3df401c84db6 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 9 Jun 2020 08:14:17 +0200 Subject: [PATCH 072/129] angvelctrl: align param order with mc_rate_control --- .../AngularVelocityControl/AngularVelocityControl.cpp | 10 +++------- .../AngularVelocityControl/AngularVelocityControl.hpp | 6 +++--- .../AngularVelocityController.cpp | 2 +- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp index 56c49c2bf10d..7c3ffb206664 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.cpp @@ -54,8 +54,8 @@ void AngularVelocityControl::setSaturationStatus(const matrix::Vector & _saturation_negative = saturation_negative; } -void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_acceleration, - const Vector3f &angular_velocity_sp, const float dt, const bool landed) +void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vector3f &angular_velocity_sp, + const Vector3f &angular_acceleration, const float dt, const bool landed) { // angular rates error Vector3f angular_velocity_error = angular_velocity_sp - angular_velocity; @@ -67,11 +67,7 @@ void AngularVelocityControl::update(const Vector3f &angular_velocity, const Vect Vector3f torque_feedforward = _angular_velocity_int + _gain_ff.emult(angular_velocity_sp); // compute torque setpoint - _torque_sp = ( - _inertia * _angular_accel_sp - + torque_feedforward - + angular_velocity.cross(_inertia * angular_velocity) - ); + _torque_sp = _inertia * _angular_accel_sp + torque_feedforward + angular_velocity.cross(_inertia * angular_velocity); // update integral only if we are not landed if (!landed) { diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp index e91dde775930..aee955c93bc5 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/AngularVelocityControl.hpp @@ -90,13 +90,13 @@ class AngularVelocityControl /** * Run one control loop cycle calculation * @param angular_velocity estimation of the current vehicle angular velocity - * @param angular_acceleration estimation of the current vehicle angular acceleration * @param angular_velocity_sp desired vehicle angular velocity setpoint + * @param angular_acceleration estimation of the current vehicle angular acceleration * @param dt elapsed time since last update * @param landed whether the vehicle is on the ground */ - void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration, - const matrix::Vector3f &angular_velocity_sp, const float dt, const bool landed); + void update(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_velocity_sp, + const matrix::Vector3f &angular_acceleration, const float dt, const bool landed); /** * Get the desired angular acceleration diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index 33db84b30c75..d40931b1eb9f 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -191,7 +191,7 @@ AngularVelocityController::Run() } // run rate controller - _control.update(angular_velocity, _angular_acceleration, _angular_velocity_sp, dt, _maybe_landed || _landed); + _control.update(angular_velocity, _angular_velocity_sp, _angular_acceleration, dt, _maybe_landed || _landed); // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; From 10e4887d3f5f0bc4743469b9c07084cad782c070 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 9 Jun 2020 08:25:08 +0200 Subject: [PATCH 073/129] allocator: fill allocation success flags --- src/modules/control_allocator/ControlAllocator.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 56b61ce7d317..8b52ba2b6710 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -532,6 +532,10 @@ ControlAllocator::publish_control_allocator_status() 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() < FLT_EPSILON); + control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4), unallocated_control(5)).norm() < FLT_EPSILON); + // Actuator saturation matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); matrix::Vector actuator_min = _control_allocation->getActuatorMin(); From 334d34b2d449b86bd4d2019c921cf9025f22cfd4 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 9 Jun 2020 19:27:10 +0200 Subject: [PATCH 074/129] Format --- src/lib/mixer_module/mixer_module.cpp | 24 +++++++++---------- .../control_allocator/ControlAllocator.cpp | 6 +++-- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 7527e6190ac5..73804b9bfb39 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -46,18 +46,18 @@ MixingOutput::MixingOutput(uint8_t max_num_outputs, OutputModuleInterface &inter bool support_esc_calibration, bool ramp_up) : ModuleParams(&interface), _control_subs{ - {&interface, ORB_ID(actuator_controls_0)}, - {&interface, ORB_ID(actuator_controls_1)}, - {&interface, ORB_ID(actuator_controls_2)}, - {&interface, ORB_ID(actuator_controls_3)}, - {&interface, ORB_ID(actuator_controls_4)}, - {&interface, ORB_ID(actuator_controls_5)}, - }, - _scheduling_policy(scheduling_policy), - _support_esc_calibration(support_esc_calibration), - _max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS), - _interface(interface), - _control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")) + {&interface, ORB_ID(actuator_controls_0)}, + {&interface, ORB_ID(actuator_controls_1)}, + {&interface, ORB_ID(actuator_controls_2)}, + {&interface, ORB_ID(actuator_controls_3)}, + {&interface, ORB_ID(actuator_controls_4)}, + {&interface, ORB_ID(actuator_controls_5)}, +}, +_scheduling_policy(scheduling_policy), +_support_esc_calibration(support_esc_calibration), +_max_num_outputs(max_num_outputs < MAX_ACTUATORS ? max_num_outputs : MAX_ACTUATORS), +_interface(interface), +_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")) { output_limit_init(&_output_limit); _output_limit.ramp_up = ramp_up; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 8b52ba2b6710..83995cf19dfd 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -533,8 +533,10 @@ ControlAllocator::publish_control_allocator_status() 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() < FLT_EPSILON); - control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4), unallocated_control(5)).norm() < FLT_EPSILON); + control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1), + unallocated_control(2)).norm() < FLT_EPSILON); + control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4), + unallocated_control(5)).norm() < FLT_EPSILON); // Actuator saturation matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); From 1ca1172a5ccf999cda33ed01e3c54d1e1bf60dd1 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 9 Jun 2020 20:04:31 +0200 Subject: [PATCH 075/129] Add AllocationMethod enum --- .../control_allocator/ControlAllocator.cpp | 25 ++++++++++++++----- .../control_allocator/ControlAllocator.hpp | 8 +++++- 2 files changed, 26 insertions(+), 7 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 83995cf19dfd..7c442398f75b 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -316,7 +316,7 @@ ControlAllocator::getActuatorTrim() void ControlAllocator::update_allocation_method() { - int method = _param_ca_method.get(); + AllocationMethod method = (AllocationMethod)_param_ca_method.get(); if (_allocation_method_id != method) { @@ -331,11 +331,11 @@ ControlAllocator::update_allocation_method() ControlAllocation *tmp = nullptr; switch (method) { - case 0: + case AllocationMethod::PSEUDO_INVERSE: tmp = new ControlAllocationPseudoInverse(); break; - case 1: + case AllocationMethod::SEQUENTIAL_DESATURATION: tmp = new ControlAllocationSequentialDesaturation(); break; @@ -349,7 +349,7 @@ ControlAllocator::update_allocation_method() if (tmp == nullptr) { // It did not work, forget about it PX4_ERR("Control allocation init failed"); - _param_ca_method.set(_allocation_method_id); + _param_ca_method.set((int)_allocation_method_id); } else if (_control_allocation == tmp) { // Nothing has changed, this should not happen @@ -379,7 +379,7 @@ ControlAllocator::update_allocation_method() if (_control_allocation == nullptr) { PX4_ERR("Falling back to ControlAllocationPseudoInverse"); _control_allocation = new ControlAllocationPseudoInverse(); - _allocation_method_id = 0; + _allocation_method_id = AllocationMethod::PSEUDO_INVERSE; } } @@ -606,7 +606,20 @@ int ControlAllocator::task_spawn(int argc, char *argv[]) int ControlAllocator::print_status() { PX4_INFO("Running"); - PX4_INFO("Allocation method: %d", _allocation_method_id); + + switch(_allocation_method_id) { + case AllocationMethod::NONE: + PX4_INFO("Method: None"); + break; + + case AllocationMethod::PSEUDO_INVERSE: + PX4_INFO("Method: Pseudo-inverse"); + break; + + case AllocationMethod::SEQUENTIAL_DESATURATION: + PX4_INFO("Method: Sequential desaturation"); + break; + } perf_print_counter(_loop_perf); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index e8e711bb2552..5a7c27f6e01e 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -108,7 +108,13 @@ class ControlAllocator : public ModuleBase, public ModuleParam const matrix::Matrix getEffectivenessMatrix(); const matrix::Vector getActuatorTrim(); - int _allocation_method_id{-1}; + enum class AllocationMethod { + NONE = -1, + PSEUDO_INVERSE = 0, + SEQUENTIAL_DESATURATION = 1, + }; + + AllocationMethod _allocation_method_id{AllocationMethod::NONE}; ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations enum class Airframe { From 7cefa56a1865c66a1490f0fb3dc637b9cd9c1d51 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 9 Jun 2020 20:13:19 +0200 Subject: [PATCH 076/129] Print more info --- .../control_allocator/ControlAllocator.cpp | 48 +++++++++++++++---- 1 file changed, 38 insertions(+), 10 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 7c442398f75b..38b718821816 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -607,20 +607,48 @@ int ControlAllocator::print_status() { PX4_INFO("Running"); - switch(_allocation_method_id) { - case AllocationMethod::NONE: - PX4_INFO("Method: None"); - break; + // Print current allocation method + switch (_allocation_method_id) { + case AllocationMethod::NONE: + PX4_INFO("Method: None"); + break; - case AllocationMethod::PSEUDO_INVERSE: - PX4_INFO("Method: Pseudo-inverse"); - break; + case AllocationMethod::PSEUDO_INVERSE: + PX4_INFO("Method: Pseudo-inverse"); + break; - case AllocationMethod::SEQUENTIAL_DESATURATION: - PX4_INFO("Method: Sequential desaturation"); - break; + case AllocationMethod::SEQUENTIAL_DESATURATION: + PX4_INFO("Method: Sequential desaturation"); + break; + } + + // Print current airframe + switch ((Airframe)_param_ca_airframe.get()) { + case Airframe::QUAD_W: + PX4_INFO("Airframe: Quad W"); + break; + + case Airframe::HEXA_X: + PX4_INFO("Airframe: Hexa X"); + break; + + case Airframe::STANDARD_VTOL: + PX4_INFO("Airframe: Standard VTOL"); + break; + + case Airframe::TILTROTOR_VTOL: + PX4_INFO("Airframe: Tiltrotor VTOL"); + break; + } + + // Print current effectiveness matrix + if (_control_allocation != nullptr) { + const matrix::Matrix &effectiveness = _control_allocation->getEffectivenessMatrix(); + PX4_INFO("Effectiveness:"); + effectiveness.print(); } + // Print perf perf_print_counter(_loop_perf); return 0; From 0e776d2f9fcead0a00640da106d849abbda5f709 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 11 Jun 2020 08:22:44 +0200 Subject: [PATCH 077/129] Move effectiveness matrices to provider classes --- .../ActuatorEffectiveness.hpp | 116 ++++ .../ActuatorEffectivenessMultirotor.cpp | 62 ++ .../ActuatorEffectivenessMultirotor.hpp | 135 +++++ .../ActuatorEffectivenessMultirotorParams.c | 560 ++++++++++++++++++ .../ActuatorEffectivenessStandardVTOL.cpp | 108 ++++ .../ActuatorEffectivenessStandardVTOL.hpp | 68 +++ .../ActuatorEffectivenessTiltrotorVTOL.cpp | 116 ++++ .../ActuatorEffectivenessTiltrotorVTOL.hpp | 68 +++ .../ActuatorEffectiveness/CMakeLists.txt | 55 ++ src/modules/control_allocator/CMakeLists.txt | 3 + .../ControlAllocation/CMakeLists.txt | 3 + .../control_allocator/ControlAllocator.cpp | 310 ++++------ .../control_allocator/ControlAllocator.hpp | 27 +- .../control_allocator_params.c | 6 +- 14 files changed, 1419 insertions(+), 218 deletions(-) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp new file mode 100644 index 000000000000..174a9069b338 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectiveness.hpp + * + * Interface for Actuator Effectiveness + * + * @author Julien Lecoeur + */ + +#pragma once + +#include + +#include +#include + +class ActuatorEffectiveness +{ +public: + ActuatorEffectiveness() = default; + virtual ~ActuatorEffectiveness() = default; + + static constexpr uint8_t NUM_ACTUATORS = ControlAllocation::NUM_ACTUATORS; + static constexpr uint8_t NUM_AXES = ControlAllocation::NUM_AXES; + + enum class FlightPhase { + HOVER_FLIGHT = 0, + FORWARD_FLIGHT = 1, + TRANSITION_HF_TO_FF = 2, + TRANSITION_FF_TO_HF = 3 + }; + + /** + * Update effectiveness matrix + * + * @return True if the effectiveness matrix has changed + */ + virtual bool update() = 0; + + /** + * Set the current flight phase + * + * @param Flight phase + */ + virtual void setFlightPhase(const FlightPhase &flight_phase) + { + _flight_phase = flight_phase; + }; + + /** + * Get the control effectiveness matrix + * + * @return Effectiveness matrix + */ + const matrix::Matrix &getEffectivenessMatrix() const + { + return _effectiveness; + }; + + /** + * Get the actuator trims + * + * @return Actuator trims + */ + const matrix::Vector &getActuatorTrim() const + { + return _trim; + }; + + /** + * Get the current flight phase + * + * @return Flight phase + */ + const FlightPhase &getFlightPhase() const + { + return _flight_phase; + }; + +protected: + matrix::Matrix _effectiveness; //< Effectiveness matrix + matrix::Vector _trim; //< Actuator trim + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; //< Current flight phase +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp new file mode 100644 index 000000000000..bf9980178538 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessMultirotor.hpp + * + * Actuator effectiveness computed from rotors position and orientation + * + * @author Julien Lecoeur + */ + +#include "ActuatorEffectivenessMultirotor.hpp" + +ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(): + ModuleParams(nullptr) +{ + const float quad_w[NUM_AXES][NUM_ACTUATORS] = { + {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + _effectiveness = matrix::Matrix(quad_w); +} + +bool +ActuatorEffectivenessMultirotor::update() +{ + return false; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp new file mode 100644 index 000000000000..928d0877c1b1 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessMultirotor.hpp + * + * Actuator effectiveness computed from rotors position and orientation + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +#include + +class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessMultirotor(); + virtual ~ActuatorEffectivenessMultirotor() = default; + + /** + * Update effectiveness matrix + * + * @return True if the effectiveness matrix has changed + */ + virtual bool update() override; + +private: + DEFINE_PARAMETERS( + (ParamFloat) _param_ca_mc_r0_px, + (ParamFloat) _param_ca_mc_r0_py, + (ParamFloat) _param_ca_mc_r0_pz, + (ParamFloat) _param_ca_mc_r0_ax, + (ParamFloat) _param_ca_mc_r0_ay, + (ParamFloat) _param_ca_mc_r0_az, + (ParamFloat) _param_ca_mc_r0_kt, + (ParamFloat) _param_ca_mc_r0_km, + + (ParamFloat) _param_ca_mc_r1_px, + (ParamFloat) _param_ca_mc_r1_py, + (ParamFloat) _param_ca_mc_r1_pz, + (ParamFloat) _param_ca_mc_r1_ax, + (ParamFloat) _param_ca_mc_r1_ay, + (ParamFloat) _param_ca_mc_r1_az, + (ParamFloat) _param_ca_mc_r1_kt, + (ParamFloat) _param_ca_mc_r1_km, + + (ParamFloat) _param_ca_mc_r2_px, + (ParamFloat) _param_ca_mc_r2_py, + (ParamFloat) _param_ca_mc_r2_pz, + (ParamFloat) _param_ca_mc_r2_ax, + (ParamFloat) _param_ca_mc_r2_ay, + (ParamFloat) _param_ca_mc_r2_az, + (ParamFloat) _param_ca_mc_r2_kt, + (ParamFloat) _param_ca_mc_r2_km, + + (ParamFloat) _param_ca_mc_r3_px, + (ParamFloat) _param_ca_mc_r3_py, + (ParamFloat) _param_ca_mc_r3_pz, + (ParamFloat) _param_ca_mc_r3_ax, + (ParamFloat) _param_ca_mc_r3_ay, + (ParamFloat) _param_ca_mc_r3_az, + (ParamFloat) _param_ca_mc_r3_kt, + (ParamFloat) _param_ca_mc_r3_km, + + (ParamFloat) _param_ca_mc_r4_px, + (ParamFloat) _param_ca_mc_r4_py, + (ParamFloat) _param_ca_mc_r4_pz, + (ParamFloat) _param_ca_mc_r4_ax, + (ParamFloat) _param_ca_mc_r4_ay, + (ParamFloat) _param_ca_mc_r4_az, + (ParamFloat) _param_ca_mc_r4_kt, + (ParamFloat) _param_ca_mc_r4_km, + + (ParamFloat) _param_ca_mc_r5_px, + (ParamFloat) _param_ca_mc_r5_py, + (ParamFloat) _param_ca_mc_r5_pz, + (ParamFloat) _param_ca_mc_r5_ax, + (ParamFloat) _param_ca_mc_r5_ay, + (ParamFloat) _param_ca_mc_r5_az, + (ParamFloat) _param_ca_mc_r5_kt, + (ParamFloat) _param_ca_mc_r5_km, + + (ParamFloat) _param_ca_mc_r6_px, + (ParamFloat) _param_ca_mc_r6_py, + (ParamFloat) _param_ca_mc_r6_pz, + (ParamFloat) _param_ca_mc_r6_ax, + (ParamFloat) _param_ca_mc_r6_ay, + (ParamFloat) _param_ca_mc_r6_az, + (ParamFloat) _param_ca_mc_r6_kt, + (ParamFloat) _param_ca_mc_r6_km, + + (ParamFloat) _param_ca_mc_r7_px, + (ParamFloat) _param_ca_mc_r7_py, + (ParamFloat) _param_ca_mc_r7_pz, + (ParamFloat) _param_ca_mc_r7_ax, + (ParamFloat) _param_ca_mc_r7_ay, + (ParamFloat) _param_ca_mc_r7_az, + (ParamFloat) _param_ca_mc_r7_kt, + (ParamFloat) _param_ca_mc_r7_km + ) +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c new file mode 100644 index 000000000000..4c5791822fc3 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c @@ -0,0 +1,560 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessMultirotorParams.c + * + * Parameters for the actuator effectiveness of multirotors. + * + * @author Julien Lecoeur + */ + +/** + * Position of rotor 0 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_PX, 0.0); + +/** + * Position of rotor 0 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_PY, 0.0); + +/** + * Position of rotor 0 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_PZ, 0.0); + +/** + * Axis of rotor 0 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_AX, 0.0); + +/** + * Axis of rotor 0 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_AY, 0.0); + +/** + * Axis of rotor 0 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_AZ, -1.0); + +/** + * Thrust coefficient of rotor 0 + * + * The thrust coefficient if defined as Thrust = KT * u^2, + * where u is the output signal send to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_KT, 0.0); + +/** + * Moment coefficient of rotor 0 + * + * The moment coefficient if defined as Torque = KM * u^2, + * where u is the output signal send to the motor controller. + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R0_KM, 0.0); + +/** + * Position of rotor 1 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_PX, 0.0); + +/** + * Position of rotor 1 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_PY, 0.0); + +/** + * Position of rotor 1 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_PZ, 0.0); + +/** + * Axis of rotor 1 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_AX, 0.0); + +/** + * Axis of rotor 1 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_AY, 0.0); + +/** + * Axis of rotor 1 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_AZ, -1.0); + +/** + * Thrust coefficient of rotor 1 + * + * The thrust coefficient if defined as Thrust = KT * u^2, + * where u is the output signal send to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_KT, 0.0); + +/** + * Moment coefficient of rotor 1 + * + * The moment coefficient if defined as Torque = KM * u^2, + * where u is the output signal send to the motor controller. + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R1_KM, 0.0); + +/** + * Position of rotor 2 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_PX, 0.0); + +/** + * Position of rotor 2 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_PY, 0.0); + +/** + * Position of rotor 2 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_PZ, 0.0); + +/** + * Axis of rotor 2 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_AX, 0.0); + +/** + * Axis of rotor 2 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_AY, 0.0); + +/** + * Axis of rotor 2 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_AZ, -1.0); + +/** + * Thrust coefficient of rotor 2 + * + * The thrust coefficient if defined as Thrust = KT * u^2, + * where u is the output signal send to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_KT, 0.0); + +/** + * Moment coefficient of rotor 2 + * + * The moment coefficient if defined as Torque = KM * u^2, + * where u is the output signal send to the motor controller. + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R2_KM, 0.0); + +/** + * Position of rotor 3 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_PX, 0.0); + +/** + * Position of rotor 3 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_PY, 0.0); + +/** + * Position of rotor 3 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_PZ, 0.0); + +/** + * Axis of rotor 3 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_AX, 0.0); + +/** + * Axis of rotor 3 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_AY, 0.0); + +/** + * Axis of rotor 3 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_AZ, -1.0); + +/** + * Thrust coefficient of rotor 3 + * + * The thrust coefficient if defined as Thrust = KT * u^2, + * where u is the output signal send to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_KT, 0.0); + +/** + * Moment coefficient of rotor 3 + * + * The moment coefficient if defined as Torque = KM * u^2, + * where u is the output signal send to the motor controller. + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R3_KM, 0.0); + +/** + * Position of rotor 4 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_PX, 0.0); + +/** + * Position of rotor 4 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_PY, 0.0); + +/** + * Position of rotor 4 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_PZ, 0.0); + +/** + * Axis of rotor 4 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_AX, 0.0); + +/** + * Axis of rotor 4 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_AY, 0.0); + +/** + * Axis of rotor 4 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_AZ, -1.0); + +/** + * Thrust coefficient of rotor 4 + * + * The thrust coefficient if defined as Thrust = KT * u^2, + * where u is the output signal send to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_KT, 0.0); + +/** + * Moment coefficient of rotor 4 + * + * The moment coefficient if defined as Torque = KM * u^2, + * where u is the output signal send to the motor controller. + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R4_KM, 0.0); + +/** + * Position of rotor 5 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_PX, 0.0); + +/** + * Position of rotor 5 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_PY, 0.0); + +/** + * Position of rotor 5 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_PZ, 0.0); + +/** + * Axis of rotor 5 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_AX, 0.0); + +/** + * Axis of rotor 5 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_AY, 0.0); + +/** + * Axis of rotor 5 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_AZ, -1.0); + +/** + * Thrust coefficient of rotor 5 + * + * The thrust coefficient if defined as Thrust = KT * u^2, + * where u is the output signal send to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_KT, 0.0); + +/** + * Moment coefficient of rotor 5 + * + * The moment coefficient if defined as Torque = KM * u^2, + * where u is the output signal send to the motor controller. + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R5_KM, 0.0); + +/** + * Position of rotor 6 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_PX, 0.0); + +/** + * Position of rotor 6 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_PY, 0.0); + +/** + * Position of rotor 6 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_PZ, 0.0); + +/** + * Axis of rotor 6 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_AX, 0.0); + +/** + * Axis of rotor 6 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_AY, 0.0); + +/** + * Axis of rotor 6 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_AZ, -1.0); + +/** + * Thrust coefficient of rotor 6 + * + * The thrust coefficient if defined as Thrust = KT * u^2, + * where u is the output signal send to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_KT, 0.0); + +/** + * Moment coefficient of rotor 6 + * + * The moment coefficient if defined as Torque = KM * u^2, + * where u is the output signal send to the motor controller. + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R6_KM, 0.0); + +/** + * Position of rotor 7 along X body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_PX, 0.0); + +/** + * Position of rotor 7 along Y body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_PY, 0.0); + +/** + * Position of rotor 7 along Z body axis + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_PZ, 0.0); + +/** + * Axis of rotor 7 thrust vector, X body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_AX, 0.0); + +/** + * Axis of rotor 7 thrust vector, Y body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_AY, 0.0); + +/** + * Axis of rotor 7 thrust vector, Z body axis component + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_AZ, -1.0); + +/** + * Thrust coefficient of rotor 7 + * + * The thrust coefficient if defined as Thrust = KT * u^2, + * where u is the output signal send to the motor controller. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_KT, 0.0); + +/** + * Moment coefficient of rotor 7 + * + * The moment coefficient if defined as Torque = KM * u^2, + * where u is the output signal send to the motor controller. + * + * Use a positive value for a rotor with CCW rotation. + * Use a negative value for a rotor with CW rotation. + * + * @group Control Allocation + */ +PARAM_DEFINE_FLOAT(CA_MC_R7_KM, 0.0); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp new file mode 100644 index 000000000000..8acde43c9d35 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessStandardVTOL.hpp + * + * Actuator effectiveness for standard VTOL + * + * @author Julien Lecoeur + */ + +#include "ActuatorEffectivenessStandardVTOL.hpp" + +ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL() +{ + setFlightPhase(FlightPhase::HOVER_FLIGHT); +} + +bool +ActuatorEffectivenessStandardVTOL::update() +{ + if (_updated) { + _updated = false; + return true; + } + + return false; +} + +void +ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + ActuatorEffectiveness::setFlightPhase(flight_phase); + + _updated = true; + + switch (_flight_phase) { + case FlightPhase::HOVER_FLIGHT: { + const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { + {-0.5f, 0.5f, 0.5f, -0.5f, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + _effectiveness = matrix::Matrix(standard_vtol); + break; + } + + case FlightPhase::FORWARD_FLIGHT: { + const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { + { 0.f, 0.f, 0.f, 0.f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + _effectiveness = matrix::Matrix(standard_vtol); + break; + } + + case FlightPhase::TRANSITION_HF_TO_FF: + case FlightPhase::TRANSITION_FF_TO_HF: { + const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { + { -0.5f, 0.5f, 0.5f, -0.5f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + _effectiveness = matrix::Matrix(standard_vtol); + break; + } + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp new file mode 100644 index 000000000000..dd36b5448164 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessStandardVTOL.hpp + * + * Actuator effectiveness for standard VTOL + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessStandardVTOL(); + virtual ~ActuatorEffectivenessStandardVTOL() = default; + + /** + * Update effectiveness matrix + * + * @return True if the effectiveness matrix has changed + */ + virtual bool update() override; + + /** + * Set the current flight phase + * + * @param Flight phase + */ + void setFlightPhase(const FlightPhase &flight_phase) override; + +protected: + bool _updated{false}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp new file mode 100644 index 000000000000..1b69a8789d60 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessTiltrotorVTOL.hpp + * + * Actuator effectiveness for tiltrotor VTOL + * + * @author Julien Lecoeur + */ + +#include "ActuatorEffectivenessTiltrotorVTOL.hpp" + +ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL() +{ + setFlightPhase(FlightPhase::HOVER_FLIGHT); +} + +bool +ActuatorEffectivenessTiltrotorVTOL::update() +{ + if (_updated) { + _updated = false; + return true; + } + + return false; +} + +void +ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + ActuatorEffectiveness::setFlightPhase(flight_phase); + + _updated = true; + + // Trim + float tilt = 0.0f; + + switch (_flight_phase) { + case FlightPhase::HOVER_FLIGHT: { + tilt = 0.0f; + break; + } + + case FlightPhase::FORWARD_FLIGHT: { + tilt = 1.5f; + break; + } + + case FlightPhase::TRANSITION_FF_TO_HF: + case FlightPhase::TRANSITION_HF_TO_FF: { + tilt = 1.0f; + break; + } + } + + // Trim: half throttle, tilted motors + _trim(0) = 0.5f; + _trim(1) = 0.5f; + _trim(2) = 0.5f; + _trim(3) = 0.5f; + _trim(4) = tilt; + _trim(5) = tilt; + _trim(6) = tilt; + _trim(7) = tilt; + + // Effectiveness + const float tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = { + {-0.5f * cosf(_trim(4)), 0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), 0.5f * _trim(0) *sinf(_trim(4)), -0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.5f * cosf(_trim(4)), -0.5f * cosf(_trim(5)), 0.5f * cosf(_trim(6)), -0.5f * cosf(_trim(7)), -0.5f * _trim(0) *sinf(_trim(4)), 0.5f * _trim(1) *sinf(_trim(5)), -0.5f * _trim(2) *sinf(_trim(6)), 0.5f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.5f * sinf(_trim(4)), 0.5f * sinf(_trim(5)), 0.5f * sinf(_trim(6)), -0.5f * sinf(_trim(7)), -0.5f * _trim(0) *cosf(_trim(4)), 0.5f * _trim(1) *cosf(_trim(5)), 0.5f * _trim(2) *cosf(_trim(6)), -0.5f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.25f * sinf(_trim(4)), 0.25f * sinf(_trim(5)), 0.25f * sinf(_trim(6)), 0.25f * sinf(_trim(7)), 0.25f * _trim(0) *cosf(_trim(4)), 0.25f * _trim(1) *cosf(_trim(5)), 0.25f * _trim(2) *cosf(_trim(6)), 0.25f * _trim(3) *cosf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f * cosf(_trim(4)), -0.25f * cosf(_trim(5)), -0.25f * cosf(_trim(6)), -0.25f * cosf(_trim(7)), 0.25f * _trim(0) *sinf(_trim(4)), 0.25f * _trim(1) *sinf(_trim(5)), 0.25f * _trim(2) *sinf(_trim(6)), 0.25f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + _effectiveness = matrix::Matrix(tiltrotor_vtol); + + // Temporarily disable a few controls (WIP) + for (size_t j = 4; j < 8; j++) { + _effectiveness(3, j) = 0.0f; + _effectiveness(4, j) = 0.0f; + _effectiveness(5, j) = 0.0f; + } + +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp new file mode 100644 index 000000000000..f2a983270363 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessTiltrotorVTOL.hpp + * + * Actuator effectiveness for tiltrotor VTOL + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ActuatorEffectiveness.hpp" + +class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessTiltrotorVTOL(); + virtual ~ActuatorEffectivenessTiltrotorVTOL() = default; + + /** + * Update effectiveness matrix + * + * @return True if the effectiveness matrix has changed + */ + virtual bool update() override; + + /** + * Set the current flight phase + * + * @param Flight phase + */ + void setFlightPhase(const FlightPhase &flight_phase) override; + +protected: + bool _updated{false}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt new file mode 100644 index 000000000000..f2e039c9219d --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -0,0 +1,55 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(ActuatorEffectiveness + ActuatorEffectiveness.hpp + ActuatorEffectivenessMultirotor.cpp + ActuatorEffectivenessMultirotor.hpp + ActuatorEffectivenessStandardVTOL.cpp + ActuatorEffectivenessStandardVTOL.hpp + ActuatorEffectivenessTiltrotorVTOL.cpp + ActuatorEffectivenessTiltrotorVTOL.hpp +) + +target_include_directories(ActuatorEffectiveness + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(ActuatorEffectiveness + PRIVATE + mathlib + ControlAllocation +) + +# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index 6628862bd802..97318cedbb38 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/src/modules/control_allocator/CMakeLists.txt @@ -31,6 +31,8 @@ # ############################################################################ +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(ActuatorEffectiveness) add_subdirectory(ControlAllocation) px4_add_module( @@ -42,6 +44,7 @@ px4_add_module( ControlAllocator.hpp DEPENDS mathlib + ActuatorEffectiveness ControlAllocation mixer px4_work_queue diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt index 36a9239859b7..b1fe1239e2df 100644 --- a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt +++ b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt @@ -33,8 +33,11 @@ px4_add_library(ControlAllocation ControlAllocation.cpp + ControlAllocation.hpp ControlAllocationPseudoInverse.cpp + ControlAllocationPseudoInverse.hpp ControlAllocationSequentialDesaturation.cpp + ControlAllocationSequentialDesaturation.hpp ) target_include_directories(ControlAllocation diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 38b718821816..7fd28e7ef5f4 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -60,6 +60,7 @@ ControlAllocator::ControlAllocator() : ControlAllocator::~ControlAllocator() { free(_control_allocation); + free(_actuator_effectiveness); perf_free(_loop_perf); } @@ -82,8 +83,9 @@ ControlAllocator::init() void ControlAllocator::parameters_updated() { - // Allocation method + // Allocation method & effectiveness source // Do this first: in case a new method is loaded, it will be configured below + update_effectiveness_source(); update_allocation_method(); // Minimum actuator values @@ -126,193 +128,14 @@ ControlAllocator::parameters_updated() actuator_max(15) = _param_ca_act15_max.get(); _control_allocation->setActuatorMax(actuator_max); - matrix::Matrix effectiveness = getEffectivenessMatrix(); - matrix::Vector trim = getActuatorTrim(); + // Get actuator effectiveness and trim + matrix::Matrix effectiveness = _actuator_effectiveness->getEffectivenessMatrix(); + matrix::Vector trim = _actuator_effectiveness->getActuatorTrim(); // Assign control effectiveness matrix _control_allocation->setEffectivenessMatrix(effectiveness, trim); } -const matrix::Matrix -ControlAllocator::getEffectivenessMatrix() -{ - // Control effectiveness - matrix::Matrix B; - - switch ((Airframe)_param_ca_airframe.get()) { - case Airframe::QUAD_W: { - const float B_quad_w[NUM_AXES][NUM_ACTUATORS] = { - // quad_w - {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; - B = matrix::Matrix(B_quad_w); - break; - } - - case Airframe::HEXA_X: { - const float B_hexa_x[NUM_AXES][NUM_ACTUATORS] = { - {-0.333333f, 0.333333f, 0.166667f, -0.166667f, -0.166667f, 0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.288675f, -0.288675f, 0.288675f, -0.288675f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.166667f, 0.166667f, -0.166667f, 0.166667f, 0.166667f, -0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f }, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f }, - {-0.166667f, -0.166667f, -0.166667f, -0.166667f, -0.166667f, -0.166667f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - }; - B = matrix::Matrix(B_hexa_x); - break; - } - - case Airframe::STANDARD_VTOL: { - switch (_flight_phase) { - case FlightPhase::HOVER_FLIGHT: { - const float B_standard_vtol[NUM_AXES][NUM_ACTUATORS] = { - {-0.5f, 0.5f, 0.5f, -0.5f, 0.f, 0.0f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; - B = matrix::Matrix(B_standard_vtol); - break; - } - - case FlightPhase::FORWARD_FLIGHT: { - const float B_standard_vtol[NUM_AXES][NUM_ACTUATORS] = { - { 0.f, 0.f, 0.f, 0.f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; - B = matrix::Matrix(B_standard_vtol); - break; - } - - case FlightPhase::TRANSITION_HF_TO_FF: - case FlightPhase::TRANSITION_FF_TO_HF: { - const float B_standard_vtol[NUM_AXES][NUM_ACTUATORS] = { - { -0.5f, 0.5f, 0.5f, -0.5f, 0.f, -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.5f, -0.5f, 0.5f, -0.5f, 0.f, 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.25f, 0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; - B = matrix::Matrix(B_standard_vtol); - break; - } - } - - break; - } - - case Airframe::TILTROTOR_VTOL: { - matrix::Vector act = getActuatorTrim(); - - const float B_tiltrotor_vtol[NUM_AXES][NUM_ACTUATORS] = { - {-0.5f * cosf(act(4)), 0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), 0.5f * act(0) *sinf(act(4)), -0.5f * act(1) *sinf(act(5)), -0.5f * act(2) *sinf(act(6)), 0.5f * act(3) *sinf(act(7)), -0.5f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.5f * cosf(act(4)), -0.5f * cosf(act(5)), 0.5f * cosf(act(6)), -0.5f * cosf(act(7)), -0.5f * act(0) *sinf(act(4)), 0.5f * act(1) *sinf(act(5)), -0.5f * act(2) *sinf(act(6)), 0.5f * act(3) *sinf(act(7)), 0.f, 0.f, 0.5f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.5f * sinf(act(4)), 0.5f * sinf(act(5)), 0.5f * sinf(act(6)), -0.5f * sinf(act(7)), -0.5f * act(0) *cosf(act(4)), 0.5f * act(1) *cosf(act(5)), 0.5f * act(2) *cosf(act(6)), -0.5f * act(3) *cosf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.25f * sinf(act(4)), 0.25f * sinf(act(5)), 0.25f * sinf(act(6)), 0.25f * sinf(act(7)), 0.25f * act(0) *cosf(act(4)), 0.25f * act(1) *cosf(act(5)), 0.25f * act(2) *cosf(act(6)), 0.25f * act(3) *cosf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.25f * cosf(act(4)), -0.25f * cosf(act(5)), -0.25f * cosf(act(6)), -0.25f * cosf(act(7)), 0.25f * act(0) *sinf(act(4)), 0.25f * act(1) *sinf(act(5)), 0.25f * act(2) *sinf(act(6)), 0.25f * act(3) *sinf(act(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; - B = matrix::Matrix(B_tiltrotor_vtol); - - // Temporarily disable a few controls (WIP) - for (size_t j = 4; j < 8; j++) { - B(3, j) = 0.0f; - B(4, j) = 0.0f; - B(5, j) = 0.0f; - } - - break; - } - - default: - // none - break; - } - - matrix::Vector actuator_max = _control_allocation->getActuatorMax(); - matrix::Vector actuator_min = _control_allocation->getActuatorMin(); - - // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) - for (size_t j = 0; j < NUM_ACTUATORS; j++) { - if (actuator_min(j) >= actuator_max(j)) { - for (size_t i = 0; i < NUM_AXES; i++) { - B(i, j) = 0.0f; - } - - } - } - - return B; -} - -const matrix::Vector -ControlAllocator::getActuatorTrim() -{ - matrix::Vector act_trim; - - // Current actuator - // matrix::Vector act = _control_allocation->getActuatorSetpoint(); - - switch ((Airframe)_param_ca_airframe.get()) { - case Airframe::QUAD_W: - case Airframe::HEXA_X: - case Airframe::STANDARD_VTOL: { - // zero trim - act_trim = matrix::Vector(); - break; - } - - case Airframe::TILTROTOR_VTOL: { - float tilt = 0.0f; - - switch (_flight_phase) { - case FlightPhase::HOVER_FLIGHT: { - tilt = 0.0f; - break; - } - - case FlightPhase::FORWARD_FLIGHT: { - tilt = 1.5f; - break; - } - - case FlightPhase::TRANSITION_FF_TO_HF: - case FlightPhase::TRANSITION_HF_TO_FF: { - tilt = 1.0f; - break; - } - } - - // Trim: half throttle, tilted motors - act_trim(0) = 0.5f; - act_trim(1) = 0.5f; - act_trim(2) = 0.5f; - act_trim(3) = 0.5f; - act_trim(4) = tilt; - act_trim(5) = tilt; - act_trim(6) = tilt; - act_trim(7) = tilt; - - break; - } - } - - return act_trim; -} - - void ControlAllocator::update_allocation_method() { @@ -383,6 +206,67 @@ ControlAllocator::update_allocation_method() } } +void +ControlAllocator::update_effectiveness_source() +{ + EffectivenessSource source = (EffectivenessSource)_param_ca_airframe.get(); + + if (_effectiveness_source_id != source) { + + // try to instanciate new effectiveness source + ActuatorEffectiveness *tmp = nullptr; + + switch (source) { + case EffectivenessSource::NONE: + case EffectivenessSource::QUAD_W: + case EffectivenessSource::MC_PARAMS: + tmp = new ActuatorEffectivenessMultirotor(); + break; + + case EffectivenessSource::STANDARD_VTOL: + tmp = new ActuatorEffectivenessStandardVTOL(); + break; + + case EffectivenessSource::TILTROTOR_VTOL: + tmp = new ActuatorEffectivenessTiltrotorVTOL(); + break; + } + + // Replace previous source with new one + if (tmp == nullptr) { + // It did not work, forget about it + PX4_ERR("Actuator effectiveness init failed"); + _param_ca_method.set((int)_effectiveness_source_id); + + } else if (_actuator_effectiveness == tmp) { + // Nothing has changed, this should not happen + PX4_ERR("Actuator effectiveness init error"); + _effectiveness_source_id = source; + + } else { + // Successful update + PX4_INFO("Actuator effectiveness init success"); + + // Swap effectiveness sources + if (_actuator_effectiveness != nullptr) { + free(_actuator_effectiveness); + } + + _actuator_effectiveness = tmp; + + // Save source id + _effectiveness_source_id = source; + } + } + + // Guard against bad initialization + if (_actuator_effectiveness == nullptr) { + PX4_ERR("Falling back to ActuatorEffectivenessMultirotor"); + _actuator_effectiveness = new ActuatorEffectivenessMultirotor(); + _effectiveness_source_id = EffectivenessSource::MC_PARAMS; + } +} + void ControlAllocator::Run() { @@ -409,24 +293,28 @@ ControlAllocator::Run() vehicle_status_s vehicle_status = {}; _vehicle_status_sub.update(&vehicle_status); + ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT}; + // Check if the current flight phase is HOVER or FIXED_WING if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _flight_phase = FlightPhase::HOVER_FLIGHT; + flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT; } else { - _flight_phase = FlightPhase::FORWARD_FLIGHT; + flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT; } // Special cases for VTOL in transition if (vehicle_status.is_vtol && vehicle_status.in_transition_mode) { if (vehicle_status.in_transition_to_fw) { - _flight_phase = FlightPhase::TRANSITION_HF_TO_FF; + flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF; } else { - _flight_phase = FlightPhase::TRANSITION_FF_TO_HF; + flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF; } } + // Forward to effectiveness source + _actuator_effectiveness->setFlightPhase(flight_phase); } // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. @@ -461,12 +349,28 @@ ControlAllocator::Run() if (do_update) { // Update effectiveness matrix if needed - matrix::Matrix B = getEffectivenessMatrix(); - matrix::Vector trim = getActuatorTrim(); + if (_actuator_effectiveness->update()) { + matrix::Matrix effectiveness = _actuator_effectiveness->getEffectivenessMatrix(); + matrix::Vector trim = _actuator_effectiveness->getActuatorTrim(); + + // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) + matrix::Vector actuator_max = _control_allocation->getActuatorMax(); + matrix::Vector actuator_min = _control_allocation->getActuatorMin(); + + for (size_t j = 0; j < NUM_ACTUATORS; j++) { + if (actuator_min(j) >= actuator_max(j)) { + for (size_t i = 0; i < NUM_AXES; i++) { + effectiveness(i, j) = 0.0f; + } + + } + } + + // Assign control effectiveness matrix + if ((effectiveness - _control_allocation->getEffectivenessMatrix()).abs().max() > 1e-5f) { + _control_allocation->setEffectivenessMatrix(effectiveness, trim); + } - // Assign control effectiveness matrix - if ((B - _control_allocation->getEffectivenessMatrix()).abs().max() > 1e-5f) { - _control_allocation->setEffectivenessMatrix(B, trim); } // Set control setpoint vector @@ -623,21 +527,25 @@ int ControlAllocator::print_status() } // Print current airframe - switch ((Airframe)_param_ca_airframe.get()) { - case Airframe::QUAD_W: - PX4_INFO("Airframe: Quad W"); + switch ((EffectivenessSource)_param_ca_airframe.get()) { + case EffectivenessSource::NONE: + PX4_INFO("EffectivenessSource: None"); + break; + + case EffectivenessSource::MC_PARAMS: + PX4_INFO("EffectivenessSource: MC parameters"); break; - case Airframe::HEXA_X: - PX4_INFO("Airframe: Hexa X"); + case EffectivenessSource::QUAD_W: + PX4_INFO("EffectivenessSource: Quad W"); break; - case Airframe::STANDARD_VTOL: - PX4_INFO("Airframe: Standard VTOL"); + case EffectivenessSource::STANDARD_VTOL: + PX4_INFO("EffectivenessSource: Standard VTOL"); break; - case Airframe::TILTROTOR_VTOL: - PX4_INFO("Airframe: Tiltrotor VTOL"); + case EffectivenessSource::TILTROTOR_VTOL: + PX4_INFO("EffectivenessSource: Tiltrotor VTOL"); break; } diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 5a7c27f6e01e..4916685317d9 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -41,6 +41,11 @@ #pragma once +#include +#include +#include +#include + #include #include #include @@ -98,6 +103,7 @@ class ControlAllocator : public ModuleBase, public ModuleParam void parameters_updated(); void update_allocation_method(); + void update_effectiveness_source(); void publish_actuator_setpoint(); void publish_control_allocator_status(); @@ -105,9 +111,6 @@ class ControlAllocator : public ModuleBase, public ModuleParam void publish_legacy_actuator_controls(); void publish_legacy_multirotor_motor_limits(); - const matrix::Matrix getEffectivenessMatrix(); - const matrix::Vector getActuatorTrim(); - enum class AllocationMethod { NONE = -1, PSEUDO_INVERSE = 0, @@ -117,13 +120,17 @@ class ControlAllocator : public ModuleBase, public ModuleParam AllocationMethod _allocation_method_id{AllocationMethod::NONE}; ControlAllocation *_control_allocation{nullptr}; ///< class for control allocation calculations - enum class Airframe { - QUAD_W = 0, - HEXA_X = 1, + enum class EffectivenessSource { + NONE = -1, + MC_PARAMS = 0, + QUAD_W = 1, STANDARD_VTOL = 2, TILTROTOR_VTOL = 3, }; + EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; + ActuatorEffectiveness *_actuator_effectiveness{nullptr}; ///< class providing actuator effectiveness + // Inputs uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)}; /**< vehicle torque setpoint subscription */ uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)}; /**< vehicle thrust setpoint subscription */ @@ -147,14 +154,6 @@ class ControlAllocator : public ModuleBase, public ModuleParam // float _battery_scale_factor{1.0f}; // float _airspeed_scale_factor{1.0f}; - enum class FlightPhase { - HOVER_FLIGHT = 0, - FORWARD_FLIGHT = 1, - TRANSITION_HF_TO_FF = 2, - TRANSITION_FF_TO_HF = 3 - }; - FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; - perf_counter_t _loop_perf; /**< loop duration performance counter */ hrt_abstime _task_start{hrt_absolute_time()}; diff --git a/src/modules/control_allocator/control_allocator_params.c b/src/modules/control_allocator/control_allocator_params.c index 0fc6a969306f..a6eb83d91600 100644 --- a/src/modules/control_allocator/control_allocator_params.c +++ b/src/modules/control_allocator/control_allocator_params.c @@ -45,12 +45,12 @@ * * This is used to retrieve pre-computed control effectiveness matrix * - * @value 0 Test * @min 0 * @max 2 - * @value 0 quad wide - * @value 1 hexa x + * @value 0 multirotor + * @value 1 quad w * @value 2 standard vtol + * @value 3 tiltrotor vtol * @group Control Allocation */ PARAM_DEFINE_INT32(CA_AIRFRAME, 0); From b2ffda0bc279904a564054d869ee5d52d9a23a55 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sun, 14 Jun 2020 13:56:09 +0200 Subject: [PATCH 078/129] Generate MC effectiveness from params Replaces px_generate_mixer.py and toml files --- .../ActuatorEffectivenessMultirotor.cpp | 159 ++++++++++++++++-- .../ActuatorEffectivenessMultirotor.hpp | 28 +++ 2 files changed, 177 insertions(+), 10 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index bf9980178538..d7b0308813d2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -44,19 +44,158 @@ ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(): ModuleParams(nullptr) { - const float quad_w[NUM_AXES][NUM_ACTUATORS] = { - {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} - }; - _effectiveness = matrix::Matrix(quad_w); + parameters_updated(); } bool ActuatorEffectivenessMultirotor::update() { - return false; + bool updated = false; + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + + updated = true; + } + + return updated; +} + +matrix::Matrix +ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry geometry) +{ + matrix::Matrix + effectiveness; + + for (size_t i = 0; i < NUM_ROTORS_MAX; i++) { + // Get rotor axis + matrix::Vector3f axis( + geometry.rotors[i].axis_x, + geometry.rotors[i].axis_y, + geometry.rotors[i].axis_z + ); + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + continue; + } + + // Get rotor position + matrix::Vector3f position( + geometry.rotors[i].position_x, + geometry.rotors[i].position_y, + geometry.rotors[i].position_z + ); + + // Get coefficients + float kt = geometry.rotors[i].k_thrust; + float km = geometry.rotors[i].k_moment; + + // Compute thrust generated by this rotor + matrix::Vector3f thrust = kt * axis; + + // Compute moment generated by this rotor + matrix::Vector3f moment = kt * position.cross(axis) - km * axis; + + // Fill corresponding items in effectiveness matrix + for (size_t j = 0; j < 3; j++) { + effectiveness(j, i) = moment(j); + effectiveness(j + 3, i) = thrust(j); + } + } + + return effectiveness; +} + +void +ActuatorEffectivenessMultirotor::parameters_updated() +{ + // Get multirotor geometry + MultirotorGeometry geometry = {}; + geometry.rotors[0].position_x = _param_ca_mc_r0_px.get(); + geometry.rotors[0].position_y = _param_ca_mc_r0_py.get(); + geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get(); + geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get(); + geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get(); + geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get(); + geometry.rotors[0].k_thrust = _param_ca_mc_r0_kt.get(); + geometry.rotors[0].k_moment = _param_ca_mc_r0_km.get(); + + geometry.rotors[1].position_x = _param_ca_mc_r1_px.get(); + geometry.rotors[1].position_y = _param_ca_mc_r1_py.get(); + geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get(); + geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get(); + geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get(); + geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get(); + geometry.rotors[1].k_thrust = _param_ca_mc_r1_kt.get(); + geometry.rotors[1].k_moment = _param_ca_mc_r1_km.get(); + + geometry.rotors[2].position_x = _param_ca_mc_r2_px.get(); + geometry.rotors[2].position_y = _param_ca_mc_r2_py.get(); + geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get(); + geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get(); + geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get(); + geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get(); + geometry.rotors[2].k_thrust = _param_ca_mc_r2_kt.get(); + geometry.rotors[2].k_moment = _param_ca_mc_r2_km.get(); + + geometry.rotors[3].position_x = _param_ca_mc_r3_px.get(); + geometry.rotors[3].position_y = _param_ca_mc_r3_py.get(); + geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get(); + geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get(); + geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get(); + geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get(); + geometry.rotors[3].k_thrust = _param_ca_mc_r3_kt.get(); + geometry.rotors[3].k_moment = _param_ca_mc_r3_km.get(); + + geometry.rotors[4].position_x = _param_ca_mc_r4_px.get(); + geometry.rotors[4].position_y = _param_ca_mc_r4_py.get(); + geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get(); + geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get(); + geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get(); + geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get(); + geometry.rotors[4].k_thrust = _param_ca_mc_r4_kt.get(); + geometry.rotors[4].k_moment = _param_ca_mc_r4_km.get(); + + geometry.rotors[5].position_x = _param_ca_mc_r5_px.get(); + geometry.rotors[5].position_y = _param_ca_mc_r5_py.get(); + geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get(); + geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get(); + geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get(); + geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get(); + geometry.rotors[5].k_thrust = _param_ca_mc_r5_kt.get(); + geometry.rotors[5].k_moment = _param_ca_mc_r5_km.get(); + + geometry.rotors[6].position_x = _param_ca_mc_r6_px.get(); + geometry.rotors[6].position_y = _param_ca_mc_r6_py.get(); + geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get(); + geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get(); + geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get(); + geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get(); + geometry.rotors[6].k_thrust = _param_ca_mc_r6_kt.get(); + geometry.rotors[6].k_moment = _param_ca_mc_r6_km.get(); + + geometry.rotors[7].position_x = _param_ca_mc_r7_px.get(); + geometry.rotors[7].position_y = _param_ca_mc_r7_py.get(); + geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get(); + geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get(); + geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get(); + geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get(); + geometry.rotors[7].k_thrust = _param_ca_mc_r7_kt.get(); + geometry.rotors[7].k_moment = _param_ca_mc_r7_km.get(); + + // Compute effectiveness matrix + _effectiveness = computeEffectivenessMatrix(geometry); } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index 928d0877c1b1..6e594635f6d6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -44,6 +44,8 @@ #include "ActuatorEffectiveness.hpp" #include +#include +#include class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffectiveness { @@ -58,7 +60,33 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec */ virtual bool update() override; + static constexpr int NUM_ROTORS_MAX = 8; + + typedef struct { + float position_x; + float position_y; + float position_z; + float axis_x; + float axis_y; + float axis_z; + float k_thrust; + float k_moment; + } RotorGeometry; + + typedef struct { + RotorGeometry rotors[NUM_ROTORS_MAX]; + } MultirotorGeometry; + + static matrix::Matrix computeEffectivenessMatrix(MultirotorGeometry); + private: + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + DEFINE_PARAMETERS( (ParamFloat) _param_ca_mc_r0_px, (ParamFloat) _param_ca_mc_r0_py, From fb0ad54fa3eaab67fd1a6f425b991795c712f8ab Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 10:48:34 +0200 Subject: [PATCH 079/129] Run control allocator in its own work queue --- .../px4_platform_common/px4_work_queue/WorkQueueManager.hpp | 4 ++-- src/modules/control_allocator/ControlAllocator.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 82c34ae88d50..ed630d624182 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -48,8 +48,8 @@ struct wq_config_t { namespace wq_configurations { -// static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1664, 0}; // PX4 inner loop highest priority -static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 9500, 0}; // PX4 inner loop highest priority +static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1664, 0}; // PX4 inner loop highest priority +static constexpr wq_config_t ctrl_alloc{"wq:ctrl_alloc", 9500, 0}; // PX4 control allocation, same priority as rate_ctrl static constexpr wq_config_t SPI0{"wq:SPI0", 2336, -1}; static constexpr wq_config_t SPI1{"wq:SPI1", 2336, -2}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 7fd28e7ef5f4..0bec4e902bec 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -51,7 +51,7 @@ using namespace time_literals; ControlAllocator::ControlAllocator() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { parameters_updated(); From e5c5ee5c3b5938af475199af406c49c57b9bcb52 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 11:24:41 +0200 Subject: [PATCH 080/129] Revert CA support from px4io --- src/drivers/px4io/px4io.cpp | 14 +------------- src/modules/px4iofirmware/protocol.h | 4 ---- src/modules/px4iofirmware/px4io.h | 2 +- 3 files changed, 2 insertions(+), 18 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 40afcd893ee9..d0de6a609d58 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -261,8 +261,6 @@ class PX4IO : public cdev::CDev uORB::Subscription _t_actuator_controls_1{ORB_ID(actuator_controls_1)}; ///< actuator controls group 1 topic uORB::Subscription _t_actuator_controls_2{ORB_ID(actuator_controls_2)};; ///< actuator controls group 2 topic uORB::Subscription _t_actuator_controls_3{ORB_ID(actuator_controls_3)};; ///< actuator controls group 3 topic - uORB::Subscription _t_actuator_controls_4{ORB_ID(actuator_controls_4)};; ///< actuator controls group 4 topic - uORB::Subscription _t_actuator_controls_5{ORB_ID(actuator_controls_5)};; ///< actuator controls group 5 topic uORB::Subscription _t_actuator_armed{ORB_ID(actuator_armed)}; ///< system armed control topic uORB::Subscription _t_vehicle_control_mode{ORB_ID(vehicle_control_mode)}; ///< vehicle control mode topic uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; ///< parameter update topic @@ -1263,8 +1261,6 @@ PX4IO::io_set_control_groups() (void)io_set_control_state(1); (void)io_set_control_state(2); (void)io_set_control_state(3); - (void)io_set_control_state(4); - (void)io_set_control_state(5); return ret; } @@ -1299,14 +1295,6 @@ PX4IO::io_set_control_state(unsigned group) case 3: changed = _t_actuator_controls_3.update(&controls); break; - - case 4: - changed = _t_actuator_controls_4.update(&controls); - break; - - case 5: - changed = _t_actuator_controls_5.update(&controls); - break; } if (!changed && (!_in_esc_calibration_mode || group != 0)) { @@ -2389,7 +2377,7 @@ PX4IO::print_status(bool extended_status) io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE)); printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - for (unsigned group = 0; group < 6; group++) { + for (unsigned group = 0; group < 4; group++) { printf("controls %u:", group); for (unsigned i = 0; i < _max_controls; i++) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 858d958d81a4..3cfcee2d1211 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -253,16 +253,12 @@ enum { /* DSM bind states */ #define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ -#define PX4IO_P_CONTROLS_GROUP_4 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 4) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ -#define PX4IO_P_CONTROLS_GROUP_5 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 5) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_VALID 64 #define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ #define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ #define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ #define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP4 (1 << 4) /**< group 4 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP5 (1 << 5) /**< group 5 is valid / received */ /* raw text load to the mixer parser - ignores offset */ #define PX4IO_PAGE_MIXERLOAD 52 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index e17001740587..0ca1819dbfad 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -58,7 +58,7 @@ #define PX4IO_BL_VERSION 3 #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_CONTROL_GROUPS 6 +#define PX4IO_CONTROL_GROUPS 4 #define PX4IO_RC_INPUT_CHANNELS 18 #define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ From 2eca2f2742cfebfb6bdefd03d5cb84298869fb33 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 11:25:44 +0200 Subject: [PATCH 081/129] Add common direct.main.mix mixer --- ROMFS/px4fmu_common/init.d-posix/10016_iris | 2 +- .../init.d-posix/1040_standard_vtol | 2 +- .../init.d-posix/6011_typhoon_h480 | 2 +- ROMFS/px4fmu_common/mixers/CMakeLists.txt | 4 +--- .../mixers/{octo.main.mix => direct.main.mix} | 2 +- ROMFS/px4fmu_common/mixers/hexa.main.mix | 8 -------- ROMFS/px4fmu_common/mixers/quad.main.mix | 20 ------------------- .../mixers/standard_vtol.main.mix | 9 --------- 8 files changed, 5 insertions(+), 44 deletions(-) rename ROMFS/px4fmu_common/mixers/{octo.main.mix => direct.main.mix} (73%) delete mode 100644 ROMFS/px4fmu_common/mixers/hexa.main.mix delete mode 100644 ROMFS/px4fmu_common/mixers/quad.main.mix delete mode 100644 ROMFS/px4fmu_common/mixers/standard_vtol.main.mix diff --git a/ROMFS/px4fmu_common/init.d-posix/10016_iris b/ROMFS/px4fmu_common/init.d-posix/10016_iris index a801c9feb3f8..7fbde2abcc6e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/10016_iris +++ b/ROMFS/px4fmu_common/init.d-posix/10016_iris @@ -27,5 +27,5 @@ then fi # set MIXER quad_w -set MIXER quad +set MIXER direct diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index 8d288fe36a7f..6ad99b11c381 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -68,4 +68,4 @@ set MAV_TYPE 22 # set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix # set MIXER custom -set MIXER standard_vtol +set MIXER direct diff --git a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 index d28c95f3c645..9a5f748823c1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 @@ -52,4 +52,4 @@ fi set MAV_TYPE 13 # set MIXER hexa_x -set MIXER hexa +set MIXER direct diff --git a/ROMFS/px4fmu_common/mixers/CMakeLists.txt b/ROMFS/px4fmu_common/mixers/CMakeLists.txt index 36320d1d1e1c..1238ae2171fd 100644 --- a/ROMFS/px4fmu_common/mixers/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_romfs_files( coax.main.mix delta.main.mix deltaquad.main.mix + direct.main.mix dodeca_bottom_cox.aux.mix dodeca_top_cox.main.mix firefly6.aux.mix @@ -51,7 +52,6 @@ px4_add_romfs_files( fw_generic_wing.main.mix FX79.main.mix generic_diff_rover.main.mix - hexa.main.mix hexa_cox.main.mix hexa_+.main.mix hexa_x.main.mix @@ -59,14 +59,12 @@ px4_add_romfs_files( mount.aux.mix mount_legs.aux.mix ocpoc_quad_x.main.mix - octo.main.mix octo_cox.main.mix octo_cox_w.main.mix octo_+.main.mix octo_x.main.mix pass.aux.mix phantom.main.mix - quad.main.mix quad_dc.main.mix quad_h.main.mix quad_+.main.mix diff --git a/ROMFS/px4fmu_common/mixers/octo.main.mix b/ROMFS/px4fmu_common/mixers/direct.main.mix similarity index 73% rename from ROMFS/px4fmu_common/mixers/octo.main.mix rename to ROMFS/px4fmu_common/mixers/direct.main.mix index a9220e8f688d..65194e94b71c 100644 --- a/ROMFS/px4fmu_common/mixers/octo.main.mix +++ b/ROMFS/px4fmu_common/mixers/direct.main.mix @@ -1,4 +1,4 @@ -# Octo +# Direct mixer A: 0 A: 1 diff --git a/ROMFS/px4fmu_common/mixers/hexa.main.mix b/ROMFS/px4fmu_common/mixers/hexa.main.mix deleted file mode 100644 index 8b96e6f38c0b..000000000000 --- a/ROMFS/px4fmu_common/mixers/hexa.main.mix +++ /dev/null @@ -1,8 +0,0 @@ -# Hexa - -A: 0 -A: 1 -A: 2 -A: 3 -A: 4 -A: 5 diff --git a/ROMFS/px4fmu_common/mixers/quad.main.mix b/ROMFS/px4fmu_common/mixers/quad.main.mix deleted file mode 100644 index 747337b2bed4..000000000000 --- a/ROMFS/px4fmu_common/mixers/quad.main.mix +++ /dev/null @@ -1,20 +0,0 @@ -Control outputs -A: 0 -A: 1 -A: 2 -A: 3 - -AUX1 Passthrough -M: 1 -S: 3 5 10000 10000 0 -10000 10000 - -AUX2 Passthrough -M: 1 -S: 3 6 10000 10000 0 -10000 10000 - -Failsafe outputs -The following outputs are set to their disarmed value -during normal operation and to their failsafe falue in case -of flight termination. -Z: -Z: diff --git a/ROMFS/px4fmu_common/mixers/standard_vtol.main.mix b/ROMFS/px4fmu_common/mixers/standard_vtol.main.mix deleted file mode 100644 index cacc9f7fefeb..000000000000 --- a/ROMFS/px4fmu_common/mixers/standard_vtol.main.mix +++ /dev/null @@ -1,9 +0,0 @@ -Control outputs -A: 0 -A: 1 -A: 2 -A: 3 -A: 4 -A: 5 -A: 6 -A: 7 From 67d45efa630be765dee8ea8e299c40899eedf322 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 12:44:27 +0200 Subject: [PATCH 082/129] Define moment coefficient as moment/thrust ratio --- .../ActuatorEffectivenessMultirotor.cpp | 40 +++++------ .../ActuatorEffectivenessMultirotor.hpp | 20 +++--- .../ActuatorEffectivenessMultirotorParams.c | 72 +++++++++---------- 3 files changed, 62 insertions(+), 70 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index d7b0308813d2..a486422193cb 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -100,14 +100,14 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry g ); // Get coefficients - float kt = geometry.rotors[i].k_thrust; - float km = geometry.rotors[i].k_moment; + float ct = geometry.rotors[i].thrust_coef; + float km = geometry.rotors[i].moment_ratio; // Compute thrust generated by this rotor - matrix::Vector3f thrust = kt * axis; + matrix::Vector3f thrust = ct * axis; // Compute moment generated by this rotor - matrix::Vector3f moment = kt * position.cross(axis) - km * axis; + matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis; // Fill corresponding items in effectiveness matrix for (size_t j = 0; j < 3; j++) { @@ -130,8 +130,8 @@ ActuatorEffectivenessMultirotor::parameters_updated() geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get(); geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get(); geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get(); - geometry.rotors[0].k_thrust = _param_ca_mc_r0_kt.get(); - geometry.rotors[0].k_moment = _param_ca_mc_r0_km.get(); + geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get(); + geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get(); geometry.rotors[1].position_x = _param_ca_mc_r1_px.get(); geometry.rotors[1].position_y = _param_ca_mc_r1_py.get(); @@ -139,8 +139,8 @@ ActuatorEffectivenessMultirotor::parameters_updated() geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get(); geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get(); geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get(); - geometry.rotors[1].k_thrust = _param_ca_mc_r1_kt.get(); - geometry.rotors[1].k_moment = _param_ca_mc_r1_km.get(); + geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get(); + geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get(); geometry.rotors[2].position_x = _param_ca_mc_r2_px.get(); geometry.rotors[2].position_y = _param_ca_mc_r2_py.get(); @@ -148,8 +148,8 @@ ActuatorEffectivenessMultirotor::parameters_updated() geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get(); geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get(); geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get(); - geometry.rotors[2].k_thrust = _param_ca_mc_r2_kt.get(); - geometry.rotors[2].k_moment = _param_ca_mc_r2_km.get(); + geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get(); + geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get(); geometry.rotors[3].position_x = _param_ca_mc_r3_px.get(); geometry.rotors[3].position_y = _param_ca_mc_r3_py.get(); @@ -157,8 +157,8 @@ ActuatorEffectivenessMultirotor::parameters_updated() geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get(); geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get(); geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get(); - geometry.rotors[3].k_thrust = _param_ca_mc_r3_kt.get(); - geometry.rotors[3].k_moment = _param_ca_mc_r3_km.get(); + geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get(); + geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get(); geometry.rotors[4].position_x = _param_ca_mc_r4_px.get(); geometry.rotors[4].position_y = _param_ca_mc_r4_py.get(); @@ -166,8 +166,8 @@ ActuatorEffectivenessMultirotor::parameters_updated() geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get(); geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get(); geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get(); - geometry.rotors[4].k_thrust = _param_ca_mc_r4_kt.get(); - geometry.rotors[4].k_moment = _param_ca_mc_r4_km.get(); + geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get(); + geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get(); geometry.rotors[5].position_x = _param_ca_mc_r5_px.get(); geometry.rotors[5].position_y = _param_ca_mc_r5_py.get(); @@ -175,8 +175,8 @@ ActuatorEffectivenessMultirotor::parameters_updated() geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get(); geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get(); geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get(); - geometry.rotors[5].k_thrust = _param_ca_mc_r5_kt.get(); - geometry.rotors[5].k_moment = _param_ca_mc_r5_km.get(); + geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get(); + geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get(); geometry.rotors[6].position_x = _param_ca_mc_r6_px.get(); geometry.rotors[6].position_y = _param_ca_mc_r6_py.get(); @@ -184,8 +184,8 @@ ActuatorEffectivenessMultirotor::parameters_updated() geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get(); geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get(); geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get(); - geometry.rotors[6].k_thrust = _param_ca_mc_r6_kt.get(); - geometry.rotors[6].k_moment = _param_ca_mc_r6_km.get(); + geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get(); + geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get(); geometry.rotors[7].position_x = _param_ca_mc_r7_px.get(); geometry.rotors[7].position_y = _param_ca_mc_r7_py.get(); @@ -193,8 +193,8 @@ ActuatorEffectivenessMultirotor::parameters_updated() geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get(); geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get(); geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get(); - geometry.rotors[7].k_thrust = _param_ca_mc_r7_kt.get(); - geometry.rotors[7].k_moment = _param_ca_mc_r7_km.get(); + geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get(); + geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get(); // Compute effectiveness matrix _effectiveness = computeEffectivenessMatrix(geometry); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index 6e594635f6d6..1b07a465864c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -69,8 +69,8 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec float axis_x; float axis_y; float axis_z; - float k_thrust; - float k_moment; + float thrust_coef; + float moment_ratio; } RotorGeometry; typedef struct { @@ -94,7 +94,7 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec (ParamFloat) _param_ca_mc_r0_ax, (ParamFloat) _param_ca_mc_r0_ay, (ParamFloat) _param_ca_mc_r0_az, - (ParamFloat) _param_ca_mc_r0_kt, + (ParamFloat) _param_ca_mc_r0_ct, (ParamFloat) _param_ca_mc_r0_km, (ParamFloat) _param_ca_mc_r1_px, @@ -103,7 +103,7 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec (ParamFloat) _param_ca_mc_r1_ax, (ParamFloat) _param_ca_mc_r1_ay, (ParamFloat) _param_ca_mc_r1_az, - (ParamFloat) _param_ca_mc_r1_kt, + (ParamFloat) _param_ca_mc_r1_ct, (ParamFloat) _param_ca_mc_r1_km, (ParamFloat) _param_ca_mc_r2_px, @@ -112,7 +112,7 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec (ParamFloat) _param_ca_mc_r2_ax, (ParamFloat) _param_ca_mc_r2_ay, (ParamFloat) _param_ca_mc_r2_az, - (ParamFloat) _param_ca_mc_r2_kt, + (ParamFloat) _param_ca_mc_r2_ct, (ParamFloat) _param_ca_mc_r2_km, (ParamFloat) _param_ca_mc_r3_px, @@ -121,7 +121,7 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec (ParamFloat) _param_ca_mc_r3_ax, (ParamFloat) _param_ca_mc_r3_ay, (ParamFloat) _param_ca_mc_r3_az, - (ParamFloat) _param_ca_mc_r3_kt, + (ParamFloat) _param_ca_mc_r3_ct, (ParamFloat) _param_ca_mc_r3_km, (ParamFloat) _param_ca_mc_r4_px, @@ -130,7 +130,7 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec (ParamFloat) _param_ca_mc_r4_ax, (ParamFloat) _param_ca_mc_r4_ay, (ParamFloat) _param_ca_mc_r4_az, - (ParamFloat) _param_ca_mc_r4_kt, + (ParamFloat) _param_ca_mc_r4_ct, (ParamFloat) _param_ca_mc_r4_km, (ParamFloat) _param_ca_mc_r5_px, @@ -139,7 +139,7 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec (ParamFloat) _param_ca_mc_r5_ax, (ParamFloat) _param_ca_mc_r5_ay, (ParamFloat) _param_ca_mc_r5_az, - (ParamFloat) _param_ca_mc_r5_kt, + (ParamFloat) _param_ca_mc_r5_ct, (ParamFloat) _param_ca_mc_r5_km, (ParamFloat) _param_ca_mc_r6_px, @@ -148,7 +148,7 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec (ParamFloat) _param_ca_mc_r6_ax, (ParamFloat) _param_ca_mc_r6_ay, (ParamFloat) _param_ca_mc_r6_az, - (ParamFloat) _param_ca_mc_r6_kt, + (ParamFloat) _param_ca_mc_r6_ct, (ParamFloat) _param_ca_mc_r6_km, (ParamFloat) _param_ca_mc_r7_px, @@ -157,7 +157,7 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec (ParamFloat) _param_ca_mc_r7_ax, (ParamFloat) _param_ca_mc_r7_ay, (ParamFloat) _param_ca_mc_r7_az, - (ParamFloat) _param_ca_mc_r7_kt, + (ParamFloat) _param_ca_mc_r7_ct, (ParamFloat) _param_ca_mc_r7_km ) }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c index 4c5791822fc3..f051f1ddff76 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c @@ -84,25 +84,24 @@ PARAM_DEFINE_FLOAT(CA_MC_R0_AZ, -1.0); /** * Thrust coefficient of rotor 0 * - * The thrust coefficient if defined as Thrust = KT * u^2, + * The thrust coefficient if defined as Thrust = CT * u^2, * where u is the output signal send to the motor controller. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R0_KT, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R0_CT, 0.0); /** * Moment coefficient of rotor 0 * - * The moment coefficient if defined as Torque = KM * u^2, - * where u is the output signal send to the motor controller. + * The moment coefficient if defined as Torque = KM * Thrust * * Use a positive value for a rotor with CCW rotation. * Use a negative value for a rotor with CW rotation. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R0_KM, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R0_KM, 0.05); /** * Position of rotor 1 along X body axis @@ -149,25 +148,24 @@ PARAM_DEFINE_FLOAT(CA_MC_R1_AZ, -1.0); /** * Thrust coefficient of rotor 1 * - * The thrust coefficient if defined as Thrust = KT * u^2, + * The thrust coefficient if defined as Thrust = CT * u^2, * where u is the output signal send to the motor controller. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R1_KT, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R1_CT, 0.0); /** * Moment coefficient of rotor 1 * - * The moment coefficient if defined as Torque = KM * u^2, - * where u is the output signal send to the motor controller. + * The moment coefficient if defined as Torque = KM * Thrust, * * Use a positive value for a rotor with CCW rotation. * Use a negative value for a rotor with CW rotation. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R1_KM, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R1_KM, 0.05); /** * Position of rotor 2 along X body axis @@ -214,25 +212,24 @@ PARAM_DEFINE_FLOAT(CA_MC_R2_AZ, -1.0); /** * Thrust coefficient of rotor 2 * - * The thrust coefficient if defined as Thrust = KT * u^2, + * The thrust coefficient if defined as Thrust = CT * u^2, * where u is the output signal send to the motor controller. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R2_KT, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R2_CT, 0.0); /** * Moment coefficient of rotor 2 * - * The moment coefficient if defined as Torque = KM * u^2, - * where u is the output signal send to the motor controller. + * The moment coefficient if defined as Torque = KM * Thrust * * Use a positive value for a rotor with CCW rotation. * Use a negative value for a rotor with CW rotation. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R2_KM, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R2_KM, 0.05); /** * Position of rotor 3 along X body axis @@ -279,25 +276,24 @@ PARAM_DEFINE_FLOAT(CA_MC_R3_AZ, -1.0); /** * Thrust coefficient of rotor 3 * - * The thrust coefficient if defined as Thrust = KT * u^2, + * The thrust coefficient if defined as Thrust = CT * u^2, * where u is the output signal send to the motor controller. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R3_KT, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R3_CT, 0.0); /** * Moment coefficient of rotor 3 * - * The moment coefficient if defined as Torque = KM * u^2, - * where u is the output signal send to the motor controller. + * The moment coefficient if defined as Torque = KM * Thrust * * Use a positive value for a rotor with CCW rotation. * Use a negative value for a rotor with CW rotation. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R3_KM, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R3_KM, 0.05); /** * Position of rotor 4 along X body axis @@ -344,25 +340,24 @@ PARAM_DEFINE_FLOAT(CA_MC_R4_AZ, -1.0); /** * Thrust coefficient of rotor 4 * - * The thrust coefficient if defined as Thrust = KT * u^2, + * The thrust coefficient if defined as Thrust = CT * u^2, * where u is the output signal send to the motor controller. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R4_KT, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R4_CT, 0.0); /** * Moment coefficient of rotor 4 * - * The moment coefficient if defined as Torque = KM * u^2, - * where u is the output signal send to the motor controller. + * The moment coefficient if defined as Torque = KM * Thrust * * Use a positive value for a rotor with CCW rotation. * Use a negative value for a rotor with CW rotation. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R4_KM, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R4_KM, 0.05); /** * Position of rotor 5 along X body axis @@ -409,25 +404,24 @@ PARAM_DEFINE_FLOAT(CA_MC_R5_AZ, -1.0); /** * Thrust coefficient of rotor 5 * - * The thrust coefficient if defined as Thrust = KT * u^2, + * The thrust coefficient if defined as Thrust = CT * u^2, * where u is the output signal send to the motor controller. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R5_KT, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R5_CT, 0.0); /** * Moment coefficient of rotor 5 * - * The moment coefficient if defined as Torque = KM * u^2, - * where u is the output signal send to the motor controller. + * The moment coefficient if defined as Torque = KM * Thrust * * Use a positive value for a rotor with CCW rotation. * Use a negative value for a rotor with CW rotation. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R5_KM, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R5_KM, 0.05); /** * Position of rotor 6 along X body axis @@ -474,25 +468,24 @@ PARAM_DEFINE_FLOAT(CA_MC_R6_AZ, -1.0); /** * Thrust coefficient of rotor 6 * - * The thrust coefficient if defined as Thrust = KT * u^2, + * The thrust coefficient if defined as Thrust = CT * u^2, * where u is the output signal send to the motor controller. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R6_KT, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R6_CT, 0.0); /** * Moment coefficient of rotor 6 * - * The moment coefficient if defined as Torque = KM * u^2, - * where u is the output signal send to the motor controller. + * The moment coefficient if defined as Torque = KM * Thrust * * Use a positive value for a rotor with CCW rotation. * Use a negative value for a rotor with CW rotation. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R6_KM, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R6_KM, 0.05); /** * Position of rotor 7 along X body axis @@ -539,22 +532,21 @@ PARAM_DEFINE_FLOAT(CA_MC_R7_AZ, -1.0); /** * Thrust coefficient of rotor 7 * - * The thrust coefficient if defined as Thrust = KT * u^2, + * The thrust coefficient if defined as Thrust = CT * u^2, * where u is the output signal send to the motor controller. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R7_KT, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R7_CT, 0.0); /** * Moment coefficient of rotor 7 * - * The moment coefficient if defined as Torque = KM * u^2, - * where u is the output signal send to the motor controller. + * The moment coefficient if defined as Torque = KM * Thrust * * Use a positive value for a rotor with CCW rotation. * Use a negative value for a rotor with CW rotation. * * @group Control Allocation */ -PARAM_DEFINE_FLOAT(CA_MC_R7_KM, 0.0); +PARAM_DEFINE_FLOAT(CA_MC_R7_KM, 0.05); From 4f4c2f7adcc3e1deb269cf1f2a4b1f3aea768216 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 12:45:19 +0200 Subject: [PATCH 083/129] Remove unused airframe --- ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol | 2 +- ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor | 2 +- ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 | 2 +- src/modules/control_allocator/ControlAllocator.cpp | 11 +++-------- src/modules/control_allocator/ControlAllocator.hpp | 7 +++---- 5 files changed, 9 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index 6ad99b11c381..711e6f34d835 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -41,7 +41,7 @@ then param set VT_TYPE 2 param set VT_B_TRANS_DUR 8 - param set CA_AIRFRAME 2 + param set CA_AIRFRAME 1 param set CA_ACT0_MIN 0.0 param set CA_ACT1_MIN 0.0 diff --git a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor index 49cad96f9b1b..c111fa2828ec 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor @@ -46,7 +46,7 @@ then # param set VT_TYPE 1 param set VT_TYPE 2 - param set CA_AIRFRAME 3 + param set CA_AIRFRAME 2 param set CA_ACT0_MIN 0 param set CA_ACT0_MAX 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 index 9a5f748823c1..93290190b23e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 @@ -34,7 +34,7 @@ then param set AVC_Y_D 0.0 param set AVC_Z_D 0.0 - param set CA_AIRFRAME 1 + param set CA_AIRFRAME 0 param set CA_ACT0_MIN 0.0 param set CA_ACT1_MIN 0.0 param set CA_ACT2_MIN 0.0 diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 0bec4e902bec..5336e384a796 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -218,8 +218,7 @@ ControlAllocator::update_effectiveness_source() switch (source) { case EffectivenessSource::NONE: - case EffectivenessSource::QUAD_W: - case EffectivenessSource::MC_PARAMS: + case EffectivenessSource::MULTIROTOR: tmp = new ActuatorEffectivenessMultirotor(); break; @@ -263,7 +262,7 @@ ControlAllocator::update_effectiveness_source() if (_actuator_effectiveness == nullptr) { PX4_ERR("Falling back to ActuatorEffectivenessMultirotor"); _actuator_effectiveness = new ActuatorEffectivenessMultirotor(); - _effectiveness_source_id = EffectivenessSource::MC_PARAMS; + _effectiveness_source_id = EffectivenessSource::MULTIROTOR; } } @@ -532,14 +531,10 @@ int ControlAllocator::print_status() PX4_INFO("EffectivenessSource: None"); break; - case EffectivenessSource::MC_PARAMS: + case EffectivenessSource::MULTIROTOR: PX4_INFO("EffectivenessSource: MC parameters"); break; - case EffectivenessSource::QUAD_W: - PX4_INFO("EffectivenessSource: Quad W"); - break; - case EffectivenessSource::STANDARD_VTOL: PX4_INFO("EffectivenessSource: Standard VTOL"); break; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 4916685317d9..4d5b1739a8a5 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -122,10 +122,9 @@ class ControlAllocator : public ModuleBase, public ModuleParam enum class EffectivenessSource { NONE = -1, - MC_PARAMS = 0, - QUAD_W = 1, - STANDARD_VTOL = 2, - TILTROTOR_VTOL = 3, + MULTIROTOR = 0, + STANDARD_VTOL = 1, + TILTROTOR_VTOL = 2, }; EffectivenessSource _effectiveness_source_id{EffectivenessSource::NONE}; From 82517e8488b495979e781196c90c495661492df4 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 12:45:53 +0200 Subject: [PATCH 084/129] Set iris geometry --- ROMFS/px4fmu_common/init.d-posix/10016_iris | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/10016_iris b/ROMFS/px4fmu_common/init.d-posix/10016_iris index 7fbde2abcc6e..84180d77abae 100644 --- a/ROMFS/px4fmu_common/init.d-posix/10016_iris +++ b/ROMFS/px4fmu_common/init.d-posix/10016_iris @@ -24,6 +24,20 @@ then param set CA_ACT1_MAX 1.0 param set CA_ACT2_MAX 1.0 param set CA_ACT3_MAX 1.0 + + param set CA_MC_R0_PX 0.1515 + param set CA_MC_R0_PY 0.245 + param set CA_MC_R0_CT 5.0 + param set CA_MC_R1_PX -0.1515 + param set CA_MC_R1_PY -0.1875 + param set CA_MC_R1_CT 5.0 + param set CA_MC_R2_PX 0.1515 + param set CA_MC_R2_PY -0.245 + param set CA_MC_R2_CT 5.0 + param set CA_MC_R3_PX -0.1515 + param set CA_MC_R3_PY 0.1875 + param set CA_MC_R3_CT 5.0 + fi # set MIXER quad_w From a102f391e1767f179140babd1ea3689870cde039 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 12:46:07 +0200 Subject: [PATCH 085/129] Set iris inertia (same as gazebo model) --- ROMFS/px4fmu_common/init.d-posix/10016_iris | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/10016_iris b/ROMFS/px4fmu_common/init.d-posix/10016_iris index 84180d77abae..e13605442807 100644 --- a/ROMFS/px4fmu_common/init.d-posix/10016_iris +++ b/ROMFS/px4fmu_common/init.d-posix/10016_iris @@ -11,11 +11,13 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF = yes ] then - param set VM_INERTIA_XX 0.0083 - param set VM_INERTIA_YY 0.0083 - param set VM_INERTIA_ZZ 0.0286 + param set VM_MASS 1.5 + param set VM_INERTIA_XX 0.03 + param set VM_INERTIA_YY 0.03 + param set VM_INERTIA_ZZ 0.05 param set CA_AIRFRAME 0 + param set CA_METHOD 1 param set CA_ACT0_MIN 0.0 param set CA_ACT1_MIN 0.0 param set CA_ACT2_MIN 0.0 @@ -27,16 +29,16 @@ then param set CA_MC_R0_PX 0.1515 param set CA_MC_R0_PY 0.245 - param set CA_MC_R0_CT 5.0 + param set CA_MC_R0_CT 6.5 param set CA_MC_R1_PX -0.1515 param set CA_MC_R1_PY -0.1875 - param set CA_MC_R1_CT 5.0 + param set CA_MC_R1_CT 6.5 param set CA_MC_R2_PX 0.1515 param set CA_MC_R2_PY -0.245 - param set CA_MC_R2_CT 5.0 + param set CA_MC_R2_CT 6.5 param set CA_MC_R3_PX -0.1515 param set CA_MC_R3_PY 0.1875 - param set CA_MC_R3_CT 5.0 + param set CA_MC_R3_CT 6.5 fi From c37f16c4f534768ada906aa370574ffb023daf3f Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 15:01:59 +0200 Subject: [PATCH 086/129] AVC: convert thrust_sp into Newtons --- .../AngularVelocityController.cpp | 17 +++++++++++++++++ .../AngularVelocityController.hpp | 10 +++++++++- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index d40931b1eb9f..2b1ca494cb26 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -37,6 +37,7 @@ #include #include #include +#include using namespace matrix; using namespace time_literals; @@ -94,6 +95,10 @@ AngularVelocityController::parameters_updated() }; _control.setInertiaMatrix(matrix::Matrix3f(inertia)); + // Hover thrust + if (!_param_mpc_use_hte.get()) { + _hover_thrust = _param_mpc_thr_hover.get(); + } } void @@ -143,6 +148,15 @@ AngularVelocityController::Run() } } + // Check for updates of hover thrust + if (_param_mpc_use_hte.get()) { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + _hover_thrust = hte.hover_thrust; + } + } + // check angular acceleration topic vehicle_angular_acceleration_s vehicle_angular_acceleration; @@ -158,6 +172,9 @@ AngularVelocityController::Run() _angular_velocity_sp(1) = vehicle_rates_setpoint.pitch; _angular_velocity_sp(2) = vehicle_rates_setpoint.yaw; _thrust_sp = Vector3f(vehicle_rates_setpoint.thrust_body); + + // Scale _thrust_sp in Newton, assuming _hover_thrust is equivalent to 1G + _thrust_sp *= (_param_vm_mass.get() * CONSTANTS_ONE_G / _hover_thrust); } // run the controller diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.hpp b/src/modules/angular_velocity_controller/AngularVelocityController.hpp index e58e19f2c337..a8fdd473e6f0 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.hpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.hpp @@ -48,6 +48,7 @@ #include #include +#include #include #include #include @@ -108,6 +109,7 @@ class AngularVelocityController : public ModuleBase, uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; @@ -130,6 +132,8 @@ class AngularVelocityController : public ModuleBase, matrix::Vector3f _angular_acceleration; /**< angular acceleration (estimated) */ matrix::Vector3f _thrust_sp; /**< thrust setpoint */ + float _hover_thrust{0.5f}; /**< Normalized hover thrust **/ + bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ hrt_abstime _task_start{hrt_absolute_time()}; @@ -158,12 +162,16 @@ class AngularVelocityController : public ModuleBase, (ParamFloat) _param_avc_z_ff, (ParamFloat) _param_avc_z_k, + (ParamFloat) _param_vm_mass, (ParamFloat) _param_vm_inertia_xx, (ParamFloat) _param_vm_inertia_yy, (ParamFloat) _param_vm_inertia_zz, (ParamFloat) _param_vm_inertia_xy, (ParamFloat) _param_vm_inertia_xz, - (ParamFloat) _param_vm_inertia_yz + (ParamFloat) _param_vm_inertia_yz, + + (ParamFloat) _param_mpc_thr_hover, + (ParamBool) _param_mpc_use_hte ) }; From 2f21ecdeec40d4c3c95054ac631dfdb8fb62d728 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 15:02:12 +0200 Subject: [PATCH 087/129] Clarify comment --- .../ActuatorEffectivenessMultirotorParams.c | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c index f051f1ddff76..2fea6eac571a 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorParams.c @@ -85,7 +85,8 @@ PARAM_DEFINE_FLOAT(CA_MC_R0_AZ, -1.0); * Thrust coefficient of rotor 0 * * The thrust coefficient if defined as Thrust = CT * u^2, - * where u is the output signal send to the motor controller. + * where u (with value between CA_ACT0_MIN and CA_ACT0_MAX) + * is the output signal sent to the motor controller. * * @group Control Allocation */ @@ -149,7 +150,8 @@ PARAM_DEFINE_FLOAT(CA_MC_R1_AZ, -1.0); * Thrust coefficient of rotor 1 * * The thrust coefficient if defined as Thrust = CT * u^2, - * where u is the output signal send to the motor controller. + * where u (with value between CA_ACT1_MIN and CA_ACT1_MAX) + * is the output signal sent to the motor controller. * * @group Control Allocation */ @@ -213,7 +215,8 @@ PARAM_DEFINE_FLOAT(CA_MC_R2_AZ, -1.0); * Thrust coefficient of rotor 2 * * The thrust coefficient if defined as Thrust = CT * u^2, - * where u is the output signal send to the motor controller. + * where u (with value between CA_ACT2_MIN and CA_ACT2_MAX) + * is the output signal sent to the motor controller. * * @group Control Allocation */ @@ -277,7 +280,8 @@ PARAM_DEFINE_FLOAT(CA_MC_R3_AZ, -1.0); * Thrust coefficient of rotor 3 * * The thrust coefficient if defined as Thrust = CT * u^2, - * where u is the output signal send to the motor controller. + * where u (with value between CA_ACT3_MIN and CA_ACT3_MAX) + * is the output signal sent to the motor controller. * * @group Control Allocation */ @@ -341,7 +345,8 @@ PARAM_DEFINE_FLOAT(CA_MC_R4_AZ, -1.0); * Thrust coefficient of rotor 4 * * The thrust coefficient if defined as Thrust = CT * u^2, - * where u is the output signal send to the motor controller. + * where u (with value between CA_ACT4_MIN and CA_ACT4_MAX) + * is the output signal sent to the motor controller. * * @group Control Allocation */ @@ -405,7 +410,8 @@ PARAM_DEFINE_FLOAT(CA_MC_R5_AZ, -1.0); * Thrust coefficient of rotor 5 * * The thrust coefficient if defined as Thrust = CT * u^2, - * where u is the output signal send to the motor controller. + * where u (with value between CA_ACT5_MIN and CA_ACT5_MAX) + * is the output signal sent to the motor controller. * * @group Control Allocation */ @@ -469,7 +475,8 @@ PARAM_DEFINE_FLOAT(CA_MC_R6_AZ, -1.0); * Thrust coefficient of rotor 6 * * The thrust coefficient if defined as Thrust = CT * u^2, - * where u is the output signal send to the motor controller. + * where u (with value between CA_ACT6_MIN and CA_ACT6_MAX) + * is the output signal sent to the motor controller. * * @group Control Allocation */ @@ -533,7 +540,8 @@ PARAM_DEFINE_FLOAT(CA_MC_R7_AZ, -1.0); * Thrust coefficient of rotor 7 * * The thrust coefficient if defined as Thrust = CT * u^2, - * where u is the output signal send to the motor controller. + * where u (with value between CA_ACT7_MIN and CA_ACT7_MAX) + * is the output signal sent to the motor controller. * * @group Control Allocation */ From c76a0bb5579a268624a996e6e0aba209b034570f Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 15:11:40 +0200 Subject: [PATCH 088/129] iris: set correct rotor rotation directions --- ROMFS/px4fmu_common/init.d-posix/10016_iris | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/10016_iris b/ROMFS/px4fmu_common/init.d-posix/10016_iris index e13605442807..d37bffc915f6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/10016_iris +++ b/ROMFS/px4fmu_common/init.d-posix/10016_iris @@ -30,15 +30,19 @@ then param set CA_MC_R0_PX 0.1515 param set CA_MC_R0_PY 0.245 param set CA_MC_R0_CT 6.5 + param set CA_MC_R0_KM 0.05 param set CA_MC_R1_PX -0.1515 param set CA_MC_R1_PY -0.1875 param set CA_MC_R1_CT 6.5 + param set CA_MC_R1_KM 0.05 param set CA_MC_R2_PX 0.1515 param set CA_MC_R2_PY -0.245 param set CA_MC_R2_CT 6.5 + param set CA_MC_R2_KM -0.05 param set CA_MC_R3_PX -0.1515 param set CA_MC_R3_PY 0.1875 param set CA_MC_R3_CT 6.5 + param set CA_MC_R3_KM -0.05 fi From 7d4f4388912d346854fe24fcfff687c233bcc839 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 15:14:59 +0200 Subject: [PATCH 089/129] Disable hover thrust estimator (does not play nice with control allocation) --- ROMFS/px4fmu_common/init.d-posix/10016_iris | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/10016_iris b/ROMFS/px4fmu_common/init.d-posix/10016_iris index d37bffc915f6..8de641251cd4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/10016_iris +++ b/ROMFS/px4fmu_common/init.d-posix/10016_iris @@ -11,6 +11,8 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF = yes ] then + param set MPC_USE_HTE 0 + param set VM_MASS 1.5 param set VM_INERTIA_XX 0.03 param set VM_INERTIA_YY 0.03 From b134f9df0c46444c6c44272258c93e6ccd71ce38 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 15:48:55 +0200 Subject: [PATCH 090/129] Display transpose of effectiveness --- src/modules/control_allocator/ControlAllocator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 5336e384a796..cb08774a5eb7 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -547,8 +547,8 @@ int ControlAllocator::print_status() // Print current effectiveness matrix if (_control_allocation != nullptr) { const matrix::Matrix &effectiveness = _control_allocation->getEffectivenessMatrix(); - PX4_INFO("Effectiveness:"); - effectiveness.print(); + PX4_INFO("Effectiveness.T ="); + effectiveness.T().print(); } // Print perf From 84536f8bb53022866828bf9e358431e26e7565db Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 15 Jun 2020 15:49:58 +0200 Subject: [PATCH 091/129] typhoon sitl: configure geometry, mass and inertia Now works with default AVC gains --- .../init.d-posix/6011_typhoon_h480 | 43 +++++++++++++------ 1 file changed, 31 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 index 93290190b23e..eb4268530ce7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 @@ -9,12 +9,6 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF = yes ] then - param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.05 - param set MC_PITCH_P 6.0 - param set MC_ROLLRATE_P 0.15 - param set MC_ROLLRATE_I 0.1 - param set MC_ROLL_P 6.0 param set MPC_XY_VEL_I_ACC 4 param set MPC_XY_VEL_P_ACC 3 @@ -26,15 +20,15 @@ then param set MNT_MODE_IN 0 param set MAV_PROTO_VER 2 - param set VM_INERTIA_XX 0.005 - param set VM_INERTIA_YY 0.005 - param set VM_INERTIA_ZZ 0.01 + param set MPC_USE_HTE 0 - param set AVC_X_D 0.0 - param set AVC_Y_D 0.0 - param set AVC_Z_D 0.0 + param set VM_MASS 2.66 + param set VM_INERTIA_XX 0.03 + param set VM_INERTIA_YY 0.03 + param set VM_INERTIA_ZZ 0.05 param set CA_AIRFRAME 0 + param set CA_METHOD 1 param set CA_ACT0_MIN 0.0 param set CA_ACT1_MIN 0.0 param set CA_ACT2_MIN 0.0 @@ -47,6 +41,31 @@ then param set CA_ACT3_MAX 1.0 param set CA_ACT4_MAX 1.0 param set CA_ACT5_MAX 1.0 + + param set CA_MC_R0_PX 0.0 + param set CA_MC_R0_PY 1.0 + param set CA_MC_R0_CT 9.5 + param set CA_MC_R0_KM -0.05 + param set CA_MC_R1_PX 0.0 + param set CA_MC_R1_PY -1.0 + param set CA_MC_R1_CT 9.5 + param set CA_MC_R1_KM 0.05 + param set CA_MC_R2_PX 0.866025 + param set CA_MC_R2_PY -0.5 + param set CA_MC_R2_CT 9.5 + param set CA_MC_R2_KM -0.05 + param set CA_MC_R3_PX -0.866025 + param set CA_MC_R3_PY 0.5 + param set CA_MC_R3_CT 9.5 + param set CA_MC_R3_KM 0.05 + param set CA_MC_R4_PX 0.866025 + param set CA_MC_R4_PY 0.5 + param set CA_MC_R4_CT 9.5 + param set CA_MC_R4_KM 0.05 + param set CA_MC_R5_PX -0.866025 + param set CA_MC_R5_PY -0.5 + param set CA_MC_R5_CT 9.5 + param set CA_MC_R5_KM -0.05 fi set MAV_TYPE 13 From 451b2a55613fca94e8fea8c8ef1bdefc00ca3758 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 06:32:57 +0200 Subject: [PATCH 092/129] Update src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp Co-authored-by: Daniel Agar --- src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp index bdf55e3f3f28..9e03335c12d3 100644 --- a/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp +++ b/src/lib/mixer/AllocatedActuatorMixer/AllocatedActuatorMixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions From e5827c6dcf819b6cca4925adc26813d1d524d8a2 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 06:38:23 +0200 Subject: [PATCH 093/129] Rename rc.new_apps -> rc.ctrlalloc --- ROMFS/px4fmu_common/init.d-posix/rcS | 4 ++-- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 2 +- ROMFS/px4fmu_common/init.d/{rc.new_apps => rc.ctrlalloc} | 0 3 files changed, 3 insertions(+), 3 deletions(-) rename ROMFS/px4fmu_common/init.d/{rc.new_apps => rc.ctrlalloc} (100%) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index dd0c27577b1c..38f2242f30a6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -300,8 +300,8 @@ else fi sh etc/init.d/rc.logging -# Start new apps -sh etc/init.d/rc.new_apps +# Start new apps for control allocation +sh etc/init.d/rc.control_allocation mavlink boot_complete replay trystart diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 0786a4ea6e3a..557a2eee1bb1 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -51,5 +51,5 @@ px4_add_romfs_files( rc.vehicle_setup rc.vtol_apps rc.vtol_defaults - rc.new_apps + rc.ctrlalloc ) diff --git a/ROMFS/px4fmu_common/init.d/rc.new_apps b/ROMFS/px4fmu_common/init.d/rc.ctrlalloc similarity index 100% rename from ROMFS/px4fmu_common/init.d/rc.new_apps rename to ROMFS/px4fmu_common/init.d/rc.ctrlalloc From 5df96d02413808c138a78d2fcd63f7e096a546f1 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 06:38:56 +0200 Subject: [PATCH 094/129] Stop mc_rate_control when using avc --- ROMFS/px4fmu_common/init.d/rc.ctrlalloc | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.ctrlalloc b/ROMFS/px4fmu_common/init.d/rc.ctrlalloc index f5b3dff12ecb..0a2bcbdf8947 100644 --- a/ROMFS/px4fmu_common/init.d/rc.ctrlalloc +++ b/ROMFS/px4fmu_common/init.d/rc.ctrlalloc @@ -9,6 +9,7 @@ # Start angular velocity controller # angular_velocity_controller start +mc_rate_control stop # # Start Control Allocator From c6edf3f2d65a49c9bd6af841db1d67580c894b8a Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 07:08:51 +0200 Subject: [PATCH 095/129] do not build control alloca modules by default --- boards/px4/fmu-v5/default.cmake | 2 -- boards/px4/sitl/default.cmake | 2 -- 2 files changed, 4 deletions(-) diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 0ab083df6574..9bede18cfd6e 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -61,12 +61,10 @@ px4_add_board( uavcan MODULES airspeed_selector - angular_velocity_controller attitude_estimator_q battery_status camera_feedback commander - control_allocator dataman ekf2 esc_battery diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 42e83abcc345..142b04c1b9c2 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -22,11 +22,9 @@ px4_add_board( #uavcan MODULES airspeed_selector - angular_velocity_controller attitude_estimator_q camera_feedback commander - control_allocator dataman ekf2 events From a87d81b39600e6e8639a0623dddea2825baeec31 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 07:09:21 +0200 Subject: [PATCH 096/129] fmu-v3, fmu-v5, sitl: add *_ctrlalloc target --- boards/px4/fmu-v3/ctrlalloc.cmake | 134 ++++++++++++++++++++++++++++++ boards/px4/fmu-v5/ctrlalloc.cmake | 132 +++++++++++++++++++++++++++++ boards/px4/sitl/ctrlalloc.cmake | 108 ++++++++++++++++++++++++ 3 files changed, 374 insertions(+) create mode 100644 boards/px4/fmu-v3/ctrlalloc.cmake create mode 100644 boards/px4/fmu-v5/ctrlalloc.cmake create mode 100644 boards/px4/sitl/ctrlalloc.cmake diff --git a/boards/px4/fmu-v3/ctrlalloc.cmake b/boards/px4/fmu-v3/ctrlalloc.cmake new file mode 100644 index 000000000000..b9fae5dd19f5 --- /dev/null +++ b/boards/px4/fmu-v3/ctrlalloc.cmake @@ -0,0 +1,134 @@ + +# FMUv3 is FMUv2 with access to the full 2MB flash + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v3 + LABEL ctrlalloc + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + ROMFSROOT px4fmu_common + IO px4_io-v2_default + TESTING + UAVCAN_INTERFACES 2 + SERIAL_PORTS + GPS1:/dev/ttyS3 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS6 + DRIVERS + adc + barometer # all available barometer drivers + batt_smbus + camera_capture + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + dshot + gps + #heater + #imu # all available imu drivers + imu/adis16448 + imu/adis16477 + imu/adis16497 + imu/l3gd20 + imu/lsm303d + imu/invensense/icm20608g + imu/invensense/mpu6000 + imu/invensense/mpu9250 + imu/icm20948 + irlock + lights/blinkm + lights/rgbled + lights/rgbled_ncp5623c + #lights/rgbled_pwm + magnetometer # all available magnetometer drivers + mkblctrl + #optical_flow # all available optical flow drivers + optical_flow/px4flow + #osd + pca9685 + #power_monitor/ina226 + #protocol_splitter + pwm_input + pwm_out_sim + pwm_out + px4io + roboclaw + tap_esc + telemetry # all available telemetry drivers + test_ppm + tone_alarm + uavcan + MODULES + airspeed_selector + angular_velocity_controller + attitude_estimator_q + battery_status + camera_feedback + commander + control_allocator + dataman + ekf2 + esc_battery + events + fw_att_control + fw_pos_control_l1 + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + rover_pos_control + sensors + sih + temperature_compensation + vmount + vtol_att_control + SYSTEMCMDS + bl_update + #dmesg + dumpfile + esc_calib + gpio + hardfault_log + i2cdetect + led_control + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + tests # tests and test runner + top + topic_listener + tune_control + usb_connected + ver + work_queue + EXAMPLES + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + hello + hwtest # Hardware test + #matlab_csv_serial + px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + rover_steering_control # Rover example app + uuv_example_app + work_item + ) diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake new file mode 100644 index 000000000000..340716aa6113 --- /dev/null +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -0,0 +1,132 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v5 + LABEL default + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 + ROMFSROOT px4fmu_common + IO px4_io-v2_default + TESTING + UAVCAN_INTERFACES 2 + SERIAL_PORTS + GPS1:/dev/ttyS0 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS3 + DRIVERS + adc + barometer # all available barometer drivers + batt_smbus + camera_capture + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + dshot + gps + #heater + #imu # all available imu drivers + imu/adis16448 + imu/adis16477 + imu/adis16497 + imu/bmi055 + imu/invensense/icm20602 + imu/invensense/icm20689 + #imu/mpu6000 # legacy icm20602/icm20689 driver + irlock + lights/blinkm + lights/rgbled + lights/rgbled_ncp5623c + lights/rgbled_pwm + magnetometer # all available magnetometer drivers + mkblctrl + optical_flow # all available optical flow drivers + #osd + pca9685 + power_monitor/ina226 + #protocol_splitter + pwm_input + pwm_out_sim + pwm_out + px4io + rc_input + roboclaw + rpm + safety_button + tap_esc + telemetry # all available telemetry drivers + test_ppm + tone_alarm + uavcan + MODULES + airspeed_selector + angular_velocity_controller + attitude_estimator_q + battery_status + camera_feedback + commander + control_allocator + dataman + ekf2 + esc_battery + events + fw_att_control + fw_pos_control_l1 + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + rover_pos_control + sensors + sih + temperature_compensation + vmount + vtol_att_control + SYSTEMCMDS + bl_update + dmesg + dumpfile + esc_calib + gpio + hardfault_log + i2cdetect + led_control + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + tests # tests and test runner + top + topic_listener + tune_control + usb_connected + ver + work_queue + EXAMPLES + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + hello + hwtest # Hardware test + #matlab_csv_serial + px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + rover_steering_control # Rover example app + uuv_example_app + work_item + ) diff --git a/boards/px4/sitl/ctrlalloc.cmake b/boards/px4/sitl/ctrlalloc.cmake new file mode 100644 index 000000000000..9fa612037fad --- /dev/null +++ b/boards/px4/sitl/ctrlalloc.cmake @@ -0,0 +1,108 @@ + +px4_add_board( + PLATFORM posix + VENDOR px4 + MODEL sitl + LABEL default + TESTING + DRIVERS + #barometer # all available barometer drivers + #batt_smbus + camera_capture + camera_trigger + #differential_pressure # all available differential pressure drivers + #distance_sensor # all available distance sensor drivers + gps + #imu # all available imu drivers + #magnetometer # all available magnetometer drivers + pwm_out_sim + rpm/rpm_simulator + #telemetry # all available telemetry drivers + tone_alarm + #uavcan + MODULES + airspeed_selector + angular_velocity_controller + attitude_estimator_q + camera_feedback + commander + control_allocator + dataman + ekf2 + events + fw_att_control + fw_pos_control_l1 + land_detector + landing_target_estimator + #load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + navigator + rc_update + replay + rover_pos_control + sensors + #sih + simulator + temperature_compensation + vmount + vtol_att_control + uuv_att_control + + SYSTEMCMDS + #dumpfile + dyn + esc_calib + led_control + mixer + motor_ramp + motor_test + #mtd + #nshterm + param + perf + pwm + sd_bench + shutdown + tests # tests and test runner + #top + topic_listener + tune_control + ver + work_queue + EXAMPLES + dyn_hello # dynamically loading modules example + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + hello + #hwtest # Hardware test + #matlab_csv_serial + px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + rover_steering_control # Rover example app + uuv_example_app + work_item + ) + +set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") +set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") + +set(config_sitl_debugger disable CACHE STRING "debugger for sitl") +set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") + +# If the environment variable 'replay' is defined, we are building with replay +# support. In this case, we enable the orb publisher rules. +set(REPLAY_FILE "$ENV{replay}") +if(REPLAY_FILE) + message(STATUS "Building with uorb publisher rules support") + add_definitions(-DORB_USE_PUBLISHER_RULES) + + message(STATUS "Building without lockstep for replay") + set(ENABLE_LOCKSTEP_SCHEDULER no) +else() + set(ENABLE_LOCKSTEP_SCHEDULER yes) +endif() From acafdb996200dbc85fe1aeb1a7d3ea7a3030e819 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 07:09:42 +0200 Subject: [PATCH 097/129] sitl: do not start control alloc by default --- ROMFS/px4fmu_common/init.d-posix/rcS | 3 --- 1 file changed, 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 38f2242f30a6..2222e5e0c3e5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -300,8 +300,5 @@ else fi sh etc/init.d/rc.logging -# Start new apps for control allocation -sh etc/init.d/rc.control_allocation - mavlink boot_complete replay trystart From f4b0188404b310bf2868c2bfceab5f525512134c Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 07:12:43 +0200 Subject: [PATCH 098/129] Add iris_ctrlalloc and typhoon_ctralloc sitl airframes --- ROMFS/px4fmu_common/init.d-posix/10016_iris | 42 +---------- .../init.d-posix/10017_iris_ctrlalloc | 53 +++++++++++++ .../init.d-posix/6012_typhoon_ctrlalloc | 75 +++++++++++++++++++ .../init.d-posix/6012_typhoon_ctrlalloc.post | 10 +++ platforms/posix/cmake/sitl_target.cmake | 2 +- 5 files changed, 140 insertions(+), 42 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/10017_iris_ctrlalloc create mode 100644 ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc create mode 100644 ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc.post diff --git a/ROMFS/px4fmu_common/init.d-posix/10016_iris b/ROMFS/px4fmu_common/init.d-posix/10016_iris index 8de641251cd4..9f90ade5bb2a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/10016_iris +++ b/ROMFS/px4fmu_common/init.d-posix/10016_iris @@ -9,45 +9,5 @@ sh /etc/init.d/rc.mc_defaults -if [ $AUTOCNF = yes ] -then - param set MPC_USE_HTE 0 - - param set VM_MASS 1.5 - param set VM_INERTIA_XX 0.03 - param set VM_INERTIA_YY 0.03 - param set VM_INERTIA_ZZ 0.05 - - param set CA_AIRFRAME 0 - param set CA_METHOD 1 - param set CA_ACT0_MIN 0.0 - param set CA_ACT1_MIN 0.0 - param set CA_ACT2_MIN 0.0 - param set CA_ACT3_MIN 0.0 - param set CA_ACT0_MAX 1.0 - param set CA_ACT1_MAX 1.0 - param set CA_ACT2_MAX 1.0 - param set CA_ACT3_MAX 1.0 - - param set CA_MC_R0_PX 0.1515 - param set CA_MC_R0_PY 0.245 - param set CA_MC_R0_CT 6.5 - param set CA_MC_R0_KM 0.05 - param set CA_MC_R1_PX -0.1515 - param set CA_MC_R1_PY -0.1875 - param set CA_MC_R1_CT 6.5 - param set CA_MC_R1_KM 0.05 - param set CA_MC_R2_PX 0.1515 - param set CA_MC_R2_PY -0.245 - param set CA_MC_R2_CT 6.5 - param set CA_MC_R2_KM -0.05 - param set CA_MC_R3_PX -0.1515 - param set CA_MC_R3_PY 0.1875 - param set CA_MC_R3_CT 6.5 - param set CA_MC_R3_KM -0.05 - -fi - -# set MIXER quad_w -set MIXER direct +set MIXER quad_w diff --git a/ROMFS/px4fmu_common/init.d-posix/10017_iris_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/10017_iris_ctrlalloc new file mode 100644 index 000000000000..905bba773554 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/10017_iris_ctrlalloc @@ -0,0 +1,53 @@ +#!/bin/sh +# +# @name 3DR Iris Quadrotor SITL +# +# @type Quadrotor Wide +# +# @maintainer Julian Oes +# + +sh /etc/init.d/rc.mc_defaults +sh /etc/init.d/rc.ctrlalloc + +if [ $AUTOCNF = yes ] +then + param set MPC_USE_HTE 0 + + param set VM_MASS 1.5 + param set VM_INERTIA_XX 0.03 + param set VM_INERTIA_YY 0.03 + param set VM_INERTIA_ZZ 0.05 + + param set CA_AIRFRAME 0 + param set CA_METHOD 1 + param set CA_ACT0_MIN 0.0 + param set CA_ACT1_MIN 0.0 + param set CA_ACT2_MIN 0.0 + param set CA_ACT3_MIN 0.0 + param set CA_ACT0_MAX 1.0 + param set CA_ACT1_MAX 1.0 + param set CA_ACT2_MAX 1.0 + param set CA_ACT3_MAX 1.0 + + param set CA_MC_R0_PX 0.1515 + param set CA_MC_R0_PY 0.245 + param set CA_MC_R0_CT 6.5 + param set CA_MC_R0_KM 0.05 + param set CA_MC_R1_PX -0.1515 + param set CA_MC_R1_PY -0.1875 + param set CA_MC_R1_CT 6.5 + param set CA_MC_R1_KM 0.05 + param set CA_MC_R2_PX 0.1515 + param set CA_MC_R2_PY -0.245 + param set CA_MC_R2_CT 6.5 + param set CA_MC_R2_KM -0.05 + param set CA_MC_R3_PX -0.1515 + param set CA_MC_R3_PY 0.1875 + param set CA_MC_R3_CT 6.5 + param set CA_MC_R3_KM -0.05 + +fi + +set MIXER direct + diff --git a/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc new file mode 100644 index 000000000000..da9ec48a0008 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc @@ -0,0 +1,75 @@ +#!/bin/sh +# +# @name Typhoon H480 SITL +# +# @type Hexarotor x +# + +sh /etc/init.d/rc.mc_defaults +sh /etc/init.d/rc.ctrlalloc + +if [ $AUTOCNF = yes ] +then + param set MPC_XY_VEL_I_ACC 4 + param set MPC_XY_VEL_P_ACC 3 + + param set RTL_DESCEND_ALT 10 + param set RTL_LAND_DELAY 0 + + param set TRIG_INTERFACE 3 + param set TRIG_MODE 4 + param set MNT_MODE_IN 0 + param set MAV_PROTO_VER 2 + + param set MPC_USE_HTE 0 + + param set VM_MASS 2.66 + param set VM_INERTIA_XX 0.03 + param set VM_INERTIA_YY 0.03 + param set VM_INERTIA_ZZ 0.05 + + param set CA_AIRFRAME 0 + param set CA_METHOD 1 + param set CA_ACT0_MIN 0.0 + param set CA_ACT1_MIN 0.0 + param set CA_ACT2_MIN 0.0 + param set CA_ACT3_MIN 0.0 + param set CA_ACT4_MIN 0.0 + param set CA_ACT5_MIN 0.0 + param set CA_ACT0_MAX 1.0 + param set CA_ACT1_MAX 1.0 + param set CA_ACT2_MAX 1.0 + param set CA_ACT3_MAX 1.0 + param set CA_ACT4_MAX 1.0 + param set CA_ACT5_MAX 1.0 + + param set CA_MC_R0_PX 0.0 + param set CA_MC_R0_PY 1.0 + param set CA_MC_R0_CT 9.5 + param set CA_MC_R0_KM -0.05 + param set CA_MC_R1_PX 0.0 + param set CA_MC_R1_PY -1.0 + param set CA_MC_R1_CT 9.5 + param set CA_MC_R1_KM 0.05 + param set CA_MC_R2_PX 0.866025 + param set CA_MC_R2_PY -0.5 + param set CA_MC_R2_CT 9.5 + param set CA_MC_R2_KM -0.05 + param set CA_MC_R3_PX -0.866025 + param set CA_MC_R3_PY 0.5 + param set CA_MC_R3_CT 9.5 + param set CA_MC_R3_KM 0.05 + param set CA_MC_R4_PX 0.866025 + param set CA_MC_R4_PY 0.5 + param set CA_MC_R4_CT 9.5 + param set CA_MC_R4_KM 0.05 + param set CA_MC_R5_PX -0.866025 + param set CA_MC_R5_PY -0.5 + param set CA_MC_R5_CT 9.5 + param set CA_MC_R5_KM -0.05 +fi + +set MAV_TYPE 13 + +# set MIXER hexa_x +set MIXER direct diff --git a/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc.post b/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc.post new file mode 100644 index 000000000000..80264373dc41 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc.post @@ -0,0 +1,10 @@ + +mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix + +mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 + +# shellcheck disable=SC2154 +mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local +# shellcheck disable=SC2154 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local +mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index 105a242d2a41..8d01fae857ea 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -77,7 +77,7 @@ ExternalProject_Add(flightgear_bridge set(viewers none jmavsim gazebo) set(debuggers none ide gdb lldb ddd valgrind callgrind) set(models none shell - if750a iris iris_dual_gps iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps px4vision solo typhoon_h480 + if750a iris iris_dual_gps iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps iris_ctrlalloc px4vision solo typhoon_h480 typhoon_ctrlalloc plane plane_cam plane_catapult plane_lidar standard_vtol tailsitter tiltrotor rover r1_rover boat From 851c4d7763ab1f6ff4bcdb288f290adc0afa08e6 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 07:13:15 +0200 Subject: [PATCH 099/129] Add ActuatorEffectivenessMultirotor unit test --- .../ActuatorEffectivenessMultirotorTest.cpp | 100 ++++++++++++++++++ .../ActuatorEffectiveness/CMakeLists.txt | 2 +- 2 files changed, 101 insertions(+), 1 deletion(-) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp new file mode 100644 index 000000000000..f78088ae767d --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ActuatorEffectivenessMultirotorTest.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include + +using namespace matrix; + +TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase) +{ + // Quad wide geometry + MultirotorGeometry geometry_quad_wide = {}; + geometry_quad_wide.rotors[0].position_x = 0.1515f; + geometry_quad_wide.rotors[0].position_y = 0.245f; + geometry_quad_wide.rotors[0].position_z = 0.0f; + geometry_quad_wide.rotors[0].axis_x = 0.0f; + geometry_quad_wide.rotors[0].axis_y = 0.0f; + geometry_quad_wide.rotors[0].axis_z = -1.0f; + geometry_quad_wide.rotors[0].thrust_coef = 1.0f; + geometry_quad_wide.rotors[0].moment_ratio = 0.05f; + + geometry_quad_wide.rotors[1].position_x = -0.1515f; + geometry_quad_wide.rotors[1].position_y = -0.1875f; + geometry_quad_wide.rotors[1].position_z = 0.0f; + geometry_quad_wide.rotors[1].axis_x = 0.0f; + geometry_quad_wide.rotors[1].axis_y = 0.0f; + geometry_quad_wide.rotors[1].axis_z = -1.0f; + geometry_quad_wide.rotors[1].thrust_coef = 1.0f; + geometry_quad_wide.rotors[1].moment_ratio = 0.05f; + + geometry_quad_wide.rotors[2].position_x = 0.1515f; + geometry_quad_wide.rotors[2].position_y = -0.245f; + geometry_quad_wide.rotors[2].position_z = 0.0f; + geometry_quad_wide.rotors[2].axis_x = 0.0f; + geometry_quad_wide.rotors[2].axis_y = 0.0f; + geometry_quad_wide.rotors[2].axis_z = -1.0f; + geometry_quad_wide.rotors[2].thrust_coef = 1.0f; + geometry_quad_wide.rotors[2].moment_ratio = -0.05f; + + geometry_quad_wide.rotors[3].position_x = -0.1515f;` + geometry_quad_wide.rotors[3].position_y = 0.1875f; + geometry_quad_wide.rotors[3].position_z = 0.0f; + geometry_quad_wide.rotors[3].axis_x = 0.0f; + geometry_quad_wide.rotors[3].axis_y = 0.0f; + geometry_quad_wide.rotors[3].axis_z = -1.0f; + geometry_quad_wide.rotors[3].thrust_coef = 1.0f; + geometry_quad_wide.rotors[3].moment_ratio = -0.05f; + + effectiveness_quad_wide = ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry_quad_wide); + + const float quad_wide[NUM_AXES][NUM_ACTUATORS] = { + {-0.245f, 0.1875f, 0.245f, -0.1875f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.1515f, -0.1515f, 0.1515f, -0.1515f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.05f, 0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + effectiveness_quad_wide_expected = matrix::Matrix(quad_wide); + + EXPECT_EQ(effectiveness_quad_wide, effectiveness_quad_wide_expected); +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index f2e039c9219d..cb59819df6b6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -52,4 +52,4 @@ target_link_libraries(ActuatorEffectiveness ControlAllocation ) -# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness) +px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness) From 3a135bf71cc4fcadf03c79e1d7f860e1ebc8e2ae Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 07:46:48 +0200 Subject: [PATCH 100/129] revert typhoon sitl airframe changes --- .../init.d-posix/6011_typhoon_h480 | 56 +++---------------- 1 file changed, 7 insertions(+), 49 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 index eb4268530ce7..4955f3b578a9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480 @@ -9,6 +9,12 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF = yes ] then + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.05 + param set MC_PITCH_P 6.0 + param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_I 0.1 + param set MC_ROLL_P 6.0 param set MPC_XY_VEL_I_ACC 4 param set MPC_XY_VEL_P_ACC 3 @@ -19,56 +25,8 @@ then param set TRIG_MODE 4 param set MNT_MODE_IN 0 param set MAV_PROTO_VER 2 - - param set MPC_USE_HTE 0 - - param set VM_MASS 2.66 - param set VM_INERTIA_XX 0.03 - param set VM_INERTIA_YY 0.03 - param set VM_INERTIA_ZZ 0.05 - - param set CA_AIRFRAME 0 - param set CA_METHOD 1 - param set CA_ACT0_MIN 0.0 - param set CA_ACT1_MIN 0.0 - param set CA_ACT2_MIN 0.0 - param set CA_ACT3_MIN 0.0 - param set CA_ACT4_MIN 0.0 - param set CA_ACT5_MIN 0.0 - param set CA_ACT0_MAX 1.0 - param set CA_ACT1_MAX 1.0 - param set CA_ACT2_MAX 1.0 - param set CA_ACT3_MAX 1.0 - param set CA_ACT4_MAX 1.0 - param set CA_ACT5_MAX 1.0 - - param set CA_MC_R0_PX 0.0 - param set CA_MC_R0_PY 1.0 - param set CA_MC_R0_CT 9.5 - param set CA_MC_R0_KM -0.05 - param set CA_MC_R1_PX 0.0 - param set CA_MC_R1_PY -1.0 - param set CA_MC_R1_CT 9.5 - param set CA_MC_R1_KM 0.05 - param set CA_MC_R2_PX 0.866025 - param set CA_MC_R2_PY -0.5 - param set CA_MC_R2_CT 9.5 - param set CA_MC_R2_KM -0.05 - param set CA_MC_R3_PX -0.866025 - param set CA_MC_R3_PY 0.5 - param set CA_MC_R3_CT 9.5 - param set CA_MC_R3_KM 0.05 - param set CA_MC_R4_PX 0.866025 - param set CA_MC_R4_PY 0.5 - param set CA_MC_R4_CT 9.5 - param set CA_MC_R4_KM 0.05 - param set CA_MC_R5_PX -0.866025 - param set CA_MC_R5_PY -0.5 - param set CA_MC_R5_CT 9.5 - param set CA_MC_R5_KM -0.05 fi set MAV_TYPE 13 -# set MIXER hexa_x -set MIXER direct +set MIXER hexa_x From 804b30d78eb9c7e8652f28b7f9ca3b7b3c5d90f3 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 07:48:14 +0200 Subject: [PATCH 101/129] Update sitl_gazebo with control_allocation sitl models --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 1af7e29dbb1e..18e714c03eca 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 1af7e29dbb1ecce7b0b191c9deb24ab1f13916ab +Subproject commit 18e714c03ecaba9f872772c0351778a16aba7a7b From 1b49fbcc0c0f50f20cf7079b8309525ede324e96 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 08:12:44 +0200 Subject: [PATCH 102/129] Add target px4_fmu-v4_ctrlalloc --- boards/px4/fmu-v4/ctrlalloc.cmake | 128 ++++++++++++++++++++++++++++++ 1 file changed, 128 insertions(+) create mode 100644 boards/px4/fmu-v4/ctrlalloc.cmake diff --git a/boards/px4/fmu-v4/ctrlalloc.cmake b/boards/px4/fmu-v4/ctrlalloc.cmake new file mode 100644 index 000000000000..7b49a7f0986f --- /dev/null +++ b/boards/px4/fmu-v4/ctrlalloc.cmake @@ -0,0 +1,128 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR px4 + MODEL fmu-v4 + LABEL default + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + ROMFSROOT px4fmu_common + TESTING + UAVCAN_INTERFACES 1 + SERIAL_PORTS + GPS1:/dev/ttyS3 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + DRIVERS + adc + barometer # all available barometer drivers + batt_smbus + camera_capture + camera_trigger + differential_pressure # all available differential pressure drivers + distance_sensor # all available distance sensor drivers + dshot + gps + heater + #imu # all available imu drivers + imu/adis16448 + imu/adis16477 + imu/adis16497 + imu/invensense/icm20602 + imu/invensense/icm20608g + imu/invensense/icm40609d + imu/invensense/mpu6500 + imu/invensense/mpu9250 + irlock + lights/blinkm + lights/rgbled + lights/rgbled_ncp5623c + magnetometer # all available magnetometer drivers + mkblctrl + optical_flow # all available optical flow drivers + #osd + pca9685 + #protocol_splitter + pwm_input + pwm_out_sim + pwm_out + rc_input + roboclaw + safety_button + tap_esc + telemetry # all available telemetry drivers + test_ppm + tone_alarm + uavcan + MODULES + airspeed_selector + angular_velocity_controller + attitude_estimator_q + battery_status + camera_feedback + commander + control_allocator + dataman + ekf2 + esc_battery + events + fw_att_control + fw_pos_control_l1 + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_hover_thrust_estimator + mc_pos_control + mc_rate_control + #micrortps_bridge + navigator + rc_update + rover_pos_control + sensors + sih + temperature_compensation + uuv_att_control + vmount + vtol_att_control + SYSTEMCMDS + bl_update + #dmesg + dumpfile + esc_calib + gpio + hardfault_log + i2cdetect + led_control + mixer + motor_ramp + motor_test + mtd + nshterm + param + perf + pwm + reboot + reflect + sd_bench + tests # tests and test runner + top + topic_listener + tune_control + usb_connected + ver + work_queue + EXAMPLES + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + hello + hwtest # Hardware test + #matlab_csv_serial + px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + rover_steering_control # Rover example app + uuv_example_app + work_item + ) From 31615ec8a081d21e1e4417862a291d3468b3519a Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Tue, 16 Jun 2020 08:13:06 +0200 Subject: [PATCH 103/129] Run AVC in wq:ctrl_alloc --- .../angular_velocity_controller/AngularVelocityController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index 2b1ca494cb26..c59f57dc5b40 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -44,7 +44,7 @@ using namespace time_literals; AngularVelocityController::AngularVelocityController() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + WorkItem(MODULE_NAME, px4::wq_configurations::ctrl_alloc), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; From f117dbe8976314324bb3cafc22388a7436f97e47 Mon Sep 17 00:00:00 2001 From: Julien Date: Wed, 24 Jun 2020 10:01:31 +0200 Subject: [PATCH 104/129] Typhoon: increase inertia --- ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc index da9ec48a0008..37139b325278 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d-posix/6012_typhoon_ctrlalloc @@ -24,9 +24,9 @@ then param set MPC_USE_HTE 0 param set VM_MASS 2.66 - param set VM_INERTIA_XX 0.03 - param set VM_INERTIA_YY 0.03 - param set VM_INERTIA_ZZ 0.05 + param set VM_INERTIA_XX 0.06 + param set VM_INERTIA_YY 0.06 + param set VM_INERTIA_ZZ 0.10 param set CA_AIRFRAME 0 param set CA_METHOD 1 From b247368f8ecc3104855054a0be4649b689b2abb8 Mon Sep 17 00:00:00 2001 From: Julien Date: Wed, 24 Jun 2020 10:02:30 +0200 Subject: [PATCH 105/129] Add params to simulate actuator failure --- .../control_allocator/ControlAllocator.cpp | 59 ++++++ .../control_allocator/ControlAllocator.hpp | 18 +- .../control_allocator_params.c | 179 ++++++++++++++++++ 3 files changed, 255 insertions(+), 1 deletion(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index cb08774a5eb7..df2cecda2042 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -403,6 +403,34 @@ ControlAllocator::publish_actuator_setpoint() { matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); + // --- + // Simulate actuator failure if desired, TODO: remove this + matrix::Vector actuator_fail; + actuator_fail(0) = _param_ca_act0_fail.get(); + actuator_fail(1) = _param_ca_act1_fail.get(); + actuator_fail(2) = _param_ca_act2_fail.get(); + actuator_fail(3) = _param_ca_act3_fail.get(); + actuator_fail(4) = _param_ca_act4_fail.get(); + actuator_fail(5) = _param_ca_act5_fail.get(); + actuator_fail(6) = _param_ca_act6_fail.get(); + actuator_fail(7) = _param_ca_act7_fail.get(); + actuator_fail(8) = _param_ca_act8_fail.get(); + actuator_fail(9) = _param_ca_act9_fail.get(); + actuator_fail(10) = _param_ca_act10_fail.get(); + actuator_fail(11) = _param_ca_act11_fail.get(); + actuator_fail(12) = _param_ca_act12_fail.get(); + actuator_fail(13) = _param_ca_act13_fail.get(); + actuator_fail(14) = _param_ca_act14_fail.get(); + actuator_fail(15) = _param_ca_act15_fail.get(); + + for (size_t i = 0; i < NUM_ACTUATORS; i++) { + if (actuator_fail(i)) { + actuator_sp(i) = _control_allocation->getActuatorMin()(i); + } + } + + // --- + vehicle_actuator_setpoint_s vehicle_actuator_setpoint{}; vehicle_actuator_setpoint.timestamp = hrt_absolute_time(); vehicle_actuator_setpoint.timestamp_sample = _timestamp_sample; @@ -470,7 +498,38 @@ ControlAllocator::publish_legacy_actuator_controls() actuator_controls_4.timestamp_sample = _timestamp_sample; actuator_controls_5.timestamp_sample = _timestamp_sample; + // Get allocated actuator setpoints matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); + + // --- + // Simulate actuator failure if desired, TODO: remove this + matrix::Vector actuator_fail; + actuator_fail(0) = _param_ca_act0_fail.get(); + actuator_fail(1) = _param_ca_act1_fail.get(); + actuator_fail(2) = _param_ca_act2_fail.get(); + actuator_fail(3) = _param_ca_act3_fail.get(); + actuator_fail(4) = _param_ca_act4_fail.get(); + actuator_fail(5) = _param_ca_act5_fail.get(); + actuator_fail(6) = _param_ca_act6_fail.get(); + actuator_fail(7) = _param_ca_act7_fail.get(); + actuator_fail(8) = _param_ca_act8_fail.get(); + actuator_fail(9) = _param_ca_act9_fail.get(); + actuator_fail(10) = _param_ca_act10_fail.get(); + actuator_fail(11) = _param_ca_act11_fail.get(); + actuator_fail(12) = _param_ca_act12_fail.get(); + actuator_fail(13) = _param_ca_act13_fail.get(); + actuator_fail(14) = _param_ca_act14_fail.get(); + actuator_fail(15) = _param_ca_act15_fail.get(); + + for (size_t i = 0; i < NUM_ACTUATORS; i++) { + if (actuator_fail(i)) { + actuator_sp(i) = _control_allocation->getActuatorMin()(i); + } + } + + // --- + + // Normalize actuator setpoints matrix::Vector actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint( actuator_sp); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 4d5b1739a8a5..4d76cc42c7f8 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -195,7 +195,23 @@ class ControlAllocator : public ModuleBase, public ModuleParam (ParamFloat) _param_ca_act12_max, (ParamFloat) _param_ca_act13_max, (ParamFloat) _param_ca_act14_max, - (ParamFloat) _param_ca_act15_max + (ParamFloat) _param_ca_act15_max, + (ParamBool) _param_ca_act0_fail, + (ParamBool) _param_ca_act1_fail, + (ParamBool) _param_ca_act2_fail, + (ParamBool) _param_ca_act3_fail, + (ParamBool) _param_ca_act4_fail, + (ParamBool) _param_ca_act5_fail, + (ParamBool) _param_ca_act6_fail, + (ParamBool) _param_ca_act7_fail, + (ParamBool) _param_ca_act8_fail, + (ParamBool) _param_ca_act9_fail, + (ParamBool) _param_ca_act10_fail, + (ParamBool) _param_ca_act11_fail, + (ParamBool) _param_ca_act12_fail, + (ParamBool) _param_ca_act13_fail, + (ParamBool) _param_ca_act14_fail, + (ParamBool) _param_ca_act15_fail ) }; diff --git a/src/modules/control_allocator/control_allocator_params.c b/src/modules/control_allocator/control_allocator_params.c index a6eb83d91600..f9118e035e47 100644 --- a/src/modules/control_allocator/control_allocator_params.c +++ b/src/modules/control_allocator/control_allocator_params.c @@ -313,3 +313,182 @@ PARAM_DEFINE_FLOAT(CA_ACT14_MAX, 0.0); * @group Control Allocation */ PARAM_DEFINE_FLOAT(CA_ACT15_MAX, 0.0); + + + + +/** + * Failure simulation for actuator 0 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT0_FAIL, 0); + +/** + * Failure simulation for actuator 1 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT1_FAIL, 0); + +/** + * Failure simulation for actuator 2 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT2_FAIL, 0); + +/** + * Failure simulation for actuator 3 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT3_FAIL, 0); + +/** + * Failure simulation for actuator 4 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT4_FAIL, 0); + +/** + * Failure simulation for actuator 5 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT5_FAIL, 0); + +/** + * Failure simulation for actuator 6 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT6_FAIL, 0); + +/** + * Failure simulation for actuator 7 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT7_FAIL, 0); + +/** + * Failure simulation for actuator 8 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT8_FAIL, 0); + +/** + * Failure simulation for actuator 9 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT9_FAIL, 0); + +/** + * Failure simulation for actuator 10 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT10_FAIL, 0); + +/** + * Failure simulation for actuator 11 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT11_FAIL, 0); + +/** + * Failure simulation for actuator 12 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT12_FAIL, 0); + +/** + * Failure simulation for actuator 13 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT13_FAIL, 0); + +/** + * Failure simulation for actuator 14 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT14_FAIL, 0); + +/** + * Failure simulation for actuator 15 + * + * When true, sets the actuator to CA_ACTx_MIN + * + * @boolean + * + * @group Control Allocation + */ +PARAM_DEFINE_INT32(CA_ACT15_FAIL, 0); From 84c7fc1c1694c40bd6ea7586249a07e139681838 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sat, 11 Jul 2020 11:35:21 +0200 Subject: [PATCH 106/129] control_allocator: build with MAX_CUSTOM_OPT_LEVEL --- .../ActuatorEffectiveness/CMakeLists.txt | 7 ++----- src/modules/control_allocator/CMakeLists.txt | 1 + .../control_allocator/ControlAllocation/CMakeLists.txt | 8 ++------ 3 files changed, 5 insertions(+), 11 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index cb59819df6b6..9f55d32945dd 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -41,11 +41,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTiltrotorVTOL.hpp ) -target_include_directories(ActuatorEffectiveness - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} -) - +target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(ActuatorEffectiveness PRIVATE mathlib diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index 97318cedbb38..06a0ff017a3e 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/src/modules/control_allocator/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( MODULE modules__control_allocator MAIN control_allocator COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS ControlAllocator.cpp ControlAllocator.hpp diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt index b1fe1239e2df..ecf5f0d2e447 100644 --- a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt +++ b/src/modules/control_allocator/ControlAllocation/CMakeLists.txt @@ -39,12 +39,8 @@ px4_add_library(ControlAllocation ControlAllocationSequentialDesaturation.cpp ControlAllocationSequentialDesaturation.hpp ) - -target_include_directories(ControlAllocation - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} -) - +target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) From b5a8d8657c16a6dde4d2fa9c88434c1bc01cb898 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sat, 11 Jul 2020 11:35:37 +0200 Subject: [PATCH 107/129] angular_velocity_controller: build with MAX_CUSTOM_OPT_LEVEL --- .../AngularVelocityControl/CMakeLists.txt | 7 ++----- src/modules/angular_velocity_controller/CMakeLists.txt | 1 + 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt b/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt index bf129205eb67..05bb7dd7b25c 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt +++ b/src/modules/angular_velocity_controller/AngularVelocityControl/CMakeLists.txt @@ -34,11 +34,8 @@ px4_add_library(AngularVelocityControl AngularVelocityControl.cpp ) -target_include_directories(AngularVelocityControl - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} -) - +target_compile_options(AngularVelocityControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(AngularVelocityControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(AngularVelocityControl PRIVATE mathlib) px4_add_unit_gtest(SRC AngularVelocityControlTest.cpp LINKLIBS AngularVelocityControl) diff --git a/src/modules/angular_velocity_controller/CMakeLists.txt b/src/modules/angular_velocity_controller/CMakeLists.txt index d99caeb0cc4e..4cfc369db7ba 100644 --- a/src/modules/angular_velocity_controller/CMakeLists.txt +++ b/src/modules/angular_velocity_controller/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( MODULE modules__angular_velocity_control MAIN angular_velocity_controller COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS AngularVelocityController.cpp AngularVelocityController.hpp From 69b4a41c583dbb1fad28c2c26cef1550c649b561 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sat, 11 Jul 2020 12:26:53 +0200 Subject: [PATCH 108/129] Small fixes --- .../ActuatorEffectivenessMultirotorTest.cpp | 2 +- .../control_allocator/ControlAllocator.cpp | 17 ++++++++++++++--- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp index f78088ae767d..98865f65b919 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp @@ -75,7 +75,7 @@ TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase) geometry_quad_wide.rotors[2].thrust_coef = 1.0f; geometry_quad_wide.rotors[2].moment_ratio = -0.05f; - geometry_quad_wide.rotors[3].position_x = -0.1515f;` + geometry_quad_wide.rotors[3].position_x = -0.1515f; geometry_quad_wide.rotors[3].position_y = 0.1875f; geometry_quad_wide.rotors[3].position_z = 0.0f; geometry_quad_wide.rotors[3].axis_x = 0.0f; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index df2cecda2042..df08e936c665 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -59,8 +59,14 @@ ControlAllocator::ControlAllocator() : ControlAllocator::~ControlAllocator() { - free(_control_allocation); - free(_actuator_effectiveness); + if (_control_allocation != nullptr) { + free(_control_allocation); + } + + if (_actuator_effectiveness != nullptr) { + free(_actuator_effectiveness); + } + perf_free(_loop_perf); } @@ -229,13 +235,18 @@ ControlAllocator::update_effectiveness_source() case EffectivenessSource::TILTROTOR_VTOL: tmp = new ActuatorEffectivenessTiltrotorVTOL(); break; + + default: + PX4_ERR("Unknown airframe"); + tmp = nullptr; + break; } // Replace previous source with new one if (tmp == nullptr) { // It did not work, forget about it PX4_ERR("Actuator effectiveness init failed"); - _param_ca_method.set((int)_effectiveness_source_id); + _param_ca_airframe.set((int)_effectiveness_source_id); } else if (_actuator_effectiveness == tmp) { // Nothing has changed, this should not happen From 6a73f6f954a9b34f347ed1e82978730473916300 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sat, 11 Jul 2020 13:04:42 +0200 Subject: [PATCH 109/129] Fix tests --- .../ActuatorEffectivenessMultirotorTest.cpp | 84 +++++++++---------- 1 file changed, 42 insertions(+), 42 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp index 98865f65b919..71b615597b2a 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp @@ -47,54 +47,54 @@ using namespace matrix; TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase) { // Quad wide geometry - MultirotorGeometry geometry_quad_wide = {}; - geometry_quad_wide.rotors[0].position_x = 0.1515f; - geometry_quad_wide.rotors[0].position_y = 0.245f; - geometry_quad_wide.rotors[0].position_z = 0.0f; - geometry_quad_wide.rotors[0].axis_x = 0.0f; - geometry_quad_wide.rotors[0].axis_y = 0.0f; - geometry_quad_wide.rotors[0].axis_z = -1.0f; - geometry_quad_wide.rotors[0].thrust_coef = 1.0f; - geometry_quad_wide.rotors[0].moment_ratio = 0.05f; + ActuatorEffectivenessMultirotor::MultirotorGeometry geometry = {}; + geometry.rotors[0].position_x = 1.0f; + geometry.rotors[0].position_y = 1.0f; + geometry.rotors[0].position_z = 0.0f; + geometry.rotors[0].axis_x = 0.0f; + geometry.rotors[0].axis_y = 0.0f; + geometry.rotors[0].axis_z = -1.0f; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = 0.05f; - geometry_quad_wide.rotors[1].position_x = -0.1515f; - geometry_quad_wide.rotors[1].position_y = -0.1875f; - geometry_quad_wide.rotors[1].position_z = 0.0f; - geometry_quad_wide.rotors[1].axis_x = 0.0f; - geometry_quad_wide.rotors[1].axis_y = 0.0f; - geometry_quad_wide.rotors[1].axis_z = -1.0f; - geometry_quad_wide.rotors[1].thrust_coef = 1.0f; - geometry_quad_wide.rotors[1].moment_ratio = 0.05f; + geometry.rotors[1].position_x = -1.0f; + geometry.rotors[1].position_y = -1.0f; + geometry.rotors[1].position_z = 0.0f; + geometry.rotors[1].axis_x = 0.0f; + geometry.rotors[1].axis_y = 0.0f; + geometry.rotors[1].axis_z = -1.0f; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; - geometry_quad_wide.rotors[2].position_x = 0.1515f; - geometry_quad_wide.rotors[2].position_y = -0.245f; - geometry_quad_wide.rotors[2].position_z = 0.0f; - geometry_quad_wide.rotors[2].axis_x = 0.0f; - geometry_quad_wide.rotors[2].axis_y = 0.0f; - geometry_quad_wide.rotors[2].axis_z = -1.0f; - geometry_quad_wide.rotors[2].thrust_coef = 1.0f; - geometry_quad_wide.rotors[2].moment_ratio = -0.05f; + geometry.rotors[2].position_x = 1.0f; + geometry.rotors[2].position_y = -1.0f; + geometry.rotors[2].position_z = 0.0f; + geometry.rotors[2].axis_x = 0.0f; + geometry.rotors[2].axis_y = 0.0f; + geometry.rotors[2].axis_z = -1.0f; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; - geometry_quad_wide.rotors[3].position_x = -0.1515f; - geometry_quad_wide.rotors[3].position_y = 0.1875f; - geometry_quad_wide.rotors[3].position_z = 0.0f; - geometry_quad_wide.rotors[3].axis_x = 0.0f; - geometry_quad_wide.rotors[3].axis_y = 0.0f; - geometry_quad_wide.rotors[3].axis_z = -1.0f; - geometry_quad_wide.rotors[3].thrust_coef = 1.0f; - geometry_quad_wide.rotors[3].moment_ratio = -0.05f; + geometry.rotors[3].position_x = -1.0f; + geometry.rotors[3].position_y = 1.0f; + geometry.rotors[3].position_z = 0.0f; + geometry.rotors[3].axis_x = 0.0f; + geometry.rotors[3].axis_y = 0.0f; + geometry.rotors[3].axis_z = -1.0f; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = -0.05f; - effectiveness_quad_wide = ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry_quad_wide); + matrix::Matrix effectiveness = ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry); - const float quad_wide[NUM_AXES][NUM_ACTUATORS] = { - {-0.245f, 0.1875f, 0.245f, -0.1875f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.1515f, -0.1515f, 0.1515f, -0.1515f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = { + {-1.0f, 1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 1.0f, -1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.05f, 0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, - {-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; - effectiveness_quad_wide_expected = matrix::Matrix(quad_wide); + matrix::Matrix effectiveness_expected(expected); - EXPECT_EQ(effectiveness_quad_wide, effectiveness_quad_wide_expected); + EXPECT_EQ(effectiveness, effectiveness_expected); } From a8f35822aea1ec5b0d7db7a51a0b6c90f5ab6d4c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 9 Jul 2020 17:18:45 +0200 Subject: [PATCH 110/129] add quad and hexa CA airframe configs Signed-off-by: Silvan Fuhrer --- .../init.d/airframes/4017_s500_ctrlalloc | 57 ++++++++++++++ .../init.d/airframes/6002_hexa_x_ctrlalloc | 75 +++++++++++++++++++ 2 files changed, 132 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/4017_s500_ctrlalloc create mode 100644 ROMFS/px4fmu_common/init.d/airframes/6002_hexa_x_ctrlalloc diff --git a/ROMFS/px4fmu_common/init.d/airframes/4017_s500_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/4017_s500_ctrlalloc new file mode 100644 index 000000000000..0ce32d4489fc --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4017_s500_ctrlalloc @@ -0,0 +1,57 @@ +#!/bin/sh +# +# @name S500 with control allocation +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Silvan Fuhrer +# + +sh /etc/init.d/rc.mc_defaults +sh /etc/init.d/rc.ctrlalloc + +if [ $AUTOCNF = yes ] +then + param set MPC_USE_HTE 0 + + param set VM_MASS 1.5 + param set VM_INERTIA_XX 0.03 + param set VM_INERTIA_YY 0.03 + param set VM_INERTIA_ZZ 0.05 + + param set CA_AIRFRAME 0 + param set CA_METHOD 1 + param set CA_ACT0_MIN 0.0 + param set CA_ACT1_MIN 0.0 + param set CA_ACT2_MIN 0.0 + param set CA_ACT3_MIN 0.0 + param set CA_ACT0_MAX 1.0 + param set CA_ACT1_MAX 1.0 + param set CA_ACT2_MAX 1.0 + param set CA_ACT3_MAX 1.0 + + param set CA_MC_R0_PX 0.177 + param set CA_MC_R0_PY 0.177 + param set CA_MC_R0_CT 6.5 + param set CA_MC_R0_KM 0.05 + param set CA_MC_R1_PX -0.177 + param set CA_MC_R1_PY -0.177 + param set CA_MC_R1_CT 6.5 + param set CA_MC_R1_KM 0.05 + param set CA_MC_R2_PX 0.177 + param set CA_MC_R2_PY -0.177 + param set CA_MC_R2_CT 6.5 + param set CA_MC_R2_KM -0.05 + param set CA_MC_R3_PX -0.177 + param set CA_MC_R3_PY 0.177 + param set CA_MC_R3_CT 6.5 + param set CA_MC_R3_KM -0.05 + +fi + +set MIXER direct +set PWM_OUT 1234 + +set MIXER_AUX direct_aux +set PWM_AUX_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/6002_hexa_x_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/6002_hexa_x_ctrlalloc new file mode 100644 index 000000000000..b62c06f45dda --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/6002_hexa_x_ctrlalloc @@ -0,0 +1,75 @@ +#!/bin/sh +# +# @name Hex X with control allocation +# +# @type Hexarotor x +# @class Copter +# +# @maintainer Silvan Fuhrer +# + +sh /etc/init.d/rc.mc_defaults +sh /etc/init.d/rc.ctrlalloc + +if [ $AUTOCNF = yes ] +then + param set MPC_USE_HTE 0 + + param set VM_MASS 1.5 + param set VM_INERTIA_XX 0.03 + param set VM_INERTIA_YY 0.03 + param set VM_INERTIA_ZZ 0.05 + + param set CA_AIRFRAME 0 + param set CA_METHOD 1 + param set CA_ACT0_MIN 0.0 + param set CA_ACT1_MIN 0.0 + param set CA_ACT2_MIN 0.0 + param set CA_ACT3_MIN 0.0 + param set CA_ACT4_MIN 0.0 + param set CA_ACT5_MIN 0.0 + + param set CA_ACT0_MAX 1.0 + param set CA_ACT1_MAX 1.0 + param set CA_ACT2_MAX 1.0 + param set CA_ACT3_MAX 1.0 + param set CA_ACT4_MAX 1.0 + param set CA_ACT5_MAX 1.0 + + param set CA_MC_R0_PX 0.0 + param set CA_MC_R0_PY 0.275 + param set CA_MC_R0_CT 6.5 + param set CA_MC_R0_KM -0.05 + + param set CA_MC_R1_PX 0.0 + param set CA_MC_R1_PY -0.275 + param set CA_MC_R1_CT 6.5 + param set CA_MC_R1_KM 0.05 + + param set CA_MC_R2_PX 0.238 + param set CA_MC_R2_PY -0.1375 + param set CA_MC_R2_CT 6.5 + param set CA_MC_R2_KM -0.05 + + param set CA_MC_R3_PX -0.238 + param set CA_MC_R3_PY 0.1375 + param set CA_MC_R3_CT 6.5 + param set CA_MC_R3_KM 0.05 + + param set CA_MC_R4_PX 0.238 + param set CA_MC_R4_PY 0.1375 + param set CA_MC_R4_CT 6.5 + param set CA_MC_R4_KM 0.05 + + param set CA_MC_R5_PX -0.238 + param set CA_MC_R5_PY -0.1375 + param set CA_MC_R5_CT 6.5 + param set CA_MC_R5_KM -0.05 + +fi + +set MIXER direct +set PWM_OUT 123456 + +set MIXER_AUX direct_aux +set PWM_AUX_OUT 123456 From a0af613ed2cbf662f1b8b978bfdda28d5542b4f7 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 8 Jul 2020 18:09:12 +0200 Subject: [PATCH 111/129] CA: add control allocation topics Signed-off-by: Silvan Fuhrer --- src/modules/logger/logged_topics.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 9b624d26f077..e1693c5a121d 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -46,6 +46,8 @@ void LoggedTopics::add_default_topics() { add_topic("actuator_controls_0", 100); add_topic("actuator_controls_1", 100); + add_topic("actuator_controls_4", 100); + add_topic("actuator_controls_5", 100); add_topic("airspeed", 1000); add_topic("airspeed_validated", 1000); add_topic("camera_capture"); @@ -97,6 +99,13 @@ void LoggedTopics::add_default_topics() add_topic("vtol_vehicle_status", 200); add_topic("yaw_estimator_status", 200); + // Control allocaton topics + add_topic("vehicle_angular_acceleration_setpoint", 20); + add_topic("vehicle_angular_acceleration", 20); + add_topic("vehicle_thrust_setpoint", 20); + add_topic("vehicle_torque_setpoint", 20); + add_topic("vehicle_actuator_setpoint", 20); + // multi topics add_topic_multi("actuator_outputs", 100); add_topic_multi("logger_status"); From 74eb95f40a8b8623a074e4e53e8d53bd96bba7d5 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sat, 11 Jul 2020 13:50:33 +0200 Subject: [PATCH 112/129] Disable hover thrust estimator and prearming --- ROMFS/px4fmu_common/init.d/rc.ctrlalloc | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.ctrlalloc b/ROMFS/px4fmu_common/init.d/rc.ctrlalloc index 0a2bcbdf8947..e94b5fb16b59 100644 --- a/ROMFS/px4fmu_common/init.d/rc.ctrlalloc +++ b/ROMFS/px4fmu_common/init.d/rc.ctrlalloc @@ -15,3 +15,12 @@ mc_rate_control stop # Start Control Allocator # control_allocator start + +# +# Disable hover thrust estimator and prearming +# These features are currently incompatible with control allocation +# +# TODO: fix +# +param set MPC_USE_HTE 0 +param set COM_PREARM_MODE 0 From 75226d86a3fd14a1a18780370e56e8aa36ddc030 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sat, 11 Jul 2020 14:02:38 +0200 Subject: [PATCH 113/129] Update sitl_gazebo submodule --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 18e714c03eca..e1cc6b0f5187 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 18e714c03ecaba9f872772c0351778a16aba7a7b +Subproject commit e1cc6b0f51875b624eb08063b6d83d765d2ddea0 From c18b2055d54e4e22a3fe427f5a72a304705b1502 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sat, 11 Jul 2020 14:05:23 +0200 Subject: [PATCH 114/129] Disable ActuatorEffectiveness tests --- .../control_allocator/ActuatorEffectiveness/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 9f55d32945dd..50eec6edcf6c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -49,4 +49,4 @@ target_link_libraries(ActuatorEffectiveness ControlAllocation ) -px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness) +# px4_add_unit_gtest(SRC ActuatorEffectivenessMultirotorTest.cpp LINKLIBS ActuatorEffectiveness) From 8a1a20d78ce95f16ad6f8e290652276369f62b9a Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Sat, 11 Jul 2020 14:08:44 +0200 Subject: [PATCH 115/129] Format --- .../ActuatorEffectivenessMultirotorTest.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp index 71b615597b2a..69b60caf341f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotorTest.cpp @@ -84,7 +84,8 @@ TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase) geometry.rotors[3].thrust_coef = 1.0f; geometry.rotors[3].moment_ratio = -0.05f; - matrix::Matrix effectiveness = ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry); + matrix::Matrix effectiveness = + ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(geometry); const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = { {-1.0f, 1.0f, 1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, @@ -94,7 +95,8 @@ TEST(ActuatorEffectivenessMultirotorTest, AllZeroCase) { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, {-1.0f, -1.0f, -1.0f, -1.0f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; - matrix::Matrix effectiveness_expected(expected); + matrix::Matrix effectiveness_expected( + expected); EXPECT_EQ(effectiveness, effectiveness_expected); } From b9485953a4e5a1988883c7e0e22f74881a5d085a Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 16 Jul 2020 11:14:37 +0200 Subject: [PATCH 116/129] mixer module: handle all actuator_controls of 4 and 5 as rotors, e.g. don't output something while pre-armed Signed-off-by: Silvan Fuhrer --- src/lib/mixer_module/mixer_module.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 73804b9bfb39..620bbd1dd377 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -510,9 +510,11 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8 /* motor spinup phase - lock throttle to zero */ if (output->_output_limit.state == OUTPUT_LIMIT_STATE_RAMP) { - if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || - control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && - control_index == actuator_controls_s::INDEX_THROTTLE) { + if (((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || + control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && + control_index == actuator_controls_s::INDEX_THROTTLE) || + (control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 || + control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) { /* limit the throttle output to zero during motor spinup, * as the motors cannot follow any demand yet */ @@ -522,9 +524,11 @@ int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8 /* throttle not arming - mark throttle input as invalid */ if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) { - if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || - control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && - control_index == actuator_controls_s::INDEX_THROTTLE) { + if (((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || + control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && + control_index == actuator_controls_s::INDEX_THROTTLE) || + (control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART1 || + control_group == actuator_controls_s::GROUP_INDEX_ALLOCATED_PART2)) { /* set the throttle to an invalid value */ input = NAN; } From b8b168ab55a7bea8e1d0e54377188d3685d1108c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 17 Jul 2020 10:14:11 +0200 Subject: [PATCH 117/129] Revert "Add params to simulate actuator failure" This reverts commit b247368f8ecc3104855054a0be4649b689b2abb8. --- .../control_allocator/ControlAllocator.cpp | 59 ------ .../control_allocator/ControlAllocator.hpp | 18 +- .../control_allocator_params.c | 179 ------------------ 3 files changed, 1 insertion(+), 255 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index df08e936c665..83a623677c58 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -414,34 +414,6 @@ ControlAllocator::publish_actuator_setpoint() { matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); - // --- - // Simulate actuator failure if desired, TODO: remove this - matrix::Vector actuator_fail; - actuator_fail(0) = _param_ca_act0_fail.get(); - actuator_fail(1) = _param_ca_act1_fail.get(); - actuator_fail(2) = _param_ca_act2_fail.get(); - actuator_fail(3) = _param_ca_act3_fail.get(); - actuator_fail(4) = _param_ca_act4_fail.get(); - actuator_fail(5) = _param_ca_act5_fail.get(); - actuator_fail(6) = _param_ca_act6_fail.get(); - actuator_fail(7) = _param_ca_act7_fail.get(); - actuator_fail(8) = _param_ca_act8_fail.get(); - actuator_fail(9) = _param_ca_act9_fail.get(); - actuator_fail(10) = _param_ca_act10_fail.get(); - actuator_fail(11) = _param_ca_act11_fail.get(); - actuator_fail(12) = _param_ca_act12_fail.get(); - actuator_fail(13) = _param_ca_act13_fail.get(); - actuator_fail(14) = _param_ca_act14_fail.get(); - actuator_fail(15) = _param_ca_act15_fail.get(); - - for (size_t i = 0; i < NUM_ACTUATORS; i++) { - if (actuator_fail(i)) { - actuator_sp(i) = _control_allocation->getActuatorMin()(i); - } - } - - // --- - vehicle_actuator_setpoint_s vehicle_actuator_setpoint{}; vehicle_actuator_setpoint.timestamp = hrt_absolute_time(); vehicle_actuator_setpoint.timestamp_sample = _timestamp_sample; @@ -509,38 +481,7 @@ ControlAllocator::publish_legacy_actuator_controls() actuator_controls_4.timestamp_sample = _timestamp_sample; actuator_controls_5.timestamp_sample = _timestamp_sample; - // Get allocated actuator setpoints matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); - - // --- - // Simulate actuator failure if desired, TODO: remove this - matrix::Vector actuator_fail; - actuator_fail(0) = _param_ca_act0_fail.get(); - actuator_fail(1) = _param_ca_act1_fail.get(); - actuator_fail(2) = _param_ca_act2_fail.get(); - actuator_fail(3) = _param_ca_act3_fail.get(); - actuator_fail(4) = _param_ca_act4_fail.get(); - actuator_fail(5) = _param_ca_act5_fail.get(); - actuator_fail(6) = _param_ca_act6_fail.get(); - actuator_fail(7) = _param_ca_act7_fail.get(); - actuator_fail(8) = _param_ca_act8_fail.get(); - actuator_fail(9) = _param_ca_act9_fail.get(); - actuator_fail(10) = _param_ca_act10_fail.get(); - actuator_fail(11) = _param_ca_act11_fail.get(); - actuator_fail(12) = _param_ca_act12_fail.get(); - actuator_fail(13) = _param_ca_act13_fail.get(); - actuator_fail(14) = _param_ca_act14_fail.get(); - actuator_fail(15) = _param_ca_act15_fail.get(); - - for (size_t i = 0; i < NUM_ACTUATORS; i++) { - if (actuator_fail(i)) { - actuator_sp(i) = _control_allocation->getActuatorMin()(i); - } - } - - // --- - - // Normalize actuator setpoints matrix::Vector actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint( actuator_sp); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 4d76cc42c7f8..4d5b1739a8a5 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -195,23 +195,7 @@ class ControlAllocator : public ModuleBase, public ModuleParam (ParamFloat) _param_ca_act12_max, (ParamFloat) _param_ca_act13_max, (ParamFloat) _param_ca_act14_max, - (ParamFloat) _param_ca_act15_max, - (ParamBool) _param_ca_act0_fail, - (ParamBool) _param_ca_act1_fail, - (ParamBool) _param_ca_act2_fail, - (ParamBool) _param_ca_act3_fail, - (ParamBool) _param_ca_act4_fail, - (ParamBool) _param_ca_act5_fail, - (ParamBool) _param_ca_act6_fail, - (ParamBool) _param_ca_act7_fail, - (ParamBool) _param_ca_act8_fail, - (ParamBool) _param_ca_act9_fail, - (ParamBool) _param_ca_act10_fail, - (ParamBool) _param_ca_act11_fail, - (ParamBool) _param_ca_act12_fail, - (ParamBool) _param_ca_act13_fail, - (ParamBool) _param_ca_act14_fail, - (ParamBool) _param_ca_act15_fail + (ParamFloat) _param_ca_act15_max ) }; diff --git a/src/modules/control_allocator/control_allocator_params.c b/src/modules/control_allocator/control_allocator_params.c index f9118e035e47..a6eb83d91600 100644 --- a/src/modules/control_allocator/control_allocator_params.c +++ b/src/modules/control_allocator/control_allocator_params.c @@ -313,182 +313,3 @@ PARAM_DEFINE_FLOAT(CA_ACT14_MAX, 0.0); * @group Control Allocation */ PARAM_DEFINE_FLOAT(CA_ACT15_MAX, 0.0); - - - - -/** - * Failure simulation for actuator 0 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT0_FAIL, 0); - -/** - * Failure simulation for actuator 1 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT1_FAIL, 0); - -/** - * Failure simulation for actuator 2 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT2_FAIL, 0); - -/** - * Failure simulation for actuator 3 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT3_FAIL, 0); - -/** - * Failure simulation for actuator 4 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT4_FAIL, 0); - -/** - * Failure simulation for actuator 5 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT5_FAIL, 0); - -/** - * Failure simulation for actuator 6 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT6_FAIL, 0); - -/** - * Failure simulation for actuator 7 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT7_FAIL, 0); - -/** - * Failure simulation for actuator 8 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT8_FAIL, 0); - -/** - * Failure simulation for actuator 9 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT9_FAIL, 0); - -/** - * Failure simulation for actuator 10 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT10_FAIL, 0); - -/** - * Failure simulation for actuator 11 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT11_FAIL, 0); - -/** - * Failure simulation for actuator 12 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT12_FAIL, 0); - -/** - * Failure simulation for actuator 13 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT13_FAIL, 0); - -/** - * Failure simulation for actuator 14 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT14_FAIL, 0); - -/** - * Failure simulation for actuator 15 - * - * When true, sets the actuator to CA_ACTx_MIN - * - * @boolean - * - * @group Control Allocation - */ -PARAM_DEFINE_INT32(CA_ACT15_FAIL, 0); From 66b066d57d8680e8e530c3d4b610c6f2326174c9 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 17 Jul 2020 10:31:46 +0200 Subject: [PATCH 118/129] CA_AIRFRAME: clean up param description Signed-off-by: Silvan Fuhrer --- src/modules/control_allocator/control_allocator_params.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/control_allocator/control_allocator_params.c b/src/modules/control_allocator/control_allocator_params.c index a6eb83d91600..c1384d4414b7 100644 --- a/src/modules/control_allocator/control_allocator_params.c +++ b/src/modules/control_allocator/control_allocator_params.c @@ -47,10 +47,9 @@ * * @min 0 * @max 2 - * @value 0 multirotor - * @value 1 quad w - * @value 2 standard vtol - * @value 3 tiltrotor vtol + * @value 0 Multirotor + * @value 1 Standard VTOL (WIP) + * @value 2 Tiltrotor VTOL (WIP) * @group Control Allocation */ PARAM_DEFINE_INT32(CA_AIRFRAME, 0); From ba29c289109c33d9681119fd991a8d4df1ff977a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:23 +0200 Subject: [PATCH 119/129] fix control_allocator_status: torque_setpoint_achieved and thrust_setpoint_achieved are bool --- msg/control_allocator_status.msg | 4 +-- .../AngularVelocityController.cpp | 26 +++++++++---------- 2 files changed, 14 insertions(+), 16 deletions(-) diff --git a/msg/control_allocator_status.msg b/msg/control_allocator_status.msg index 25ae4600f201..b26d2d36b12c 100644 --- a/msg/control_allocator_status.msg +++ b/msg/control_allocator_status.msg @@ -1,11 +1,11 @@ uint64 timestamp # time since system start (microseconds) -uint8 torque_setpoint_achieved # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. +bool torque_setpoint_achieved # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. float32[3] allocated_torque # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved. float32[3] unallocated_torque # Unallocated torque. Equal to 0 if the setpoint was achieved. # Computed as: unallocated_torque = torque_setpoint - allocated_torque -uint8 thrust_setpoint_achieved # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. +bool thrust_setpoint_achieved # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. float32[3] allocated_thrust # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved. float32[3] unallocated_thrust # Unallocated thrust. Equal to 0 if the setpoint was achieved. # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index c59f57dc5b40..1d8dcba7c947 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -185,26 +185,24 @@ AngularVelocityController::Run() } // update saturation status from mixer feedback - if (_control_allocator_status_sub.updated()) { - control_allocator_status_s control_allocator_status; + control_allocator_status_s control_allocator_status; - if (_control_allocator_status_sub.copy(&control_allocator_status)) { - Vector saturation_positive; - Vector saturation_negative; + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; - if (not control_allocator_status.torque_setpoint_achieved) { - for (size_t i = 0; i < 3; i++) { - if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { - saturation_positive(i) = true; + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; - } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { - saturation_negative(i) = true; - } + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; } } - - _control.setSaturationStatus(saturation_positive, saturation_negative); } + + _control.setSaturationStatus(saturation_positive, saturation_negative); } // run rate controller From 354126ffbccbfe4f39073c12e1653eb4d352889b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:30 +0200 Subject: [PATCH 120/129] control_allocator: remove unused _task_start --- src/modules/control_allocator/ControlAllocator.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 4d5b1739a8a5..13a5894a363b 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -155,7 +155,6 @@ class ControlAllocator : public ModuleBase, public ModuleParam perf_counter_t _loop_perf; /**< loop duration performance counter */ - hrt_abstime _task_start{hrt_absolute_time()}; hrt_abstime _last_run{0}; hrt_abstime _timestamp_sample{0}; From 703fb00dd2b7803b47bb2bb482caa698c4c5a8d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:33 +0200 Subject: [PATCH 121/129] fix control_allocator: use 'delete' instead of 'free', guard against out-of-memory --- .../control_allocator/ControlAllocator.cpp | 65 +++++-------------- 1 file changed, 15 insertions(+), 50 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 83a623677c58..49fb75f372c4 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -59,13 +59,8 @@ ControlAllocator::ControlAllocator() : ControlAllocator::~ControlAllocator() { - if (_control_allocation != nullptr) { - free(_control_allocation); - } - - if (_actuator_effectiveness != nullptr) { - free(_actuator_effectiveness); - } + delete _control_allocation; + delete _actuator_effectiveness; perf_free(_loop_perf); } @@ -94,6 +89,10 @@ ControlAllocator::parameters_updated() update_effectiveness_source(); update_allocation_method(); + if (_control_allocation == nullptr) { + return; + } + // Minimum actuator values matrix::Vector actuator_min; actuator_min(0) = _param_ca_act0_min.get(); @@ -170,7 +169,6 @@ ControlAllocator::update_allocation_method() default: PX4_ERR("Unknown allocation method"); - tmp = nullptr; break; } @@ -180,20 +178,9 @@ ControlAllocator::update_allocation_method() PX4_ERR("Control allocation init failed"); _param_ca_method.set((int)_allocation_method_id); - } else if (_control_allocation == tmp) { - // Nothing has changed, this should not happen - PX4_ERR("Control allocation init error"); - _allocation_method_id = method; - } else { - // Successful update - PX4_INFO("Control allocation init success"); - // Swap allocation methods - if (_control_allocation != nullptr) { - free(_control_allocation); - } - + delete _control_allocation; _control_allocation = tmp; // Save method id @@ -203,13 +190,6 @@ ControlAllocator::update_allocation_method() _control_allocation->setActuatorSetpoint(actuator_sp); } } - - // Guard against bad initialization - if (_control_allocation == nullptr) { - PX4_ERR("Falling back to ControlAllocationPseudoInverse"); - _control_allocation = new ControlAllocationPseudoInverse(); - _allocation_method_id = AllocationMethod::PSEUDO_INVERSE; - } } void @@ -238,7 +218,6 @@ ControlAllocator::update_effectiveness_source() default: PX4_ERR("Unknown airframe"); - tmp = nullptr; break; } @@ -248,33 +227,15 @@ ControlAllocator::update_effectiveness_source() PX4_ERR("Actuator effectiveness init failed"); _param_ca_airframe.set((int)_effectiveness_source_id); - } else if (_actuator_effectiveness == tmp) { - // Nothing has changed, this should not happen - PX4_ERR("Actuator effectiveness init error"); - _effectiveness_source_id = source; - } else { - // Successful update - PX4_INFO("Actuator effectiveness init success"); - // Swap effectiveness sources - if (_actuator_effectiveness != nullptr) { - free(_actuator_effectiveness); - } - + delete _actuator_effectiveness; _actuator_effectiveness = tmp; // Save source id _effectiveness_source_id = source; } } - - // Guard against bad initialization - if (_actuator_effectiveness == nullptr) { - PX4_ERR("Falling back to ActuatorEffectivenessMultirotor"); - _actuator_effectiveness = new ActuatorEffectivenessMultirotor(); - _effectiveness_source_id = EffectivenessSource::MULTIROTOR; - } } void @@ -299,9 +260,13 @@ ControlAllocator::Run() parameters_updated(); } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status = {}; - _vehicle_status_sub.update(&vehicle_status); + if (_control_allocation == nullptr || _actuator_effectiveness == nullptr) { + return; + } + + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT}; From 54dfdb0c1a6d1424d44384558594500eadfc192a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:35 +0200 Subject: [PATCH 122/129] fix control_allocator: set _last_run at the correct place --- src/modules/control_allocator/ControlAllocator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 49fb75f372c4..192ab56fa14a 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -295,7 +295,6 @@ ControlAllocator::Run() // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const hrt_abstime now = hrt_absolute_time(); const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); - _last_run = now; bool do_update = false; vehicle_torque_setpoint_s vehicle_torque_setpoint; @@ -322,6 +321,7 @@ ControlAllocator::Run() } if (do_update) { + _last_run = now; // Update effectiveness matrix if needed if (_actuator_effectiveness->update()) { From de10a3c4cb54775c912733e7e9ece212cd932651 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:38 +0200 Subject: [PATCH 123/129] refactor control_allocator: directly get the effectiveness matrix when updated Reduces stack + RAM usage --- .../ActuatorEffectiveness.hpp | 27 +-- .../ActuatorEffectivenessMultirotor.cpp | 181 ++++++++---------- .../ActuatorEffectivenessMultirotor.hpp | 18 +- .../ActuatorEffectivenessStandardVTOL.cpp | 34 ++-- .../ActuatorEffectivenessStandardVTOL.hpp | 9 +- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 37 ++-- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 9 +- .../control_allocator/ControlAllocator.cpp | 57 +++--- .../control_allocator/ControlAllocator.hpp | 2 + 9 files changed, 165 insertions(+), 209 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 174a9069b338..f0ac4bcb9299 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -62,13 +62,6 @@ class ActuatorEffectiveness TRANSITION_FF_TO_HF = 3 }; - /** - * Update effectiveness matrix - * - * @return True if the effectiveness matrix has changed - */ - virtual bool update() = 0; - /** * Set the current flight phase * @@ -77,17 +70,14 @@ class ActuatorEffectiveness virtual void setFlightPhase(const FlightPhase &flight_phase) { _flight_phase = flight_phase; - }; + } /** - * Get the control effectiveness matrix + * Get the control effectiveness matrix if updated * - * @return Effectiveness matrix + * @return true if updated and matrix is set */ - const matrix::Matrix &getEffectivenessMatrix() const - { - return _effectiveness; - }; + virtual bool getEffectivenessMatrix(matrix::Matrix &matrix) = 0; /** * Get the actuator trims @@ -97,7 +87,7 @@ class ActuatorEffectiveness const matrix::Vector &getActuatorTrim() const { return _trim; - }; + } /** * Get the current flight phase @@ -107,10 +97,9 @@ class ActuatorEffectiveness const FlightPhase &getFlightPhase() const { return _flight_phase; - }; + } protected: - matrix::Matrix _effectiveness; //< Effectiveness matrix - matrix::Vector _trim; //< Actuator trim - FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; //< Current flight phase + matrix::Vector _trim; ///< Actuator trim + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index a486422193cb..a30ac2fde69b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -44,35 +44,105 @@ ActuatorEffectivenessMultirotor::ActuatorEffectivenessMultirotor(): ModuleParams(nullptr) { - parameters_updated(); } bool -ActuatorEffectivenessMultirotor::update() +ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix &matrix) { - bool updated = false; - // Check if parameters have changed - if (_parameter_update_sub.updated()) { + if (_updated || _parameter_update_sub.updated()) { // clear update parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); updateParams(); - parameters_updated(); - - updated = true; + _updated = false; + + // Get multirotor geometry + MultirotorGeometry geometry = {}; + geometry.rotors[0].position_x = _param_ca_mc_r0_px.get(); + geometry.rotors[0].position_y = _param_ca_mc_r0_py.get(); + geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get(); + geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get(); + geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get(); + geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get(); + geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get(); + geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get(); + + geometry.rotors[1].position_x = _param_ca_mc_r1_px.get(); + geometry.rotors[1].position_y = _param_ca_mc_r1_py.get(); + geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get(); + geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get(); + geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get(); + geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get(); + geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get(); + geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get(); + + geometry.rotors[2].position_x = _param_ca_mc_r2_px.get(); + geometry.rotors[2].position_y = _param_ca_mc_r2_py.get(); + geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get(); + geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get(); + geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get(); + geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get(); + geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get(); + geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get(); + + geometry.rotors[3].position_x = _param_ca_mc_r3_px.get(); + geometry.rotors[3].position_y = _param_ca_mc_r3_py.get(); + geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get(); + geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get(); + geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get(); + geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get(); + geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get(); + geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get(); + + geometry.rotors[4].position_x = _param_ca_mc_r4_px.get(); + geometry.rotors[4].position_y = _param_ca_mc_r4_py.get(); + geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get(); + geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get(); + geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get(); + geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get(); + geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get(); + geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get(); + + geometry.rotors[5].position_x = _param_ca_mc_r5_px.get(); + geometry.rotors[5].position_y = _param_ca_mc_r5_py.get(); + geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get(); + geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get(); + geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get(); + geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get(); + geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get(); + geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get(); + + geometry.rotors[6].position_x = _param_ca_mc_r6_px.get(); + geometry.rotors[6].position_y = _param_ca_mc_r6_py.get(); + geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get(); + geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get(); + geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get(); + geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get(); + geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get(); + geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get(); + + geometry.rotors[7].position_x = _param_ca_mc_r7_px.get(); + geometry.rotors[7].position_y = _param_ca_mc_r7_py.get(); + geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get(); + geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get(); + geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get(); + geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get(); + geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get(); + geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get(); + + computeEffectivenessMatrix(geometry, matrix); + return true; } - return updated; + return false; } -matrix::Matrix -ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry geometry) +void +ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeometry &geometry, + matrix::Matrix &effectiveness) { - matrix::Matrix - effectiveness; - for (size_t i = 0; i < NUM_ROTORS_MAX; i++) { // Get rotor axis matrix::Vector3f axis( @@ -115,87 +185,4 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(MultirotorGeometry g effectiveness(j + 3, i) = thrust(j); } } - - return effectiveness; -} - -void -ActuatorEffectivenessMultirotor::parameters_updated() -{ - // Get multirotor geometry - MultirotorGeometry geometry = {}; - geometry.rotors[0].position_x = _param_ca_mc_r0_px.get(); - geometry.rotors[0].position_y = _param_ca_mc_r0_py.get(); - geometry.rotors[0].position_z = _param_ca_mc_r0_pz.get(); - geometry.rotors[0].axis_x = _param_ca_mc_r0_ax.get(); - geometry.rotors[0].axis_y = _param_ca_mc_r0_ay.get(); - geometry.rotors[0].axis_z = _param_ca_mc_r0_az.get(); - geometry.rotors[0].thrust_coef = _param_ca_mc_r0_ct.get(); - geometry.rotors[0].moment_ratio = _param_ca_mc_r0_km.get(); - - geometry.rotors[1].position_x = _param_ca_mc_r1_px.get(); - geometry.rotors[1].position_y = _param_ca_mc_r1_py.get(); - geometry.rotors[1].position_z = _param_ca_mc_r1_pz.get(); - geometry.rotors[1].axis_x = _param_ca_mc_r1_ax.get(); - geometry.rotors[1].axis_y = _param_ca_mc_r1_ay.get(); - geometry.rotors[1].axis_z = _param_ca_mc_r1_az.get(); - geometry.rotors[1].thrust_coef = _param_ca_mc_r1_ct.get(); - geometry.rotors[1].moment_ratio = _param_ca_mc_r1_km.get(); - - geometry.rotors[2].position_x = _param_ca_mc_r2_px.get(); - geometry.rotors[2].position_y = _param_ca_mc_r2_py.get(); - geometry.rotors[2].position_z = _param_ca_mc_r2_pz.get(); - geometry.rotors[2].axis_x = _param_ca_mc_r2_ax.get(); - geometry.rotors[2].axis_y = _param_ca_mc_r2_ay.get(); - geometry.rotors[2].axis_z = _param_ca_mc_r2_az.get(); - geometry.rotors[2].thrust_coef = _param_ca_mc_r2_ct.get(); - geometry.rotors[2].moment_ratio = _param_ca_mc_r2_km.get(); - - geometry.rotors[3].position_x = _param_ca_mc_r3_px.get(); - geometry.rotors[3].position_y = _param_ca_mc_r3_py.get(); - geometry.rotors[3].position_z = _param_ca_mc_r3_pz.get(); - geometry.rotors[3].axis_x = _param_ca_mc_r3_ax.get(); - geometry.rotors[3].axis_y = _param_ca_mc_r3_ay.get(); - geometry.rotors[3].axis_z = _param_ca_mc_r3_az.get(); - geometry.rotors[3].thrust_coef = _param_ca_mc_r3_ct.get(); - geometry.rotors[3].moment_ratio = _param_ca_mc_r3_km.get(); - - geometry.rotors[4].position_x = _param_ca_mc_r4_px.get(); - geometry.rotors[4].position_y = _param_ca_mc_r4_py.get(); - geometry.rotors[4].position_z = _param_ca_mc_r4_pz.get(); - geometry.rotors[4].axis_x = _param_ca_mc_r4_ax.get(); - geometry.rotors[4].axis_y = _param_ca_mc_r4_ay.get(); - geometry.rotors[4].axis_z = _param_ca_mc_r4_az.get(); - geometry.rotors[4].thrust_coef = _param_ca_mc_r4_ct.get(); - geometry.rotors[4].moment_ratio = _param_ca_mc_r4_km.get(); - - geometry.rotors[5].position_x = _param_ca_mc_r5_px.get(); - geometry.rotors[5].position_y = _param_ca_mc_r5_py.get(); - geometry.rotors[5].position_z = _param_ca_mc_r5_pz.get(); - geometry.rotors[5].axis_x = _param_ca_mc_r5_ax.get(); - geometry.rotors[5].axis_y = _param_ca_mc_r5_ay.get(); - geometry.rotors[5].axis_z = _param_ca_mc_r5_az.get(); - geometry.rotors[5].thrust_coef = _param_ca_mc_r5_ct.get(); - geometry.rotors[5].moment_ratio = _param_ca_mc_r5_km.get(); - - geometry.rotors[6].position_x = _param_ca_mc_r6_px.get(); - geometry.rotors[6].position_y = _param_ca_mc_r6_py.get(); - geometry.rotors[6].position_z = _param_ca_mc_r6_pz.get(); - geometry.rotors[6].axis_x = _param_ca_mc_r6_ax.get(); - geometry.rotors[6].axis_y = _param_ca_mc_r6_ay.get(); - geometry.rotors[6].axis_z = _param_ca_mc_r6_az.get(); - geometry.rotors[6].thrust_coef = _param_ca_mc_r6_ct.get(); - geometry.rotors[6].moment_ratio = _param_ca_mc_r6_km.get(); - - geometry.rotors[7].position_x = _param_ca_mc_r7_px.get(); - geometry.rotors[7].position_y = _param_ca_mc_r7_py.get(); - geometry.rotors[7].position_z = _param_ca_mc_r7_pz.get(); - geometry.rotors[7].axis_x = _param_ca_mc_r7_ax.get(); - geometry.rotors[7].axis_y = _param_ca_mc_r7_ay.get(); - geometry.rotors[7].axis_z = _param_ca_mc_r7_az.get(); - geometry.rotors[7].thrust_coef = _param_ca_mc_r7_ct.get(); - geometry.rotors[7].moment_ratio = _param_ca_mc_r7_km.get(); - - // Compute effectiveness matrix - _effectiveness = computeEffectivenessMatrix(geometry); } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index 1b07a465864c..c5be27fc9115 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -53,13 +53,6 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec ActuatorEffectivenessMultirotor(); virtual ~ActuatorEffectivenessMultirotor() = default; - /** - * Update effectiveness matrix - * - * @return True if the effectiveness matrix has changed - */ - virtual bool update() override; - static constexpr int NUM_ROTORS_MAX = 8; typedef struct { @@ -77,16 +70,17 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec RotorGeometry rotors[NUM_ROTORS_MAX]; } MultirotorGeometry; - static matrix::Matrix computeEffectivenessMatrix(MultirotorGeometry); + static void computeEffectivenessMatrix(const MultirotorGeometry &geometry, + matrix::Matrix &effectiveness); + + bool getEffectivenessMatrix(matrix::Matrix &matrix) override; private: - /** - * initialize some vectors/matrices from parameters - */ - void parameters_updated(); uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + bool _updated{true}; + DEFINE_PARAMETERS( (ParamFloat) _param_ca_mc_r0_px, (ParamFloat) _param_ca_mc_r0_py, diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 8acde43c9d35..c5b2b8bb7574 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -47,23 +47,12 @@ ActuatorEffectivenessStandardVTOL::ActuatorEffectivenessStandardVTOL() } bool -ActuatorEffectivenessStandardVTOL::update() +ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(matrix::Matrix &matrix) { - if (_updated) { - _updated = false; - return true; + if (!_updated) { + return false; } - return false; -} - -void -ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase) -{ - ActuatorEffectiveness::setFlightPhase(flight_phase); - - _updated = true; - switch (_flight_phase) { case FlightPhase::HOVER_FLIGHT: { const float standard_vtol[NUM_AXES][NUM_ACTUATORS] = { @@ -74,7 +63,7 @@ ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phas { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; - _effectiveness = matrix::Matrix(standard_vtol); + matrix = matrix::Matrix(standard_vtol); break; } @@ -87,7 +76,7 @@ ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phas { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; - _effectiveness = matrix::Matrix(standard_vtol); + matrix = matrix::Matrix(standard_vtol); break; } @@ -101,8 +90,19 @@ ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phas { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; - _effectiveness = matrix::Matrix(standard_vtol); + matrix = matrix::Matrix(standard_vtol); break; } } + + _updated = false; + return true; +} + +void +ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + ActuatorEffectiveness::setFlightPhase(flight_phase); + _updated = true; + } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index dd36b5448164..01cff0a7644d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -49,12 +49,7 @@ class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness ActuatorEffectivenessStandardVTOL(); virtual ~ActuatorEffectivenessStandardVTOL() = default; - /** - * Update effectiveness matrix - * - * @return True if the effectiveness matrix has changed - */ - virtual bool update() override; + bool getEffectivenessMatrix(matrix::Matrix &matrix) override; /** * Set the current flight phase @@ -64,5 +59,5 @@ class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness void setFlightPhase(const FlightPhase &flight_phase) override; protected: - bool _updated{false}; + bool _updated{true}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 1b69a8789d60..c2001386a9a4 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -45,25 +45,13 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL() { setFlightPhase(FlightPhase::HOVER_FLIGHT); } - bool -ActuatorEffectivenessTiltrotorVTOL::update() +ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(matrix::Matrix &matrix) { - if (_updated) { - _updated = false; - return true; + if (!_updated) { + return false; } - return false; -} - -void -ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) -{ - ActuatorEffectiveness::setFlightPhase(flight_phase); - - _updated = true; - // Trim float tilt = 0.0f; @@ -104,13 +92,24 @@ ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_pha { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, {-0.25f * cosf(_trim(4)), -0.25f * cosf(_trim(5)), -0.25f * cosf(_trim(6)), -0.25f * cosf(_trim(7)), 0.25f * _trim(0) *sinf(_trim(4)), 0.25f * _trim(1) *sinf(_trim(5)), 0.25f * _trim(2) *sinf(_trim(6)), 0.25f * _trim(3) *sinf(_trim(7)), 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} }; - _effectiveness = matrix::Matrix(tiltrotor_vtol); + matrix = matrix::Matrix(tiltrotor_vtol); // Temporarily disable a few controls (WIP) for (size_t j = 4; j < 8; j++) { - _effectiveness(3, j) = 0.0f; - _effectiveness(4, j) = 0.0f; - _effectiveness(5, j) = 0.0f; + matrix(3, j) = 0.0f; + matrix(4, j) = 0.0f; + matrix(5, j) = 0.0f; } + + _updated = false; + return true; +} + +void +ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) +{ + ActuatorEffectiveness::setFlightPhase(flight_phase); + + _updated = true; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index f2a983270363..d2f0633341f9 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -49,12 +49,7 @@ class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness ActuatorEffectivenessTiltrotorVTOL(); virtual ~ActuatorEffectivenessTiltrotorVTOL() = default; - /** - * Update effectiveness matrix - * - * @return True if the effectiveness matrix has changed - */ - virtual bool update() override; + bool getEffectivenessMatrix(matrix::Matrix &matrix) override; /** * Set the current flight phase @@ -64,5 +59,5 @@ class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness void setFlightPhase(const FlightPhase &flight_phase) override; protected: - bool _updated{false}; + bool _updated{true}; }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 192ab56fa14a..eaa26470698b 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -132,13 +132,6 @@ ControlAllocator::parameters_updated() actuator_max(14) = _param_ca_act14_max.get(); actuator_max(15) = _param_ca_act15_max.get(); _control_allocation->setActuatorMax(actuator_max); - - // Get actuator effectiveness and trim - matrix::Matrix effectiveness = _actuator_effectiveness->getEffectivenessMatrix(); - matrix::Vector trim = _actuator_effectiveness->getActuatorTrim(); - - // Assign control effectiveness matrix - _control_allocation->setEffectivenessMatrix(effectiveness, trim); } void @@ -323,30 +316,7 @@ ControlAllocator::Run() if (do_update) { _last_run = now; - // Update effectiveness matrix if needed - if (_actuator_effectiveness->update()) { - matrix::Matrix effectiveness = _actuator_effectiveness->getEffectivenessMatrix(); - matrix::Vector trim = _actuator_effectiveness->getActuatorTrim(); - - // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) - matrix::Vector actuator_max = _control_allocation->getActuatorMax(); - matrix::Vector actuator_min = _control_allocation->getActuatorMin(); - - for (size_t j = 0; j < NUM_ACTUATORS; j++) { - if (actuator_min(j) >= actuator_max(j)) { - for (size_t i = 0; i < NUM_AXES; i++) { - effectiveness(i, j) = 0.0f; - } - - } - } - - // Assign control effectiveness matrix - if ((effectiveness - _control_allocation->getEffectivenessMatrix()).abs().max() > 1e-5f) { - _control_allocation->setEffectivenessMatrix(effectiveness, trim); - } - - } + update_effectiveness_matrix_if_needed(); // Set control setpoint vector matrix::Vector c; @@ -374,6 +344,31 @@ ControlAllocator::Run() perf_end(_loop_perf); } +void +ControlAllocator::update_effectiveness_matrix_if_needed() +{ + matrix::Matrix effectiveness; + + if (_actuator_effectiveness->getEffectivenessMatrix(effectiveness)) { + const matrix::Vector &trim = _actuator_effectiveness->getActuatorTrim(); + + // Set 0 effectiveness for actuators that are disabled (act_min >= act_max) + matrix::Vector actuator_max = _control_allocation->getActuatorMax(); + matrix::Vector actuator_min = _control_allocation->getActuatorMin(); + + for (size_t j = 0; j < NUM_ACTUATORS; j++) { + if (actuator_min(j) >= actuator_max(j)) { + for (size_t i = 0; i < NUM_AXES; i++) { + effectiveness(i, j) = 0.0f; + } + } + } + + // Assign control effectiveness matrix + _control_allocation->setEffectivenessMatrix(effectiveness, trim, _actuator_effectiveness->numActuators()); + } +} + void ControlAllocator::publish_actuator_setpoint() { diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 13a5894a363b..9a1045401200 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -105,6 +105,8 @@ class ControlAllocator : public ModuleBase, public ModuleParam void update_allocation_method(); void update_effectiveness_source(); + void update_effectiveness_matrix_if_needed(); + void publish_actuator_setpoint(); void publish_control_allocator_status(); From 8c4cfe63f06d4f792f0041a000f6031c011f4e13 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:47 +0200 Subject: [PATCH 124/129] control_allocator: change SequentialDesaturation to existing MC mixer And limit the operations to the number of configured outputs. Only using the number of configured actuators reduces CPU load by ~2% on F7 @1khz. --- .../ActuatorEffectiveness.hpp | 5 + .../ActuatorEffectivenessMultirotor.cpp | 14 +- .../ActuatorEffectivenessMultirotor.hpp | 6 +- .../ActuatorEffectivenessStandardVTOL.hpp | 1 + .../ActuatorEffectivenessTiltrotorVTOL.hpp | 1 + .../ControlAllocation/ControlAllocation.cpp | 31 ++-- .../ControlAllocation/ControlAllocation.hpp | 11 +- .../ControlAllocationPseudoInverse.cpp | 6 +- .../ControlAllocationPseudoInverse.hpp | 2 +- ...ontrolAllocationSequentialDesaturation.cpp | 166 +++++++++++++++--- ...ontrolAllocationSequentialDesaturation.hpp | 75 ++++++-- .../control_allocator/ControlAllocator.cpp | 5 + 12 files changed, 250 insertions(+), 73 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index f0ac4bcb9299..bd6ea7af11ee 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -99,6 +99,11 @@ class ActuatorEffectiveness return _flight_phase; } + /** + * Get the number of actuators + */ + virtual int numActuators() const = 0; + protected: matrix::Vector _trim; ///< Actuator trim FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index a30ac2fde69b..e2e7d2e88376 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -132,17 +132,19 @@ ActuatorEffectivenessMultirotor::getEffectivenessMatrix(matrix::Matrix &effectiveness) { + int num_actuators = 0; + for (size_t i = 0; i < NUM_ROTORS_MAX; i++) { // Get rotor axis matrix::Vector3f axis( @@ -173,6 +175,10 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom float ct = geometry.rotors[i].thrust_coef; float km = geometry.rotors[i].moment_ratio; + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + // Compute thrust generated by this rotor matrix::Vector3f thrust = ct * axis; @@ -184,5 +190,9 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom effectiveness(j, i) = moment(j); effectiveness(j + 3, i) = thrust(j); } + + num_actuators = i + 1; } + + return num_actuators; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index c5be27fc9115..1bb4c9efc223 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -70,16 +70,18 @@ class ActuatorEffectivenessMultirotor: public ModuleParams, public ActuatorEffec RotorGeometry rotors[NUM_ROTORS_MAX]; } MultirotorGeometry; - static void computeEffectivenessMatrix(const MultirotorGeometry &geometry, - matrix::Matrix &effectiveness); + static int computeEffectivenessMatrix(const MultirotorGeometry &geometry, + matrix::Matrix &effectiveness); bool getEffectivenessMatrix(matrix::Matrix &matrix) override; + int numActuators() const override { return _num_actuators; } private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ bool _updated{true}; + int _num_actuators{0}; DEFINE_PARAMETERS( (ParamFloat) _param_ca_mc_r0_px, diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 01cff0a7644d..135dc976df4d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -58,6 +58,7 @@ class ActuatorEffectivenessStandardVTOL: public ActuatorEffectiveness */ void setFlightPhase(const FlightPhase &flight_phase) override; + int numActuators() const override { return 7; } protected: bool _updated{true}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index d2f0633341f9..3c266b833c55 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -58,6 +58,7 @@ class ActuatorEffectivenessTiltrotorVTOL: public ActuatorEffectiveness */ void setFlightPhase(const FlightPhase &flight_phase) override; + int numActuators() const override { return 10; } protected: bool _updated{true}; }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index 4242f99cb54a..f32522fdadd8 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -68,11 +68,13 @@ ControlAllocation::getAllocatedControl() const void ControlAllocation::setEffectivenessMatrix( const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim) + const matrix::Vector &actuator_trim, int num_actuators) { _effectiveness = effectiveness; - _actuator_trim = clipActuatorSetpoint(actuator_trim); + _actuator_trim = actuator_trim; + clipActuatorSetpoint(_actuator_trim); _control_trim = _effectiveness * _actuator_trim; + _num_actuators = num_actuators; } const matrix::Matrix & @@ -115,34 +117,27 @@ ControlAllocation::setActuatorSetpoint( _actuator_sp = actuator_sp; // Clip - _actuator_sp = clipActuatorSetpoint(_actuator_sp); + clipActuatorSetpoint(_actuator_sp); // Compute achieved control _control_allocated = _effectiveness * _actuator_sp; } -matrix::Vector -ControlAllocation::clipActuatorSetpoint(const matrix::Vector &actuator) const +void +ControlAllocation::clipActuatorSetpoint(matrix::Vector &actuator) const { - matrix::Vector actuator_clipped; - - for (size_t i = 0; i < ControlAllocation::NUM_ACTUATORS; i++) { + for (int i = 0; i < _num_actuators; i++) { if (_actuator_max(i) < _actuator_min(i)) { - actuator_clipped(i) = _actuator_trim(i); + actuator(i) = _actuator_trim(i); - } else if (actuator_clipped(i) < _actuator_min(i)) { - actuator_clipped(i) = _actuator_min(i); + } else if (actuator(i) < _actuator_min(i)) { + actuator(i) = _actuator_min(i); - } else if (actuator_clipped(i) > _actuator_max(i)) { - actuator_clipped(i) = _actuator_max(i); - - } else { - actuator_clipped(i) = actuator(i); + } else if (actuator(i) > _actuator_max(i)) { + actuator(i) = _actuator_max(i); } } - - return actuator_clipped; } matrix::Vector diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 1423520f72d7..6cc0529b0480 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -105,7 +105,7 @@ class ControlAllocation * @param B Effectiveness matrix */ virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim); + const matrix::Vector &actuator_trim, int num_actuators); /** * Get the allocated actuator vector @@ -187,10 +187,8 @@ class ControlAllocation * The output is in the range [min; max] * * @param actuator Actuator vector to clip - * - * @return Clipped actuator setpoint */ - matrix::Vector clipActuatorSetpoint(const matrix::Vector &actuator) const; + void clipActuatorSetpoint(matrix::Vector &actuator) const; /** * Normalize the actuator setpoint between minimum and maximum values. @@ -204,6 +202,10 @@ class ControlAllocation matrix::Vector normalizeActuatorSetpoint(const matrix::Vector &actuator) const; + virtual void updateParameters() {} + + int numConfiguredActuators() const { return _num_actuators; } + protected: matrix::Matrix _effectiveness; //< Effectiveness matrix matrix::Vector _actuator_trim; //< Neutral actuator values @@ -213,4 +215,5 @@ class ControlAllocation matrix::Vector _control_sp; //< Control setpoint matrix::Vector _control_allocated; //< Allocated control matrix::Vector _control_trim; //< Control at trim actuator values + int _num_actuators{0}; }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index c4a57ffb9c6a..b613e77c739c 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -44,9 +44,9 @@ void ControlAllocationPseudoInverse::setEffectivenessMatrix( const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim) + const matrix::Vector &actuator_trim, int num_actuators) { - ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim); + ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, num_actuators); _mix_update_needed = true; } @@ -69,7 +69,7 @@ ControlAllocationPseudoInverse::allocate() _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); // Clip - _actuator_sp = clipActuatorSetpoint(_actuator_sp); + clipActuatorSetpoint(_actuator_sp); // Compute achieved control _control_allocated = _effectiveness * _actuator_sp; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp index c39c6c19f2b3..148b5af6cdb8 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -55,7 +55,7 @@ class ControlAllocationPseudoInverse: public ControlAllocation virtual void allocate() override; virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, - const matrix::Vector &actuator_trim) override; + const matrix::Vector &actuator_trim, int num_actuators) override; protected: matrix::Matrix _mix; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 6af2bd472c08..a497fd8c68ad 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -35,28 +35,36 @@ * @file ControlAllocationSequentialDesaturation.cpp * * @author Roman Bapst + * @author Beat Küng */ #include "ControlAllocationSequentialDesaturation.hpp" - void ControlAllocationSequentialDesaturation::allocate() { //Compute new gains if needed updatePseudoInverse(); - // Allocate - _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); + switch (_param_mc_airmode.get()) { + case 1: + mixAirmodeRP(); + break; + + case 2: + mixAirmodeRPY(); + break; - // go through control axes from lowest to highest priority and unsaturate the actuators - for (unsigned i = 0; i < NUM_AXES; i++) { - desaturateActuators(_actuator_sp, _axis_prio_increasing[i]); + default: + mixAirmodeDisabled(); + break; } + // TODO: thrust model (THR_MDL_FAC) + // Clip - _actuator_sp = clipActuatorSetpoint(_actuator_sp); + clipActuatorSetpoint(_actuator_sp); // Compute achieved control _control_allocated = _effectiveness * _actuator_sp; @@ -64,39 +72,32 @@ ControlAllocationSequentialDesaturation::allocate() void ControlAllocationSequentialDesaturation::desaturateActuators( ActuatorVector &actuator_sp, - const ControlAxis &axis) + const ActuatorVector &desaturation_vector, bool increase_only) { - ActuatorVector desaturation_vector = getDesaturationVector(axis); - float gain = computeDesaturationGain(desaturation_vector, actuator_sp); - actuator_sp = actuator_sp + gain * desaturation_vector; - - gain = computeDesaturationGain(desaturation_vector, actuator_sp); + if (increase_only && gain < 0.f) { + return; + } - actuator_sp = actuator_sp + 0.5f * gain * desaturation_vector; -} + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } -ControlAllocation::ActuatorVector ControlAllocationSequentialDesaturation::getDesaturationVector( - const ControlAxis &axis) -{ - ActuatorVector ret; + gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp); - for (unsigned i = 0; i < NUM_ACTUATORS; i++) { - ret(i) = _mix(i, axis); + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); } - - return ret; } - float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp) { float k_min = 0.f; float k_max = 0.f; - for (unsigned i = 0; i < NUM_ACTUATORS; i++) { + for (int i = 0; i < _num_actuators; i++) { // Avoid division by zero. If desaturation_vector(i) is zero, there's nothing we can do to unsaturate anyway if (fabsf(desaturation_vector(i)) < FLT_EPSILON) { continue; @@ -122,3 +123,118 @@ float ControlAllocationSequentialDesaturation::computeDesaturationGain(const Act // Reduce the saturation as much as possible return k_min + k_max; } + +void +ControlAllocationSequentialDesaturation::mixAirmodeRP() +{ + // Airmode for roll and pitch, but not yaw + + // Mix without yaw + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeRPY() +{ + // Airmode for roll, pitch and yaw + + // Do full mixing + ActuatorVector thrust_z; + ActuatorVector yaw; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + yaw(i) = _mix(i, ControlAxis::YAW); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Unsaturate yaw (in case upper and lower bounds are exceeded) + // to prioritize roll/pitch over yaw. + desaturateActuators(_actuator_sp, yaw); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeDisabled() +{ + // Airmode disabled: never allow to increase the thrust to unsaturate a motor + + // Mix without yaw + ActuatorVector thrust_z; + ActuatorVector roll; + ActuatorVector pitch; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + roll(i) = _mix(i, ControlAxis::ROLL); + pitch(i) = _mix(i, ControlAxis::PITCH); + } + + // only reduce thrust + desaturateActuators(_actuator_sp, thrust_z, true); + + // Reduce roll/pitch acceleration if needed to unsaturate + desaturateActuators(_actuator_sp, roll); + desaturateActuators(_actuator_sp, pitch); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixYaw() +{ + // Add yaw to outputs + ActuatorVector yaw; + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); + yaw(i) = _mix(i, ControlAxis::YAW); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), + // and allow some yaw response at maximum thrust + ActuatorVector max_prev = _actuator_max; + _actuator_max += (_actuator_max - _actuator_min) * 0.15f; + desaturateActuators(_actuator_sp, yaw); + _actuator_max = max_prev; + + // reduce thrust only + desaturateActuators(_actuator_sp, thrust_z, true); +} + +void +ControlAllocationSequentialDesaturation::updateParameters() +{ + updateParams(); +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp index 6765a2532288..53c422cd3940 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp @@ -45,45 +45,84 @@ #include "ControlAllocationPseudoInverse.hpp" -class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse +#include + +class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse, public ModuleParams { public: - ControlAllocationSequentialDesaturation() = default; + ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {} virtual ~ControlAllocationSequentialDesaturation() = default; void allocate() override; + void updateParameters() override; private: /** - * List of control axis used for desaturating the actuator vector. The desaturation logic will sequentially - * go through this list and if needed apply corrections to the demand of the corresponding axis in order to desaturate - * the actuator vector. + * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. + * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular + * acceleration on a specific axis. + * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the + * saturation will be minimized by shifting the vertical thrust setpoint, without changing the + * roll/pitch/yaw accelerations. + * + * Note that as we only slide along the given axis, in extreme cases outputs can still contain values + * outside of [min_output, max_output]. + * + * @param actuator_sp Actuator setpoint, vector that is modified + * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale + * @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector */ - ControlAxis _axis_prio_increasing [NUM_AXES] = {ControlAxis::YAW, ControlAxis::THRUST_Y, ControlAxis::THRUST_X, ControlAxis::THRUST_Z, ControlAxis::PITCH, ControlAxis::ROLL}; + void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector, + bool increase_only = false); /** - * Desaturate actuator setpoint vector. + * Computes the gain k by which desaturation_vector has to be multiplied + * in order to unsaturate the output that has the greatest saturation. * - * @return Desaturated actuator setpoint vector. + * @return desaturation gain */ - void desaturateActuators(ActuatorVector &actuator_sp, const ControlAxis &axis); + float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp); /** - * Get desaturation vector. + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. * - * @param axis Control axis - * @return ActuatorVector Column of the pseudo-inverse matrix corresponding to the given control axis. + * Desaturation behavior: airmode for roll/pitch: + * thrust is increased/decreased as much as required to meet the demanded roll/pitch. + * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. */ - ActuatorVector getDesaturationVector(const ControlAxis &axis); + void mixAirmodeRP(); /** - * Compute desaturation gain. + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. * - * @param desaturation_vector Column of the pseudo-inverse matrix corresponding to a given control axis. - * @param Actuator setpoint vector. - * @return Gain which eliminates the saturation of the highest saturated actuator. + * Desaturation behavior: full airmode for roll/pitch/yaw: + * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, + * while giving priority to roll and pitch over yaw. */ - float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp); + void mixAirmodeRPY(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded + * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. + * Thrust can be reduced to unsaturate the upper side. + * @see mixYaw() for the exact yaw behavior. + */ + void mixAirmodeDisabled(); + + /** + * Mix yaw by updating the actuator setpoint (that already contains roll/pitch/thrust). + * + * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow + * some yaw control on the upper end. On the lower end thrust will never be increased, + * but yaw is decreased as much as required. + */ + void mixYaw(); + + DEFINE_PARAMETERS( + (ParamInt) _param_mc_airmode ///< air-mode + ); }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index eaa26470698b..b0bf782302db 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -249,6 +249,10 @@ ControlAllocator::Run() parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); + if (_control_allocation) { + _control_allocation->updateParameters(); + } + updateParams(); parameters_updated(); } @@ -520,6 +524,7 @@ int ControlAllocator::print_status() const matrix::Matrix &effectiveness = _control_allocation->getEffectivenessMatrix(); PX4_INFO("Effectiveness.T ="); effectiveness.T().print(); + PX4_INFO("Configured actuators: %i", _control_allocation->numConfiguredActuators()); } // Print perf From fde6fb85c51ddcdee1ca02d7ac7c0ab6cf38aeb9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:51 +0200 Subject: [PATCH 125/129] control_allocator: avoid vector copies and sqrt() --- src/modules/control_allocator/ControlAllocator.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index b0bf782302db..1bb0ae312fad 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -393,7 +393,7 @@ ControlAllocator::publish_control_allocator_status() control_allocator_status.timestamp = hrt_absolute_time(); // Allocated control - matrix::Vector allocated_control = _control_allocation->getAllocatedControl(); + const matrix::Vector &allocated_control = _control_allocation->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); @@ -412,14 +412,14 @@ ControlAllocator::publish_control_allocator_status() // Allocation success flags control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1), - unallocated_control(2)).norm() < FLT_EPSILON); + unallocated_control(2)).norm_squared() < 1e-6f); control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4), - unallocated_control(5)).norm() < FLT_EPSILON); + unallocated_control(5)).norm_squared() < 1e-6f); // Actuator saturation - matrix::Vector actuator_sp = _control_allocation->getActuatorSetpoint(); - matrix::Vector actuator_min = _control_allocation->getActuatorMin(); - matrix::Vector actuator_max = _control_allocation->getActuatorMax(); + const matrix::Vector &actuator_sp = _control_allocation->getActuatorSetpoint(); + const matrix::Vector &actuator_min = _control_allocation->getActuatorMin(); + const matrix::Vector &actuator_max = _control_allocation->getActuatorMax(); for (size_t i = 0; i < NUM_ACTUATORS; i++) { if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) { From f8d245dc13e16528e5d4bb5a098380d76ce8f92d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:54 +0200 Subject: [PATCH 126/129] control_allocator: inline one-line setters & getters --- .../ControlAllocation/ControlAllocation.cpp | 55 ------------------- .../ControlAllocation/ControlAllocation.hpp | 18 +++--- 2 files changed, 9 insertions(+), 64 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index f32522fdadd8..b442d183f055 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -41,30 +41,6 @@ #include "ControlAllocation.hpp" -const matrix::Vector & -ControlAllocation::getActuatorSetpoint() const -{ - return _actuator_sp; -} - -void -ControlAllocation::setControlSetpoint(const matrix::Vector &control) -{ - _control_sp = control; -} - -const matrix::Vector & -ControlAllocation::getControlSetpoint() const -{ - return _control_sp; -} - -const matrix::Vector & -ControlAllocation::getAllocatedControl() const -{ - return _control_allocated; -} - void ControlAllocation::setEffectivenessMatrix( const matrix::Matrix &effectiveness, @@ -75,38 +51,7 @@ ControlAllocation::setEffectivenessMatrix( clipActuatorSetpoint(_actuator_trim); _control_trim = _effectiveness * _actuator_trim; _num_actuators = num_actuators; -} -const matrix::Matrix & -ControlAllocation::getEffectivenessMatrix() const -{ - return _effectiveness; -} - -void -ControlAllocation::setActuatorMin(const matrix::Vector - &actuator_min) -{ - _actuator_min = actuator_min; -} - -const matrix::Vector & -ControlAllocation::getActuatorMin() const -{ - return _actuator_min; -} - -void -ControlAllocation::setActuatorMax(const matrix::Vector - &actuator_max) -{ - _actuator_max = actuator_max; -} - -const matrix::Vector & -ControlAllocation::getActuatorMax() const -{ - return _actuator_max; } void diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 6cc0529b0480..86a2cfc8ed61 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -112,63 +112,63 @@ class ControlAllocation * * @return Actuator vector */ - const matrix::Vector &getActuatorSetpoint() const; + const matrix::Vector &getActuatorSetpoint() const { return _actuator_sp; } /** * Set the desired control vector * * @param Control vector */ - void setControlSetpoint(const matrix::Vector &control); + void setControlSetpoint(const matrix::Vector &control) { _control_sp = control; } /** * Set the desired control vector * * @param Control vector */ - const matrix::Vector &getControlSetpoint() const; + const matrix::Vector &getControlSetpoint() const { return _control_sp; } /** * Get the allocated control vector * * @return Control vector */ - const matrix::Vector &getAllocatedControl() const; + const matrix::Vector &getAllocatedControl() const { return _control_allocated; } /** * Get the control effectiveness matrix * * @return Effectiveness matrix */ - const matrix::Matrix &getEffectivenessMatrix() const; + const matrix::Matrix &getEffectivenessMatrix() const { return _effectiveness; } /** * Set the minimum actuator values * * @param actuator_min Minimum actuator values */ - void setActuatorMin(const matrix::Vector &actuator_min); + void setActuatorMin(const matrix::Vector &actuator_min) { _actuator_min = actuator_min; } /** * Get the minimum actuator values * * @return Minimum actuator values */ - const matrix::Vector &getActuatorMin() const; + const matrix::Vector &getActuatorMin() const { return _actuator_min; } /** * Set the maximum actuator values * * @param actuator_max Maximum actuator values */ - void setActuatorMax(const matrix::Vector &actuator_max); + void setActuatorMax(const matrix::Vector &actuator_max) { _actuator_max = actuator_max; } /** * Get the maximum actuator values * * @return Maximum actuator values */ - const matrix::Vector &getActuatorMax() const; + const matrix::Vector &getActuatorMax() const { return _actuator_max; } /** * Set the current actuator setpoint. From bdacca35e6b5195fa98c64cd0fbb61f0fded9d4a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Aug 2020 08:20:57 +0200 Subject: [PATCH 127/129] control_allocator: ensure unused outputs are initialized to min Mostly important in cases where the ouputs would change (e.g. param change) --- .../control_allocator/ControlAllocation/ControlAllocation.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index b442d183f055..eb2653db8f44 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -52,6 +52,10 @@ ControlAllocation::setEffectivenessMatrix( _control_trim = _effectiveness * _actuator_trim; _num_actuators = num_actuators; + // make sure unused actuators are initialized to min + for (int i = num_actuators; i < NUM_ACTUATORS; ++i) { + _actuator_sp(i) = _actuator_min(i); + } } void From 2d3d6e814c48c96edcf2352920fb140db98c836b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 14 Aug 2020 09:55:48 +0200 Subject: [PATCH 128/129] control_allocator: set unused actuators to trim instead of min --- .../control_allocator/ControlAllocation/ControlAllocation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index eb2653db8f44..9fac9f55c841 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -52,9 +52,9 @@ ControlAllocation::setEffectivenessMatrix( _control_trim = _effectiveness * _actuator_trim; _num_actuators = num_actuators; - // make sure unused actuators are initialized to min + // make sure unused actuators are initialized to trim for (int i = num_actuators; i < NUM_ACTUATORS; ++i) { - _actuator_sp(i) = _actuator_min(i); + _actuator_sp(i) = _actuator_trim(i); } } From 014d6acae4712875fac3fefd14b0de7369a379f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 14 Aug 2020 09:56:49 +0200 Subject: [PATCH 129/129] ActuatorEffectivenessMultirotor: set effectiveness to zero when computing the matrix Just to be on the safe side. --- .../ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp index e2e7d2e88376..11cab60aa070 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp @@ -145,6 +145,8 @@ ActuatorEffectivenessMultirotor::computeEffectivenessMatrix(const MultirotorGeom { int num_actuators = 0; + effectiveness.setZero(); + for (size_t i = 0; i < NUM_ROTORS_MAX; i++) { // Get rotor axis matrix::Vector3f axis(