From d4fb1b1f8b8c4575716b83be2043e785565cb0e6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 29 Oct 2022 10:55:20 -0400 Subject: [PATCH 01/43] Update submodule GPS drivers to latest Sat Oct 29 12:39:01 UTC 2022 - GPS drivers in PX4/Firmware (d67b19ac1d41b2dcfc61ed6d353ae513ac3f4a82): https://github.com/PX4/PX4-GPSDrivers/commit/1ff87868f6008f06e2033ee05dd904ec54109e52 - GPS drivers current upstream: https://github.com/PX4/PX4-GPSDrivers/commit/fa2177d690207e42e0d8c92e9663578340d44fe4 - Changes: https://github.com/PX4/PX4-GPSDrivers/compare/1ff87868f6008f06e2033ee05dd904ec54109e52...fa2177d690207e42e0d8c92e9663578340d44fe4 fa2177d 2022-10-10 Michael Schaeuble - Return from GPSDriverUBX::receive when ready, don't wait until no data is received Co-authored-by: PX4 BuildBot --- src/drivers/gps/devices | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 1ff87868f6..fa2177d690 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 1ff87868f6008f06e2033ee05dd904ec54109e52 +Subproject commit fa2177d690207e42e0d8c92e9663578340d44fe4 From a3caaa137249d288952a78ddbb11506491a047b4 Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Sat, 29 Oct 2022 12:38:55 +0000 Subject: [PATCH 02/43] Update submodule sitl_gazebo to latest Sat Oct 29 12:38:55 UTC 2022 - sitl_gazebo in PX4/Firmware (498937c56cd1b1a676bb93fa22ee7b7bc99c69cc): https://github.com/PX4/PX4-SITL_gazebo/commit/e80432759540c91f85a012f47aa6ebb0ee9de7e4 - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/56b5508b72f40339448f3525dd4dc51f30512cbd - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/e80432759540c91f85a012f47aa6ebb0ee9de7e4...56b5508b72f40339448f3525dd4dc51f30512cbd 56b5508 2022-10-13 junkdood - Update boat.sdf.jinja --- Tools/simulation/gazebo/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gazebo/sitl_gazebo b/Tools/simulation/gazebo/sitl_gazebo index e804327595..56b5508b72 160000 --- a/Tools/simulation/gazebo/sitl_gazebo +++ b/Tools/simulation/gazebo/sitl_gazebo @@ -1 +1 @@ -Subproject commit e80432759540c91f85a012f47aa6ebb0ee9de7e4 +Subproject commit 56b5508b72f40339448f3525dd4dc51f30512cbd From 80af8262b51322c1537c2d1c88bb902d1f599b1a Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Sat, 29 Oct 2022 12:39:05 +0000 Subject: [PATCH 03/43] Update submodule mavlink to latest Sat Oct 29 12:39:05 UTC 2022 - mavlink in PX4/Firmware (f8b38591ac0bd31a87cb38ae4b2f7dd74400cda2): https://github.com/mavlink/mavlink/commit/dda5a18ddb002a871ba301bb584893ee6378e2f3 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/d012c7afd5f4e2a9388710596fd3eb9a1b3eb086 - Changes: https://github.com/mavlink/mavlink/compare/dda5a18ddb002a871ba301bb584893ee6378e2f3...d012c7afd5f4e2a9388710596fd3eb9a1b3eb086 d012c7af 2022-10-27 Hamish Willee - update pymavlink to latest (#1906) e1058881 2022-10-27 Hamish Willee - BATTERY_STATUS_V2 - update to 20221013 RFC version (#1846) 27007cc3 2022-10-25 Hamish Willee - fix typo MAV_PROTOCOL_CAPABILITY_PARAM_ENCODE_C_CAST (#1904) 00007aca 2022-10-19 Hamish Willee - SET_DEFAULT_INTERVAL may be 0 (#1903) af35d3a4 2022-10-09 Ashish Kurmi - ci: add minimum GitHub token permissions for workflow (#1898) 33dde554 2022-10-09 Siddharth Bharat Purohit - add vendor specs for cubepilot (#1901) --- src/modules/mavlink/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index dda5a18ddb..d012c7afd5 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit dda5a18ddb002a871ba301bb584893ee6378e2f3 +Subproject commit d012c7afd5f4e2a9388710596fd3eb9a1b3eb086 From 21f49ff5bed595533d750b791be0ba775eab5b9d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 31 Oct 2022 14:09:17 +1300 Subject: [PATCH 04/43] drivers: fix two includes for CLion This fixes two errors where CLion complains: error: 'size_t' does not name a type --- .../imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp | 1 + .../imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp b/src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp index 7ea181f339..418f7b2add 100644 --- a/src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp +++ b/src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp @@ -41,6 +41,7 @@ #pragma once #include +#include namespace InvenSense_ICM20649 { diff --git a/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp b/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp index 5e9d8259c7..5d1c580bd5 100644 --- a/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp +++ b/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp @@ -41,6 +41,7 @@ #pragma once #include +#include namespace InvenSense_ICM42670P { From 3f5d7f38cd47febae9a56373e4be1c8d7293553a Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 31 Oct 2022 09:13:13 +0100 Subject: [PATCH 05/43] Handle waypoint altitude acceptance radius for boats (#20508) This corrects the waypoint handling logic to include boat type vehicles --- src/modules/commander/Commander.cpp | 2 +- src/modules/commander/commander_helper.cpp | 12 +----------- src/modules/commander/commander_helper.h | 2 -- 3 files changed, 2 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index b174bfd21b..f53a72983a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1575,7 +1575,7 @@ void Commander::updateParameters() && _vtol_vehicle_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); const bool is_fixed = is_fixed_wing(_vehicle_status) || (is_vtol(_vehicle_status) && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - const bool is_ground = is_ground_rover(_vehicle_status); + const bool is_ground = is_ground_vehicle(_vehicle_status); /* disable manual override for all systems that rely on electronic stabilization */ if (is_rotary) { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index c864fd89d8..4790d3d2f7 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -118,19 +118,9 @@ bool is_fixed_wing(const vehicle_status_s ¤t_status) return current_status.system_type == VEHICLE_TYPE_FIXED_WING; } -bool is_ground_rover(const vehicle_status_s ¤t_status) -{ - return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER; -} - -bool is_boat(const vehicle_status_s ¤t_status) -{ - return current_status.system_type == VEHICLE_TYPE_BOAT; -} - bool is_ground_vehicle(const vehicle_status_s ¤t_status) { - return is_ground_rover(current_status) || is_boat(current_status); + return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER); } // End time for currently blinking LED message, 0 if no blink message diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index a7dd82edde..3e96a636e5 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -55,8 +55,6 @@ bool is_rotary_wing(const vehicle_status_s ¤t_status); bool is_vtol(const vehicle_status_s ¤t_status); bool is_vtol_tailsitter(const vehicle_status_s ¤t_status); bool is_fixed_wing(const vehicle_status_s ¤t_status); -bool is_ground_rover(const vehicle_status_s ¤t_status); -bool is_boat(const vehicle_status_s ¤t_status); bool is_ground_vehicle(const vehicle_status_s ¤t_status); int buzzer_init(void); From ba3f3935abd7c639fb807e0d3573d8a345d5f92f Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Mon, 31 Oct 2022 10:19:25 +0100 Subject: [PATCH 06/43] hardfault_log: Correctly annotate adddresses for the stack trace in the hardfault log. --- src/systemcmds/hardfault_log/hardfault_log.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/hardfault_log/hardfault_log.c b/src/systemcmds/hardfault_log/hardfault_log.c index 30e3616ddd..3f0b9ca8fe 100644 --- a/src/systemcmds/hardfault_log/hardfault_log.c +++ b/src/systemcmds/hardfault_log/hardfault_log.c @@ -363,7 +363,7 @@ static int write_stack(bool inValid, int winsize, uint32_t wtopaddr, ret = -EIO; } - wtopaddr--; + wtopaddr -= sizeof(stack_word_t); } } } @@ -621,7 +621,7 @@ static int write_intterupt_stack(int fdin, int fdout, info_s *pi, char *buffer, lseek(fdin, offsetof(fullcontext_s, istack), SEEK_SET); ret = write_stack((pi->flags & eInvalidIntStackPrt) != 0, CONFIG_ISTACK_SIZE, - pi->stacks.interrupt.sp + CONFIG_ISTACK_SIZE / 2, + pi->stacks.interrupt.sp + (CONFIG_ISTACK_SIZE / 2) * sizeof(stack_word_t), pi->stacks.interrupt.top, pi->stacks.interrupt.sp, pi->stacks.interrupt.top - pi->stacks.interrupt.size, @@ -644,7 +644,7 @@ static int write_user_stack(int fdin, int fdout, info_s *pi, char *buffer, lseek(fdin, offsetof(fullcontext_s, ustack), SEEK_SET); ret = write_stack((pi->flags & eInvalidUserStackPtr) != 0, CONFIG_USTACK_SIZE, - pi->stacks.user.sp + CONFIG_USTACK_SIZE / 2, + pi->stacks.user.sp + (CONFIG_USTACK_SIZE / 2) * sizeof(stack_word_t), pi->stacks.user.top, pi->stacks.user.sp, pi->stacks.user.top - pi->stacks.user.size, From 34c852255e16bb62d2caf0c3be1f2fea660036ba Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Mon, 31 Oct 2022 08:51:23 -0700 Subject: [PATCH 07/43] Changed M_PI to M_PI_F in the matrix library since M_PI is non-standard. (#20458) * Changed M_PI to M_PI_F in the matrix library since M_PI is non-standard. * Added a new M_PI_PRECISE constant definition to px4_platform_common/defines.h to be used in places when M_PI is desired but shouldn't be used because it is not C standard. * Added the px4_platform_common/defines.h include to the matrix library math.hpp header to pull in some non-standard M_PI constants and updated the test files to use those constants. * Fixed PI constants in matrix helper test to prevent test failure --- .../common/include/px4_platform_common/defines.h | 4 ++++ src/lib/matrix/matrix/Euler.hpp | 4 ++-- src/lib/matrix/matrix/helper_functions.hpp | 6 +++--- src/lib/matrix/matrix/math.hpp | 1 + src/lib/matrix/test/MatrixAttitudeTest.cpp | 6 +++--- src/lib/matrix/test/MatrixHelperTest.cpp | 16 ++++++++-------- src/lib/matrix/test/MatrixUnwrapTest.cpp | 4 ++-- 7 files changed, 23 insertions(+), 18 deletions(-) diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h index e0a740943b..913481bfee 100644 --- a/platforms/common/include/px4_platform_common/defines.h +++ b/platforms/common/include/px4_platform_common/defines.h @@ -136,5 +136,9 @@ __END_DECLS #define M_LOG2_E_F 0.69314718f #define M_INVLN2_F 1.44269504f // 1 / log(2) +/* The M_PI, as stated above, is not C standard. If you need it and + * it isn't in your math.h file then you can use this instead. */ +#define M_PI_PRECISE 3.141592653589793238462643383279502884 + #define M_DEG_TO_RAD 0.017453292519943295 #define M_RAD_TO_DEG 57.295779513082323 diff --git a/src/lib/matrix/matrix/Euler.hpp b/src/lib/matrix/matrix/Euler.hpp index cb977e690e..d885b85215 100644 --- a/src/lib/matrix/matrix/Euler.hpp +++ b/src/lib/matrix/matrix/Euler.hpp @@ -93,11 +93,11 @@ public: { theta() = std::asin(-dcm(2, 0)); - if ((std::fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) { + if ((std::fabs(theta() - Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) { phi() = 0; psi() = std::atan2(dcm(1, 2), dcm(0, 2)); - } else if ((std::fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) { + } else if ((std::fabs(theta() + Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) { phi() = 0; psi() = std::atan2(-dcm(1, 2), -dcm(0, 2)); diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp index 8af22ac53a..6a9e871ef0 100644 --- a/src/lib/matrix/matrix/helper_functions.hpp +++ b/src/lib/matrix/matrix/helper_functions.hpp @@ -95,7 +95,7 @@ Integer wrap(Integer x, Integer low, Integer high) template Type wrap_pi(Type x) { - return wrap(x, Type(-M_PI), Type(M_PI)); + return wrap(x, Type(-M_PI_PRECISE), Type(M_PI_PRECISE)); } /** @@ -104,7 +104,7 @@ Type wrap_pi(Type x) template Type wrap_2pi(Type x) { - return wrap(x, Type(0), Type((2 * M_PI))); + return wrap(x, Type(0), Type((2 * M_PI_PRECISE))); } /** @@ -132,7 +132,7 @@ Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high template Type unwrap_pi(const Type last_angle, const Type new_angle) { - return unwrap(last_angle, new_angle, Type(-M_PI), Type(M_PI)); + return unwrap(last_angle, new_angle, Type(-M_PI_PRECISE), Type(M_PI_PRECISE)); } /** diff --git a/src/lib/matrix/matrix/math.hpp b/src/lib/matrix/matrix/math.hpp index 24a43723bf..6601a5bc1c 100644 --- a/src/lib/matrix/matrix/math.hpp +++ b/src/lib/matrix/matrix/math.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include "helper_functions.hpp" diff --git a/src/lib/matrix/test/MatrixAttitudeTest.cpp b/src/lib/matrix/test/MatrixAttitudeTest.cpp index 40e6dea4d2..3ea81ebf3a 100644 --- a/src/lib/matrix/test/MatrixAttitudeTest.cpp +++ b/src/lib/matrix/test/MatrixAttitudeTest.cpp @@ -200,8 +200,8 @@ TEST(MatrixAttitudeTest, Attitude) } // constants - double deg2rad = M_PI / 180.0; - double rad2deg = 180.0 / M_PI; + double deg2rad = M_PI_PRECISE / 180.0; + double rad2deg = 180.0 / M_PI_PRECISE; // euler dcm round trip check for (double roll = -90; roll <= 90; roll += 90) { @@ -417,7 +417,7 @@ TEST(MatrixAttitudeTest, Attitude) EXPECT_EQ(q, q_true); // from axis angle, with length of vector the rotation - float n = float(std::sqrt(4 * M_PI * M_PI / 3)); + float n = float(std::sqrt(4 * M_PI_F * M_PI_F / 3)); q = AxisAnglef(n, n, n); EXPECT_EQ(q, Quatf(-1, 0, 0, 0)); q = AxisAnglef(0, 0, 0); diff --git a/src/lib/matrix/test/MatrixHelperTest.cpp b/src/lib/matrix/test/MatrixHelperTest.cpp index 6133f497df..19e74bdd19 100644 --- a/src/lib/matrix/test/MatrixHelperTest.cpp +++ b/src/lib/matrix/test/MatrixHelperTest.cpp @@ -81,20 +81,20 @@ TEST(MatrixHelperTest, Helper) // wrap pi EXPECT_FLOAT_EQ(wrap_pi(0.), 0.); - EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI))); - EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI))); + EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI_PRECISE))); + EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI_PRECISE))); EXPECT_FLOAT_EQ(wrap_pi(3.), 3.); - EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI)); - EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI)); - EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI)); + EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI_PRECISE)); + EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI_PRECISE)); + EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI_PRECISE)); EXPECT_FALSE(std::isfinite(wrap_pi(NAN))); // wrap 2pi EXPECT_FLOAT_EQ(wrap_2pi(0.), 0.); - EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI)); + EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI_PRECISE)); EXPECT_FLOAT_EQ(wrap_2pi(3.), (3.)); - EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI))); - EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI))); + EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI_PRECISE))); + EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI_PRECISE))); EXPECT_FALSE(std::isfinite(wrap_2pi(NAN))); // Equality checks diff --git a/src/lib/matrix/test/MatrixUnwrapTest.cpp b/src/lib/matrix/test/MatrixUnwrapTest.cpp index f774174ccf..138074b072 100644 --- a/src/lib/matrix/test/MatrixUnwrapTest.cpp +++ b/src/lib/matrix/test/MatrixUnwrapTest.cpp @@ -5,7 +5,7 @@ using namespace matrix; TEST(MatrixUnwrapTest, UnwrapFloats) { - const float M_TWO_PI_F = float(M_PI * 2); + const float M_TWO_PI_F = float(M_PI_F * 2); float unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25}; float wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25}; @@ -34,7 +34,7 @@ TEST(MatrixUnwrapTest, UnwrapFloats) TEST(MatrixUnwrapTest, UnwrapDoubles) { - const double M_TWO_PI = M_PI * 2; + const double M_TWO_PI = M_PI_PRECISE * 2; double unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25}; double wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25}; From 82f63475d7f97056171fff1a79eddfa266441041 Mon Sep 17 00:00:00 2001 From: Zachary Lowell Date: Mon, 31 Oct 2022 11:59:10 -0500 Subject: [PATCH 08/43] Qurt work_queue Implementation (#20522) --- platforms/qurt/cmake/px4_impl_os.cmake | 1 + platforms/qurt/include/px4_arch/micro_hal.h | 35 +++++++++++++++ platforms/qurt/include/qurt_reqs.h | 48 +++++++++++++++++++++ platforms/qurt/src/px4/CMakeLists.txt | 2 + 4 files changed, 86 insertions(+) create mode 100644 platforms/qurt/include/px4_arch/micro_hal.h create mode 100644 platforms/qurt/include/qurt_reqs.h diff --git a/platforms/qurt/cmake/px4_impl_os.cmake b/platforms/qurt/cmake/px4_impl_os.cmake index b7906c495a..09b606040b 100644 --- a/platforms/qurt/cmake/px4_impl_os.cmake +++ b/platforms/qurt/cmake/px4_impl_os.cmake @@ -126,6 +126,7 @@ function(px4_os_add_flags) -Wno-unknown-warning-option -Wno-cast-align + --include=${PX4_SOURCE_DIR}/platforms/qurt/include/qurt_reqs.h ) # Clear -rdynamic flag which fails for hexagon diff --git a/platforms/qurt/include/px4_arch/micro_hal.h b/platforms/qurt/include/px4_arch/micro_hal.h new file mode 100644 index 0000000000..c23a40dbcf --- /dev/null +++ b/platforms/qurt/include/px4_arch/micro_hal.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. 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. + * + ****************************************************************************/ + +// Placeholder + diff --git a/platforms/qurt/include/qurt_reqs.h b/platforms/qurt/include/qurt_reqs.h new file mode 100644 index 0000000000..a3f6989b17 --- /dev/null +++ b/platforms/qurt/include/qurt_reqs.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. 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. + * + ****************************************************************************/ + +// This file is meant to tackle the dependencies found in PX4 +// that have not been implemented in the Hexagon SDK yet. + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +typedef unsigned long useconds_t; +int usleep(useconds_t usec); + +#ifdef __cplusplus +} +#endif diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt index 9bd2ebb7eb..45c33c674d 100644 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ b/platforms/qurt/src/px4/CMakeLists.txt @@ -39,3 +39,5 @@ set(QURT_LAYER_SRCS add_library(px4_layer ${QURT_LAYER_SRCS} ) + +target_link_libraries(px4_layer PRIVATE work_queue) From 6d2dd798a08fc2514f8049f5ec6cdf27239ba859 Mon Sep 17 00:00:00 2001 From: Zachary Lowell Date: Mon, 31 Oct 2022 17:40:29 -0500 Subject: [PATCH 09/43] Qurt drv_hrt Implementation (#20528) --- platforms/qurt/src/px4/CMakeLists.txt | 1 + platforms/qurt/src/px4/drv_hrt.cpp | 326 ++++++++++++++++++++++++++ 2 files changed, 327 insertions(+) create mode 100644 platforms/qurt/src/px4/drv_hrt.cpp diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt index 45c33c674d..020a44bfec 100644 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ b/platforms/qurt/src/px4/CMakeLists.txt @@ -33,6 +33,7 @@ # Placeholder set(QURT_LAYER_SRCS + drv_hrt.cpp tasks.cpp ) diff --git a/platforms/qurt/src/px4/drv_hrt.cpp b/platforms/qurt/src/px4/drv_hrt.cpp new file mode 100644 index 0000000000..31b274f3b2 --- /dev/null +++ b/platforms/qurt/src/px4/drv_hrt.cpp @@ -0,0 +1,326 @@ +/**************************************************************************** + * + * Copyright (C) 2022 ModalAI, Inc. 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 +#include +#include + +#include +#include +#include +#include + +#include "hrt_work.h" + +static constexpr unsigned HRT_INTERVAL_MIN = 50; +static constexpr unsigned HRT_INTERVAL_MAX = 50000000; + +static struct sq_queue_s callout_queue; + +static uint64_t latency_baseline; + +static uint64_t latency_actual; + +const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + +static px4_sem_t _hrt_lock; +static struct work_s _hrt_work; + +static int32_t dsp_offset = 0; + +static void hrt_latency_update(); + +static void hrt_call_reschedule(); +static void hrt_call_invoke(); + +hrt_abstime hrt_absolute_time_offset() +{ + return 0; +} + +static void hrt_lock() +{ + px4_sem_wait(&_hrt_lock); +} + +static void hrt_unlock() +{ + px4_sem_post(&_hrt_lock); +} + +int px4_clock_settime(clockid_t clk_id, struct timespec *tp) +{ + return 0; +} + +int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) +{ + int rv = clock_gettime(clk_id, tp); + hrt_abstime temp_abstime = ts_to_abstime(tp); + + if (dsp_offset < 0) { + hrt_abstime temp_offset = -dsp_offset; + + if (temp_offset >= temp_abstime) { temp_abstime = 0; } + + else { temp_abstime -= temp_offset; } + + } else { + temp_abstime += (hrt_abstime) dsp_offset; + } + + tp->tv_sec = temp_abstime / 1000000; + tp->tv_nsec = (temp_abstime % 1000000) * 1000; + return rv; +} + +hrt_abstime hrt_absolute_time() +{ + struct timespec ts; + px4_clock_gettime(CLOCK_MONOTONIC, &ts); + return ts_to_abstime(&ts); +} + +int hrt_set_absolute_time_offset(int32_t time_diff_us) +{ + dsp_offset = time_diff_us; + return 0; +} + +hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then) +{ + hrt_abstime delta = hrt_absolute_time() - *then; + return delta; +} + +void hrt_store_absolute_time(volatile hrt_abstime *t) +{ + *t = hrt_absolute_time(); +} + +bool hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +void hrt_cancel(struct hrt_call *entry) +{ + hrt_lock(); + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + entry->period = 0; + hrt_unlock(); +} + +static void hrt_latency_update() +{ + uint16_t latency = latency_actual - latency_baseline; + unsigned index; + + for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { + if (latency <= latency_buckets[index]) { + latency_counters[index]++; + return; + } + } + + latency_counters[index]++; +} + +void hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} + +void hrt_init() +{ + sq_init(&callout_queue); + + int sem_ret = px4_sem_init(&_hrt_lock, 0, 1); + + if (sem_ret) { + PX4_ERR("SEM INIT FAIL: %s", strerror(errno)); + } + + memset(&_hrt_work, 0, sizeof(_hrt_work)); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == nullptr) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == nullptr) || (entry->deadline < next->deadline)) { + //lldbg("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != nullptr); + } +} + +static void +hrt_tim_isr(void *p) +{ + latency_actual = hrt_absolute_time(); + hrt_latency_update(); + hrt_call_invoke(); + hrt_lock(); + hrt_call_reschedule(); + hrt_unlock(); +} + +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + hrt_abstime delay = HRT_INTERVAL_MAX; + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + if (next != nullptr) { + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + delay = HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + delay = next->deadline - now; + } + } + + latency_baseline = now + delay; + hrt_work_cancel(&_hrt_work); + hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay); +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval); + hrt_lock(); + + if (entry->deadline != 0) { + sq_rem(&entry->link, &callout_queue); + } + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); + hrt_unlock(); +} + +void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); +} + +static void +hrt_call_invoke() +{ + struct hrt_call *call; + hrt_abstime deadline; + + hrt_lock(); + + while (true) { + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == nullptr) { + break; + } + + if (call->deadline > now) { + break; + } + + sq_rem(&call->link, &callout_queue); + deadline = call->deadline; + call->deadline = 0; + + if (call->callout) { + hrt_unlock(); + call->callout(call->arg); + hrt_lock(); + } + + if (call->period != 0) { + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + + hrt_call_enter(call); + } + } + + hrt_unlock(); +} From a90857f651c3ab63a1ce63637b93f25e452e793b Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Tue, 1 Nov 2022 06:06:27 +0100 Subject: [PATCH 10/43] FW separate reset integrals for messages (#20502) This commit separates integral resets for attitude and rate control setpoints --- msg/VehicleAttitudeSetpoint.msg | 2 +- msg/VehicleRatesSetpoint.msg | 2 ++ .../fw_att_control/FixedwingAttitudeControl.cpp | 10 +++++----- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 6 +++--- .../PositionControl/PositionControlTest.cpp | 2 +- 5 files changed, 12 insertions(+), 10 deletions(-) diff --git a/msg/VehicleAttitudeSetpoint.msg b/msg/VehicleAttitudeSetpoint.msg index 05888d52c5..e1f444aae0 100644 --- a/msg/VehicleAttitudeSetpoint.msg +++ b/msg/VehicleAttitudeSetpoint.msg @@ -13,7 +13,7 @@ float32[4] q_d # Desired quaternion for quaternion control # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] -bool reset_rate_integrals # Reset roll/pitch/yaw integrals (navigation logic change) +bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) diff --git a/msg/VehicleRatesSetpoint.msg b/msg/VehicleRatesSetpoint.msg index f30ae2fc8f..88adcf3bea 100644 --- a/msg/VehicleRatesSetpoint.msg +++ b/msg/VehicleRatesSetpoint.msg @@ -8,3 +8,5 @@ float32 yaw # [rad/s] yaw rate setpoint # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] + +bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 172f54f8d9..80c7457596 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -157,7 +157,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); - _att_sp.reset_rate_integrals = false; + _att_sp.reset_integral = false; _att_sp.timestamp = hrt_absolute_time(); @@ -392,8 +392,8 @@ void FixedwingAttitudeControl::Run() const float airspeed = get_airspeed_and_update_scaling(); /* reset integrals where needed */ - if (_att_sp.reset_rate_integrals) { - _rate_control.resetIntegral(); + if (_att_sp.reset_integral) { + _rates_sp.reset_integral = true; } /* Reset integrators if the aircraft is on ground @@ -403,7 +403,7 @@ void FixedwingAttitudeControl::Run() || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) { - _rate_control.resetIntegral(); + _rates_sp.reset_integral = true; _wheel_ctrl.reset_integrator(); } @@ -605,7 +605,7 @@ void FixedwingAttitudeControl::Run() _actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw; - if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) { + if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u) || _rates_sp.reset_integral) { _rate_control.resetIntegral(); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 3391f01e8e..c525801035 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1487,7 +1487,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo if (_runway_takeoff.resetIntegrators()) { // reset integrals except yaw (which also counts for the wheel controller) - _att_sp.reset_rate_integrals = true; + _att_sp.reset_integral = true; // throttle is open loop anyway during ground roll, no need to wind up the integrator _tecs.resetIntegrals(); @@ -1626,7 +1626,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } else { /* Tell the attitude controller to stop integrating while we are waiting for the launch */ - _att_sp.reset_rate_integrals = true; + _att_sp.reset_integral = true; /* Set default roll and pitch setpoints during detection phase */ _att_sp.roll_body = 0.0f; @@ -2249,7 +2249,7 @@ FixedwingPositionControl::Run() _npfg.setPeriod(_param_npfg_period.get()); _l1_control.set_l1_period(_param_fw_l1_period.get()); - _att_sp.reset_rate_integrals = false; + _att_sp.reset_integral = false; // by default we don't want yaw to be contoller directly with rudder _att_sp.fw_control_yaw = false; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 384646afd4..ebcdb5b782 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -62,7 +62,7 @@ TEST(PositionControlTest, EmptySetpoint) EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); - EXPECT_EQ(attitude.reset_rate_integrals, false); + EXPECT_EQ(attitude.reset_integral, false); EXPECT_EQ(attitude.fw_control_yaw, false); EXPECT_FLOAT_EQ(attitude.apply_flaps, 0.f);//vehicle_attitude_setpoint_s::FLAPS_OFF); // TODO why no reference? } From afe1f8242380ca4ab9be683b1c88f35f92430be2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 31 Oct 2022 19:07:45 +0100 Subject: [PATCH 11/43] ver command: clarify PX4 version instead of FW version --- src/systemcmds/ver/ver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/systemcmds/ver/ver.cpp b/src/systemcmds/ver/ver.cpp index 2a68c67a8f..8120e1805d 100644 --- a/src/systemcmds/ver/ver.cpp +++ b/src/systemcmds/ver/ver.cpp @@ -158,7 +158,7 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[]) } if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - printf("FW git-hash: %s\n", px4_firmware_version_string()); + printf("PX4 git-hash: %s\n", px4_firmware_version_string()); unsigned fwver = px4_firmware_version(); unsigned major = (fwver >> (8 * 3)) & 0xFF; unsigned minor = (fwver >> (8 * 2)) & 0xFF; @@ -166,17 +166,17 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[]) unsigned type = (fwver >> (8 * 0)) & 0xFF; if (type == 255) { - printf("FW version: Release %u.%u.%u (%u)\n", major, minor, patch, fwver); + printf("PX4 version: Release %u.%u.%u (%u)\n", major, minor, patch, fwver); } else { - printf("FW version: %u.%u.%u %x (%u)\n", major, minor, patch, type, fwver); + printf("PX4 version: %u.%u.%u %x (%u)\n", major, minor, patch, type, fwver); } if (show_all) { const char *git_branch = px4_firmware_git_branch(); if (git_branch && git_branch[0]) { - printf("FW git-branch: %s\n", git_branch); + printf("PX4 git-branch: %s\n", git_branch); } } From c5c634be7f4467506c83dee875c269f11366f9fa Mon Sep 17 00:00:00 2001 From: benjinne Date: Tue, 1 Nov 2022 02:42:27 -0400 Subject: [PATCH 12/43] lisXmdl: use I2CSPIDriverConfig (#20506) - allows to configure the I2C address - lis3mdl: add 2nd possible address to start --- ROMFS/px4fmu_common/init.d/rc.sensors | 5 ++++- src/drivers/magnetometer/lis2mdl/lis2mdl.h | 4 ++-- src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp | 12 ++++++------ src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp | 7 +++---- src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp | 12 ++++++------ src/drivers/magnetometer/lis3mdl/lis3mdl.h | 4 ++-- src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp | 12 ++++++------ src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp | 8 ++++---- src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp | 12 ++++++------ 9 files changed, 39 insertions(+), 37 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 2694eb3c64..3931af6ebd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -191,7 +191,10 @@ then ist8308 -X -q start ist8310 -X -q start lis2mdl -X -q start - lis3mdl -X -q start + if ! lis3mdl -X -q start + then + lis3mdl -X -q -a 0x1c start + fi qmc5883l -X -q start rm3100 -X -q start diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl.h b/src/drivers/magnetometer/lis2mdl/lis2mdl.h index 197739b2ca..1296ddfa4d 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl.h +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl.h @@ -80,8 +80,8 @@ #define CFG_REG_C_BDU (1 << 4) /* avoids reading of incorrect data due to async reads */ /* interface factories */ -extern device::Device *LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); -extern device::Device *LIS2MDL_I2C_interface(int bus, int bus_frequency); +extern device::Device *LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config); +extern device::Device *LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config); #define LIS2MDLL_ADDRESS 0x1e diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp index 6bbfe0dede..3dcc472e60 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp @@ -56,7 +56,7 @@ class LIS2MDL_I2C : public device::I2C { public: - LIS2MDL_I2C(int bus, int bus_frequency); + LIS2MDL_I2C(const I2CSPIDriverConfig &config); virtual ~LIS2MDL_I2C() = default; virtual int read(unsigned address, void *data, unsigned count); @@ -68,16 +68,16 @@ protected: }; device::Device * -LIS2MDL_I2C_interface(int bus, int bus_frequency); +LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config); device::Device * -LIS2MDL_I2C_interface(int bus, int bus_frequency) +LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config) { - return new LIS2MDL_I2C(bus, bus_frequency); + return new LIS2MDL_I2C(config); } -LIS2MDL_I2C::LIS2MDL_I2C(int bus, int bus_frequency) : - I2C(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_I2C", bus, LIS2MDLL_ADDRESS, bus_frequency) +LIS2MDL_I2C::LIS2MDL_I2C(const I2CSPIDriverConfig &config) : + I2C(config) { } diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp index 11f8c8f3d4..d04435b50f 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp @@ -46,10 +46,10 @@ I2CSPIDriverBase *LIS2MDL::instantiate(const I2CSPIDriverConfig &config, int run device::Device *interface = nullptr; if (config.bus_type == BOARD_I2C_BUS) { - interface = LIS2MDL_I2C_interface(config.bus, config.bus_frequency); + interface = LIS2MDL_I2C_interface(config); } else if (config.bus_type == BOARD_SPI_BUS) { - interface = LIS2MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); + interface = LIS2MDL_SPI_interface(config); } if (interface == nullptr) { @@ -94,6 +94,7 @@ extern "C" int lis2mdl_main(int argc, char *argv[]) using ThisDriver = LIS2MDL; int ch; BusCLIArguments cli{true, true}; + cli.i2c_address = LIS2MDLL_ADDRESS; cli.default_i2c_frequency = 400000; cli.default_spi_frequency = 11 * 1000 * 1000; @@ -112,8 +113,6 @@ extern "C" int lis2mdl_main(int argc, char *argv[]) return -1; } - cli.i2c_address = LIS2MDLL_ADDRESS; - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS2MDL); if (!strcmp(verb, "start")) { diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp index 54e5a24b61..b017ad4afe 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp @@ -61,7 +61,7 @@ class LIS2MDL_SPI : public device::SPI { public: - LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); + LIS2MDL_SPI(const I2CSPIDriverConfig &config); virtual ~LIS2MDL_SPI() = default; virtual int init(); @@ -70,16 +70,16 @@ public: }; device::Device * -LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); +LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config); device::Device * -LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) +LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config) { - return new LIS2MDL_SPI(bus, devid, bus_frequency, spi_mode); + return new LIS2MDL_SPI(config); } -LIS2MDL_SPI::LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) : - SPI(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_SPI", bus, devid, spi_mode, bus_frequency) +LIS2MDL_SPI::LIS2MDL_SPI(const I2CSPIDriverConfig &config) : + SPI(config) { } diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.h b/src/drivers/magnetometer/lis3mdl/lis3mdl.h index f36d99972d..cff8c9466f 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.h +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.h @@ -84,8 +84,8 @@ #define CNTL_REG5_DEFAULT 0x00 /* interface factories */ -extern device::Device *LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); -extern device::Device *LIS3MDL_I2C_interface(int bus, int bus_frequency); +extern device::Device *LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config); +extern device::Device *LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config); enum OPERATING_MODE { CONTINUOUS = 0, diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp index c85c03b752..a0957cff3c 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp @@ -56,7 +56,7 @@ class LIS3MDL_I2C : public device::I2C { public: - LIS3MDL_I2C(int bus, int bus_frequency); + LIS3MDL_I2C(const I2CSPIDriverConfig &config); virtual ~LIS3MDL_I2C() = default; virtual int read(unsigned address, void *data, unsigned count); @@ -68,16 +68,16 @@ protected: }; device::Device * -LIS3MDL_I2C_interface(int bus, int bus_frequency); +LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config); device::Device * -LIS3MDL_I2C_interface(int bus, int bus_frequency) +LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config) { - return new LIS3MDL_I2C(bus, bus_frequency); + return new LIS3MDL_I2C(config); } -LIS3MDL_I2C::LIS3MDL_I2C(int bus, int bus_frequency) : - I2C(DRV_MAG_DEVTYPE_LIS3MDL, MODULE_NAME, bus, LIS3MDLL_ADDRESS, bus_frequency) +LIS3MDL_I2C::LIS3MDL_I2C(const I2CSPIDriverConfig &config) : + I2C(config) { } diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp index edd1f280f3..7bdb49908a 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp @@ -46,10 +46,10 @@ I2CSPIDriverBase *LIS3MDL::instantiate(const I2CSPIDriverConfig &config, int run device::Device *interface = nullptr; if (config.bus_type == BOARD_I2C_BUS) { - interface = LIS3MDL_I2C_interface(config.bus, config.bus_frequency); + interface = LIS3MDL_I2C_interface(config); } else if (config.bus_type == BOARD_SPI_BUS) { - interface = LIS3MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); + interface = LIS3MDL_SPI_interface(config); } if (interface == nullptr) { @@ -90,6 +90,7 @@ void LIS3MDL::print_usage() PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x1e); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_COMMAND("reset"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); @@ -100,6 +101,7 @@ extern "C" int lis3mdl_main(int argc, char *argv[]) using ThisDriver = LIS3MDL; int ch; BusCLIArguments cli{true, true}; + cli.i2c_address = LIS3MDLL_ADDRESS; cli.default_i2c_frequency = 400000; cli.default_spi_frequency = 11 * 1000 * 1000; @@ -118,8 +120,6 @@ extern "C" int lis3mdl_main(int argc, char *argv[]) return -1; } - cli.i2c_address = LIS3MDLL_ADDRESS; - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS3MDL); if (!strcmp(verb, "start")) { diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp index af9f99d776..9646259b63 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp @@ -61,7 +61,7 @@ class LIS3MDL_SPI : public device::SPI { public: - LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); + LIS3MDL_SPI(const I2CSPIDriverConfig &config); virtual ~LIS3MDL_SPI() = default; virtual int init(); @@ -70,16 +70,16 @@ public: }; device::Device * -LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); +LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config); device::Device * -LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) +LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config) { - return new LIS3MDL_SPI(bus, devid, bus_frequency, spi_mode); + return new LIS3MDL_SPI(config); } -LIS3MDL_SPI::LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) : - SPI(DRV_MAG_DEVTYPE_LIS3MDL, MODULE_NAME, bus, devid, spi_mode, bus_frequency) +LIS3MDL_SPI::LIS3MDL_SPI(const I2CSPIDriverConfig &config) : + SPI(config) { } From 70975183732a01e1d20c0daafe07ad175bcaaaeb Mon Sep 17 00:00:00 2001 From: tanja Date: Mon, 31 Oct 2022 13:39:36 +0100 Subject: [PATCH 13/43] px_uploader: Allow for multiple firmware files --- Tools/px_uploader.py | 46 ++++++++++++++++++++++++++++++-------------- 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 88c4d26a33..d479be3148 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -582,7 +582,35 @@ class uploader(object): self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) # upload the firmware - def upload(self, fw, force=False, boot_delay=None): + def upload(self, fw_list, force=False, boot_delay=None, boot_check=False): + # select correct binary + found_suitable_firmware = False + for file in fw_list: + fw = firmware(file) + if self.board_type == fw.property('board_id'): + if len(fw_list) > 1: print("using firmware binary {}".format(file)) + found_suitable_firmware = True + break + + if not found_suitable_firmware: + msg = "Firmware not suitable for this board (Firmware board_type=%u board_id=%u)" % ( + self.board_type, fw.property('board_id')) + print("WARNING: %s" % msg) + if force: + if len(fw_list) > 1: + raise FirmwareNotSuitableException("force flashing failed, more than one file provided, none suitable") + print("FORCED WRITE, FLASHING ANYWAY!") + else: + raise FirmwareNotSuitableException(msg) + + percent = fw.property('image_size') / fw.property('image_maxsize') + binary_size = float(fw.property('image_size')) + binary_max_size = float(fw.property('image_maxsize')) + percent = (binary_size / binary_max_size) * 100 + + print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%) " % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent)) + print() + # Make sure we are doing the right thing start = _time() if self.board_type != fw.property('board_id'): @@ -764,7 +792,7 @@ def main(): parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading') parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') parser.add_argument('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot') - parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") + parser.add_argument('firmware', action="store", nargs='+', help="Firmware file(s)") args = parser.parse_args() if args.use_protocol_splitter_format: @@ -776,17 +804,7 @@ def main(): print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)") print("==========================================================================================================") - # Load the firmware file - fw = firmware(args.firmware) - - percent = fw.property('image_size') / fw.property('image_maxsize') - binary_size = float(fw.property('image_size')) - binary_max_size = float(fw.property('image_maxsize')) - percent = (binary_size / binary_max_size) * 100 - - print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%), waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent)) - print() - + print("Waiting for bootloader...") # tell any GCS that might be connected to the autopilot to give up # control of the serial port @@ -889,7 +907,7 @@ def main(): try: # ok, we have a bootloader, try flashing it - up.upload(fw, force=args.force, boot_delay=args.boot_delay) + up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay) # if we made this far without raising exceptions, the upload was successful successful = True From bace45ba8dea6d1af0c94f70162d7bbdb504641c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 14 Oct 2022 12:11:21 +0200 Subject: [PATCH 14/43] LandDetector: log rotational_movement Signed-off-by: Silvan Fuhrer --- msg/VehicleLandDetected.msg | 1 + src/modules/land_detector/LandDetector.cpp | 2 ++ src/modules/land_detector/LandDetector.h | 1 + .../land_detector/MulticopterLandDetector.cpp | 17 +++++++++-------- .../land_detector/MulticopterLandDetector.h | 2 ++ 5 files changed, 15 insertions(+), 8 deletions(-) diff --git a/msg/VehicleLandDetected.msg b/msg/VehicleLandDetected.msg index a35a8414c3..fc0ca4a6d8 100644 --- a/msg/VehicleLandDetected.msg +++ b/msg/VehicleLandDetected.msg @@ -12,6 +12,7 @@ bool has_low_throttle bool vertical_movement bool horizontal_movement +bool rotational_movement bool close_to_ground_or_skipped_check diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index f16e8ed383..4ae554f1b8 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -57,6 +57,7 @@ LandDetector::LandDetector() : _land_detected.has_low_throttle = false; _land_detected.vertical_movement = false; _land_detected.horizontal_movement = false; + _land_detected.rotational_movement = false; _land_detected.close_to_ground_or_skipped_check = true; _land_detected.at_rest = true; } @@ -174,6 +175,7 @@ void LandDetector::Run() _land_detected.has_low_throttle = _get_has_low_throttle(); _land_detected.horizontal_movement = _get_horizontal_movement(); _land_detected.vertical_movement = _get_vertical_movement(); + _land_detected.rotational_movement = _get_rotational_movement(); _land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check(); _land_detected.at_rest = at_rest; _land_detected.timestamp = hrt_absolute_time(); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 26e74eacd4..6c8247b84e 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -138,6 +138,7 @@ protected: virtual bool _get_has_low_throttle() { return false; } virtual bool _get_horizontal_movement() { return false; } virtual bool _get_vertical_movement() { return false; } + virtual bool _get_rotational_movement() { return false; } virtual bool _get_close_to_ground_or_skipped_check() { return false; } virtual void _set_hysteresis_factor(const int factor) = 0; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index ab569a20d7..e3c0e23122 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -278,21 +278,22 @@ bool MulticopterLandDetector::_get_maybe_landed_state() return false; } + // Next look if vehicle is not rotating (do not consider yaw) + float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get()); - float landThresholdFactor = 1.f; - - // Widen acceptance thresholds for landed state right after landed + // Widen max rotation thresholds for landed state right after landed if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { - landThresholdFactor = 2.5f; + max_rotation_threshold *= 2.5f; } - // Next look if all rotation angles are not moving. - const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor; - matrix::Vector2f angular_velocity{_angular_velocity(0), _angular_velocity(1)}; - if (angular_velocity.norm() > max_rotation_scaled) { + if (angular_velocity.norm() > max_rotation_threshold) { + _rotational_movement = true; return false; + + } else { + _rotational_movement = false; } // If vertical velocity is available: ground contact, no thrust, no movement -> landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 1bbd7c7315..1d35b98d92 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -74,6 +74,7 @@ protected: bool _get_has_low_throttle() override { return _has_low_throttle; } bool _get_horizontal_movement() override { return _horizontal_movement; } bool _get_vertical_movement() override { return _vertical_movement; } + bool _get_rotational_movement() override { return _rotational_movement; } bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; } void _set_hysteresis_factor(const int factor) override; @@ -131,6 +132,7 @@ private: bool _in_descend{false}; ///< vehicle is commanded to desend bool _horizontal_movement{false}; ///< vehicle is moving horizontally bool _vertical_movement{false}; + bool _rotational_movement{false}; bool _has_low_throttle{false}; bool _close_to_ground_or_skipped_check{false}; bool _below_gnd_effect_hgt{false}; ///< vehicle height above ground is below height where ground effect occurs From 263c7923d6a252a270b735a5a49bdd02548bffc8 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 14 Oct 2022 12:48:58 +0200 Subject: [PATCH 15/43] MPC params: MPC_LAN_CRWL: fix description and reduce min Signed-off-by: Silvan Fuhrer --- src/modules/mc_pos_control/mc_pos_control_params.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index df0fb22d45..36769f03c0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -448,10 +448,11 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); /** - * Land crawl descend rate. Used below + * Land crawl descend rate. Used below MPC_LAND_ALT3 if distance + * sensor data is availabe. * * @unit m/s - * @min 0.3 + * @min 0.1 * @decimal 1 * @group Multicopter Position Control */ From 27309a45cc9760e62dddb3ff154e9935050ad00e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 17 Oct 2022 09:47:05 +0200 Subject: [PATCH 16/43] MC LandDetector: remove dependency on MPC_LAND_SPEED and MPC_LAND_CRWL Don't consider these params for vertical speed threshold, and instead increase the default for LNDMC_Z_VEL_MAX and use solely that one. Makes the land detector outcome more predictable and its interal logic simpler, while the new default tuning is resulting in about the same vz threshold as before. Signed-off-by: Silvan Fuhrer --- .../land_detector/MulticopterLandDetector.cpp | 25 ++++--------------- .../land_detector/MulticopterLandDetector.h | 6 +---- .../land_detector/land_detector_params_mc.c | 6 +++-- .../mc_pos_control/mc_pos_control_params.c | 5 ++-- 4 files changed, 13 insertions(+), 29 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index e3c0e23122..c329b684c0 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 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 @@ -75,12 +75,10 @@ namespace land_detector MulticopterLandDetector::MulticopterLandDetector() { - _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); _paramHandle.minThrottle = param_find("MPC_THR_MIN"); _paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE"); _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); - _paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL"); } void MulticopterLandDetector::_update_topics() @@ -119,16 +117,6 @@ void MulticopterLandDetector::_update_params() { param_get(_paramHandle.minThrottle, &_params.minThrottle); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); - param_get(_paramHandle.landSpeed, &_params.landSpeed); - param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed); - - if (_param_lndmc_z_vel_max.get() > _params.landSpeed) { - PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_SPEED, updating %.3f -> %.3f", - (double)_param_lndmc_z_vel_max.get(), (double)_params.landSpeed); - - _param_lndmc_z_vel_max.set(_params.landSpeed); - _param_lndmc_z_vel_max.commit_no_notification(); - } int32_t use_hover_thrust_estimate = 0; param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate); @@ -158,22 +146,19 @@ bool MulticopterLandDetector::_get_ground_contact_state() const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s); - // land speed threshold, 90% of MPC_LAND_SPEED - const float crawl_speed_threshold = 0.9f * math::max(_params.crawlSpeed, 0.1f); - if (lpos_available && _vehicle_local_position.v_z_valid) { // Check if we are moving vertically - this might see a spike after arming due to // throttle-up vibration. If accelerating fast the throttle thresholds will still give // an accurate in-air indication. - float max_climb_rate = math::min(crawl_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get()); + float max_vertical_velocity = _param_lndmc_z_vel_max.get(); if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { // Widen acceptance thresholds for landed state right after arming // so that motor spool-up and other effects do not trigger false negatives. - max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f; + max_vertical_velocity *= 2.5f; } - _vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate); + _vertical_movement = (fabsf(_vehicle_local_position.vz) > max_vertical_velocity); } else { _vertical_movement = true; @@ -217,7 +202,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { // Setpoints can be NAN _in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2]) - && (trajectory_setpoint.velocity[2] >= crawl_speed_threshold); + && (trajectory_setpoint.velocity[2] > FLT_EPSILON); } // ground contact requires commanded descent until landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 1d35b98d92..6d73451cd0 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 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 @@ -95,8 +95,6 @@ private: param_t minThrottle; param_t hoverThrottle; param_t minManThrottle; - param_t landSpeed; - param_t crawlSpeed; param_t useHoverThrustEstimate; } _paramHandle{}; @@ -104,8 +102,6 @@ private: float minThrottle; float hoverThrottle; float minManThrottle; - float landSpeed; - float crawlSpeed; bool useHoverThrustEstimate; } _params{}; diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 3c979ac767..d3db8a38c8 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -50,14 +50,16 @@ PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f); /** * Multicopter max climb rate * - * Maximum vertical velocity allowed in the landed state + * Maximum vertical velocity allowed in the landed state. + * Should be set lower than MPC_LAND_SPEED (and MPC_LAND_CRWL + * if distance sensor is present). * * @unit m/s * @decimal 1 * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.50f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.25f); /** * Multicopter max horizontal velocity diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 36769f03c0..3006995d92 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -448,8 +448,9 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); /** - * Land crawl descend rate. Used below MPC_LAND_ALT3 if distance - * sensor data is availabe. + * Land crawl descend rate + * + * Used below MPC_LAND_ALT3 if distance sensor data is availabe. * * @unit m/s * @min 0.1 From 91adb4c9e079a5e93c9ad64bc6af84d43c9b3b01 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 17 Oct 2022 11:03:00 +0200 Subject: [PATCH 17/43] MC LandDetector: widen thresholds for vz and rotational movment always in maybe landed and 2s after Signed-off-by: Silvan Fuhrer --- .../land_detector/MulticopterLandDetector.cpp | 27 +++++++++---------- .../land_detector/MulticopterLandDetector.h | 5 ++-- 2 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index c329b684c0..3207a961b5 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -147,14 +147,15 @@ bool MulticopterLandDetector::_get_ground_contact_state() const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s); if (lpos_available && _vehicle_local_position.v_z_valid) { - // Check if we are moving vertically - this might see a spike after arming due to - // throttle-up vibration. If accelerating fast the throttle thresholds will still give - // an accurate in-air indication. + // Check if we are moving vertically. + // Use wider threshold if currently in "maybe landed" state, or time since then is less + // than LAND_DETECTOR_TAKEOFF_PHASE_TIME_US, as estimation for + // vertical speed is often deteriorated when on the ground or due to propeller + // up/down throttling. + float max_vertical_velocity = _param_lndmc_z_vel_max.get(); - if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { - // Widen acceptance thresholds for landed state right after arming - // so that motor spool-up and other effects do not trigger false negatives. + if ((time_now_us - _last_time_maybe_landed) < LAND_DETECTOR_TAKEOFF_PHASE_TIME_US) { max_vertical_velocity *= 2.5f; } @@ -266,8 +267,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state() // Next look if vehicle is not rotating (do not consider yaw) float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get()); - // Widen max rotation thresholds for landed state right after landed - if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { + // Widen max rotation thresholds if either in maybe landed state or less than + // LAND_DETECTOR_TAKEOFF_PHASE_TIME_US passed since then. + if ((time_now_us - _last_time_maybe_landed) < LAND_DETECTOR_TAKEOFF_PHASE_TIME_US) { max_rotation_threshold *= 2.5f; } @@ -292,12 +294,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state() bool MulticopterLandDetector::_get_landed_state() { - // reset the landed_time - if (!_maybe_landed_hysteresis.get_state()) { - _landed_time = 0; - - } else if (_landed_time == 0) { - _landed_time = hrt_absolute_time(); + // update _last_time_maybe_landed as long as the vehicle is maybe landed + if (_maybe_landed_hysteresis.get_state()) { + _last_time_maybe_landed = hrt_absolute_time(); } // When not armed, consider to be landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 6d73451cd0..c088ba954b 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -84,8 +84,8 @@ private: /** Time in us that freefall has to hold before triggering freefall */ static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms; - /** Time interval in us in which wider acceptance thresholds are used after landed. */ - static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s; + /** Time interval in us in which wider acceptance thresholds are used after the "maybe landed" is cleared before takeoff. */ + static constexpr hrt_abstime LAND_DETECTOR_TAKEOFF_PHASE_TIME_US = 2_s; /** Distance above ground below which entering ground contact state is possible when distance to ground is available. */ static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f; @@ -123,7 +123,6 @@ private: uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED}; hrt_abstime _min_thrust_start{0}; ///< timestamp when minimum trust was applied first - hrt_abstime _landed_time{0}; bool _in_descend{false}; ///< vehicle is commanded to desend bool _horizontal_movement{false}; ///< vehicle is moving horizontally From 4e744739324d4b75d3240c6381cb0b9da9bd7f1d Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 17 Oct 2022 11:19:19 +0200 Subject: [PATCH 18/43] MC LandDetector: remove 2s phase after not maybe landed to still increase thresholds I don't see where this is necessary. During takeoff, the maybe landed flag should only get cleared once system is about to takeoff, and thus well after the spool up is complete (for which the higher thresholds are meant in this case). Signed-off-by: Silvan Fuhrer --- .../land_detector/MulticopterLandDetector.cpp | 16 +++++----------- .../land_detector/MulticopterLandDetector.h | 3 --- 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 3207a961b5..cba3d049d5 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -148,14 +148,13 @@ bool MulticopterLandDetector::_get_ground_contact_state() if (lpos_available && _vehicle_local_position.v_z_valid) { // Check if we are moving vertically. - // Use wider threshold if currently in "maybe landed" state, or time since then is less - // than LAND_DETECTOR_TAKEOFF_PHASE_TIME_US, as estimation for + // Use wider threshold if currently in "maybe landed" state, as estimation for // vertical speed is often deteriorated when on the ground or due to propeller // up/down throttling. float max_vertical_velocity = _param_lndmc_z_vel_max.get(); - if ((time_now_us - _last_time_maybe_landed) < LAND_DETECTOR_TAKEOFF_PHASE_TIME_US) { + if (_maybe_landed_hysteresis.get_state()) { max_vertical_velocity *= 2.5f; } @@ -267,9 +266,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state() // Next look if vehicle is not rotating (do not consider yaw) float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get()); - // Widen max rotation thresholds if either in maybe landed state or less than - // LAND_DETECTOR_TAKEOFF_PHASE_TIME_US passed since then. - if ((time_now_us - _last_time_maybe_landed) < LAND_DETECTOR_TAKEOFF_PHASE_TIME_US) { + // Widen max rotation thresholds if either in maybe landed state, thus making it harder + // to trigger a false positive !maybe_landed e.g. due to propeller throttling up/down. + if (_maybe_landed_hysteresis.get_state()) { max_rotation_threshold *= 2.5f; } @@ -294,11 +293,6 @@ bool MulticopterLandDetector::_get_maybe_landed_state() bool MulticopterLandDetector::_get_landed_state() { - // update _last_time_maybe_landed as long as the vehicle is maybe landed - if (_maybe_landed_hysteresis.get_state()) { - _last_time_maybe_landed = hrt_absolute_time(); - } - // When not armed, consider to be landed if (!_armed) { return true; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index c088ba954b..1e473eff83 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -84,9 +84,6 @@ private: /** Time in us that freefall has to hold before triggering freefall */ static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms; - /** Time interval in us in which wider acceptance thresholds are used after the "maybe landed" is cleared before takeoff. */ - static constexpr hrt_abstime LAND_DETECTOR_TAKEOFF_PHASE_TIME_US = 2_s; - /** Distance above ground below which entering ground contact state is possible when distance to ground is available. */ static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f; From 06bf60672bb4aba2e323a8de8c076c8adb8f7e9e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 17 Oct 2022 11:49:04 +0200 Subject: [PATCH 19/43] MC LandDetector: add constant (0.3) for vz threshold for in_descend flag Signed-off-by: Silvan Fuhrer --- src/modules/land_detector/MulticopterLandDetector.cpp | 2 +- src/modules/land_detector/MulticopterLandDetector.h | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index cba3d049d5..99a6638c20 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -202,7 +202,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { // Setpoints can be NAN _in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2]) - && (trajectory_setpoint.velocity[2] > FLT_EPSILON); + && (trajectory_setpoint.velocity[2] > DESCENT_TRAJECTORY_VZ_THRESHOLD); } // ground contact requires commanded descent until landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 1e473eff83..6493ec4261 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -87,6 +87,9 @@ private: /** Distance above ground below which entering ground contact state is possible when distance to ground is available. */ static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f; + /** Down velocity threshold for setting "in_descend" flag */ + static constexpr float DESCENT_TRAJECTORY_VZ_THRESHOLD = 0.3f; + /** Handles for interesting parameters. **/ struct { param_t minThrottle; From d9764f2ef4c1955c63ee4c80f98e891e9b1c6de2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 25 Oct 2022 00:00:34 +0200 Subject: [PATCH 20/43] MulticopterLandDetector: rename vertical velocity threshold variable --- src/modules/land_detector/MulticopterLandDetector.cpp | 6 +++--- src/modules/land_detector/land_detector_params_mc.c | 9 +++++---- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 99a6638c20..c021e78164 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -152,13 +152,13 @@ bool MulticopterLandDetector::_get_ground_contact_state() // vertical speed is often deteriorated when on the ground or due to propeller // up/down throttling. - float max_vertical_velocity = _param_lndmc_z_vel_max.get(); + float vertical_velocity_threshold = _param_lndmc_z_vel_max.get(); if (_maybe_landed_hysteresis.get_state()) { - max_vertical_velocity *= 2.5f; + vertical_velocity_threshold *= 2.5f; } - _vertical_movement = (fabsf(_vehicle_local_position.vz) > max_vertical_velocity); + _vertical_movement = (fabsf(_vehicle_local_position.vz) > vertical_velocity_threshold); } else { _vertical_movement = true; diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index d3db8a38c8..889085e79e 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -48,11 +48,12 @@ PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f); /** - * Multicopter max climb rate + * Multicopter vertical velocity threshold * - * Maximum vertical velocity allowed in the landed state. - * Should be set lower than MPC_LAND_SPEED (and MPC_LAND_CRWL - * if distance sensor is present). + * Vertical velocity threshold to detect landing. + * Should be set lower than the expected minimal speed for landing + * so MPC_LAND_SPEED for autonomous landing and MPC_LAND_CRWL + * if distance sensor is present and slowing down close to ground. * * @unit m/s * @decimal 1 From 509c37c373f819e5fcbc2a4acbf62430091e8828 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 25 Oct 2022 00:03:46 +0200 Subject: [PATCH 21/43] MulticopterLandDetector: use quick access for xy angular velocity components --- src/modules/land_detector/MulticopterLandDetector.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index c021e78164..28b739e557 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -272,9 +272,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state() max_rotation_threshold *= 2.5f; } - matrix::Vector2f angular_velocity{_angular_velocity(0), _angular_velocity(1)}; - - if (angular_velocity.norm() > max_rotation_threshold) { + if (_angular_velocity.xy().norm() > max_rotation_threshold) { _rotational_movement = true; return false; From ac646d32e6f9e4ff687c1f7ec32597d40e702294 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 25 Oct 2022 00:02:02 +0200 Subject: [PATCH 22/43] MulticopterLandDetector: Apply threshold widening only when landed, not maybe landed --- src/modules/land_detector/MulticopterLandDetector.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 28b739e557..20c0f38623 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -154,7 +154,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() float vertical_velocity_threshold = _param_lndmc_z_vel_max.get(); - if (_maybe_landed_hysteresis.get_state()) { + if (_landed_hysteresis.get_state()) { vertical_velocity_threshold *= 2.5f; } @@ -266,9 +266,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state() // Next look if vehicle is not rotating (do not consider yaw) float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get()); - // Widen max rotation thresholds if either in maybe landed state, thus making it harder - // to trigger a false positive !maybe_landed e.g. due to propeller throttling up/down. - if (_maybe_landed_hysteresis.get_state()) { + // Widen max rotation thresholds if either in landed state, thus making it harder + // to trigger a false positive !landed e.g. due to propeller throttling up/down. + if (_landed_hysteresis.get_state()) { max_rotation_threshold *= 2.5f; } From 7667883385865680d5c1687ca0e61e708ca1cee5 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 25 Oct 2022 00:09:40 +0200 Subject: [PATCH 23/43] MulticopterLandDetector: make in descend detection depend on vertical speed threshold It's very important that the in descend detection is always at a strictly higher velocity than the vertical movement check. This combination is basically used to check for vertical downwards velocity tracking. Desired descend, no movement -> ground If in descend threshold is lower than vertical movement it is by definition even with perfect tracking the case that with any velocity between the two thresholds there is no movement even though a descend is commanded. See first fix of this problem #7831 e39b38ba96971245aaf6d2b1c249868c8717665e --- src/modules/land_detector/MulticopterLandDetector.cpp | 2 +- src/modules/land_detector/MulticopterLandDetector.h | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 20c0f38623..6741e45c4f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -202,7 +202,7 @@ bool MulticopterLandDetector::_get_ground_contact_state() if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) { // Setpoints can be NAN _in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2]) - && (trajectory_setpoint.velocity[2] > DESCENT_TRAJECTORY_VZ_THRESHOLD); + && (trajectory_setpoint.velocity[2] >= 1.1f * _param_lndmc_z_vel_max.get()); } // ground contact requires commanded descent until landed diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 6493ec4261..1e473eff83 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -87,9 +87,6 @@ private: /** Distance above ground below which entering ground contact state is possible when distance to ground is available. */ static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f; - /** Down velocity threshold for setting "in_descend" flag */ - static constexpr float DESCENT_TRAJECTORY_VZ_THRESHOLD = 0.3f; - /** Handles for interesting parameters. **/ struct { param_t minThrottle; From a2d01995163eac503b1d1121c177b44a1c3cb35b Mon Sep 17 00:00:00 2001 From: Zachary Lowell Date: Tue, 1 Nov 2022 17:35:20 -0500 Subject: [PATCH 24/43] Uncommenting Qurt platform task code (#20533) --- platforms/qurt/src/px4/tasks.cpp | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/platforms/qurt/src/px4/tasks.cpp b/platforms/qurt/src/px4/tasks.cpp index 2dd044da44..de6c685233 100644 --- a/platforms/qurt/src/px4/tasks.cpp +++ b/platforms/qurt/src/px4/tasks.cpp @@ -35,8 +35,11 @@ #include #include #include +#include +#include #include +#include "hrt_work.h" #define PX4_TASK_STACK_SIZE 8192 #define PX4_TASK_MAX_NAME_LENGTH 32 @@ -338,25 +341,25 @@ const char *px4_get_taskname() } -// static void timer_cb(void *data) -// { -// px4_sem_t *sem = reinterpret_cast(data); +static void timer_cb(void *data) +{ + px4_sem_t *sem = reinterpret_cast(data); -// sem_post(sem); -// } + sem_post(sem); +} int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) { - // work_s _hpwork = {}; + work_s _hpwork = {}; - // struct timespec ts_now; - // px4_clock_gettime(CLOCK_MONOTONIC, &ts_now); + struct timespec ts_now; + px4_clock_gettime(CLOCK_MONOTONIC, &ts_now); - // hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now); + hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now); - // hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us); - // sem_wait(sem); - // hrt_work_cancel(&_hpwork); + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us); + sem_wait(sem); + hrt_work_cancel(&_hpwork); return 0; } From 2e20fb7f97f00f5d5db29c8a227f93f30c09b5a8 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 31 Oct 2022 09:00:13 +0100 Subject: [PATCH 25/43] ActuatorEffectiveness: add _collective_ keyword to controls for collective tilt to disinct from yaw tilt Signed-off-by: Silvan Fuhrer --- .../ActuatorEffectivenessRotors.cpp | 11 ++++--- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 32 +++++++++---------- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 4 +-- 3 files changed, 24 insertions(+), 23 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index 65b987aba4..2afb41b002 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -233,10 +233,11 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry return num_actuators; } -uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control) +uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, + float collective_tilt_control) { - if (!PX4_ISFINITE(tilt_control)) { - tilt_control = -1.f; + if (!PX4_ISFINITE(collective_tilt_control)) { + collective_tilt_control = -1.f; } uint32_t nontilted_motors = 0; @@ -250,8 +251,8 @@ uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectiv } const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index); - float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (tilt_control + 1.f) / 2.f); - float tilt_direction = math::radians((float)tilt.tilt_direction); + const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f); + const float tilt_direction = math::radians((float)tilt.tilt_direction); _geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction); } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 9379b83786..6f5301a7ef 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -54,7 +54,7 @@ bool ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { - if (!_combined_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + if (!_collective_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { return false; } @@ -66,9 +66,9 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config // Update matrix with tilts in vertical position when update is triggered by a manual // configuration (parameter) change. This is to make sure the normalization // scales are tilt-invariant. Note: configuration updates are only possible when disarmed. - const float tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f : - _last_tilt_control; - _nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied) + const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? + -1.f : _last_collective_tilt_control; + _nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied) << configuration.num_actuators[(int)ActuatorType::MOTORS]; const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); @@ -86,7 +86,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config // If it was an update coming from a config change, then make sure to update matrix in // the next iteration again with the correct tilt (but without updating the normalization scale). - _combined_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE); + _collective_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE); return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully); } @@ -113,24 +113,24 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector 0.99f ? 1.f : control_tilt; + // set control_collective_tilt to exactly -1 or 1 if close to these end points + control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt; + control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt; - // initialize _last_tilt_control - if (!PX4_ISFINITE(_last_tilt_control)) { - _last_tilt_control = control_tilt; + // initialize _last_collective_tilt_control + if (!PX4_ISFINITE(_last_collective_tilt_control)) { + _last_collective_tilt_control = control_collective_tilt; - } else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) { - _combined_tilt_updated = true; - _last_tilt_control = control_tilt; + } else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) { + _collective_tilt_updated = true; + _last_collective_tilt_control = control_collective_tilt; } for (int i = 0; i < _tilts.count(); ++i) { if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) { - actuator_sp(i + _first_tilt_idx) += control_tilt; + actuator_sp(i + _first_tilt_idx) += control_collective_tilt; } } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 7f242915fe..d7d136727c 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -82,7 +82,7 @@ public: uint32_t getStoppedMotors() const override { return _stopped_motors; } protected: - bool _combined_tilt_updated{true}; + bool _collective_tilt_updated{true}; ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; ActuatorEffectivenessTilts _tilts; @@ -93,7 +93,7 @@ protected: int _first_control_surface_idx{0}; ///< applies to matrix 1 int _first_tilt_idx{0}; ///< applies to matrix 0 - float _last_tilt_control{NAN}; + float _last_collective_tilt_control{NAN}; uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)}; uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; From f44713e8a35d6c2770a295f2be7fc3f9ee545a41 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 31 Oct 2022 11:09:00 +0100 Subject: [PATCH 26/43] ControlAllocator: enable custom saturation logic to override default one Custom saturation logic currently implemented for Tiltrotor VTOL and Helicopter. Signed-off-by: Silvan Fuhrer --- .../ActuatorEffectiveness.hpp | 6 +-- .../ActuatorEffectivenessHelicopter.cpp | 15 +----- .../ActuatorEffectivenessHelicopter.hpp | 2 +- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 36 +++++++++++++ .../ActuatorEffectivenessTiltrotorVTOL.hpp | 10 ++++ .../control_allocator/ControlAllocator.cpp | 50 ++++++++++--------- 6 files changed, 78 insertions(+), 41 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 8a1f0df1f4..344c468a16 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -199,10 +199,10 @@ public: virtual uint32_t getStoppedMotors() const { return 0; } /** - * Fill in the allocated and unallocated torque and thrust. - * Should return false if not filled in and the effectivenes matrix should be used instead + * Fill in the allocated and unallocated torque and thrust, customized by effectiveness type. + * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead. */ - virtual bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const { return false; } + virtual void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 8cf7a9cb5d..a0ca1808d8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -217,48 +217,37 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit } } -bool ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const +void ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(int matrix_index, + control_allocator_status_s &status) { - status.torque_setpoint_achieved = true; - status.thrust_setpoint_achieved = true; // Note: the values '-1', '1' and '0' are just to indicate a negative, // positive or no saturation to the rate controller. The actual magnitude is not used. if (_saturation_flags.roll_pos) { status.unallocated_torque[0] = 1.f; - status.torque_setpoint_achieved = false; } else if (_saturation_flags.roll_neg) { status.unallocated_torque[0] = -1.f; - status.torque_setpoint_achieved = false; } if (_saturation_flags.pitch_pos) { status.unallocated_torque[1] = 1.f; - status.torque_setpoint_achieved = false; } else if (_saturation_flags.pitch_neg) { status.unallocated_torque[1] = -1.f; - status.torque_setpoint_achieved = false; } if (_saturation_flags.yaw_pos) { status.unallocated_torque[2] = 1.f; - status.torque_setpoint_achieved = false; } else if (_saturation_flags.yaw_neg) { status.unallocated_torque[2] = -1.f; - status.torque_setpoint_achieved = false; } if (_saturation_flags.thrust_pos) { status.unallocated_thrust[2] = 1.f; - status.thrust_setpoint_achieved = false; } else if (_saturation_flags.thrust_neg) { status.unallocated_thrust[2] = -1.f; - status.thrust_setpoint_achieved = false; } - - return true; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 64bf52138b..83d28d2330 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -79,7 +79,7 @@ public: ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, const matrix::Vector &actuator_max) override; - bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const override; + void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; private: float throttleSpoolupProgress(); bool mainMotorEnaged(); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 6f5301a7ef..aee4f78644 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -147,6 +147,21 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector 1.f) { + _yaw_tilt_saturation_flags.tilt_yaw_pos = true; + } + } } void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase) @@ -170,3 +185,24 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh break; } } + +void ActuatorEffectivenessTiltrotorVTOL::getAllocatedAndUnallocatedControl(int matrix_index, + control_allocator_status_s &status) +{ + // only handle matrix 0 (motors and tilts) + if (matrix_index == 1) { + return; + } + + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_yaw_tilt_saturation_flags.tilt_yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) { + status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index d7d136727c..7ad8477e02 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -81,6 +81,9 @@ public: const char *name() const override { return "VTOL Tiltrotor"; } uint32_t getStoppedMotors() const override { return _stopped_motors; } + + void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + protected: bool _collective_tilt_updated{true}; ActuatorEffectivenessRotors _mc_rotors; @@ -97,4 +100,11 @@ protected: uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)}; uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; + + struct YawTiltSaturationFlags { + bool tilt_yaw_pos; + bool tilt_yaw_neg; + }; + + YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index fcd4209cfa..dee3cb8e6a 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -589,33 +589,35 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) // TODO: disabled motors (?) - if (!_actuator_effectiveness->getAllocatedAndUnallocatedControl(control_allocator_status)) { - // Allocated control - const matrix::Vector &allocated_control = _control_allocation[matrix_index]->getAllocatedControl(); - control_allocator_status.allocated_torque[0] = allocated_control(0); - control_allocator_status.allocated_torque[1] = allocated_control(1); - control_allocator_status.allocated_torque[2] = allocated_control(2); - control_allocator_status.allocated_thrust[0] = allocated_control(3); - control_allocator_status.allocated_thrust[1] = allocated_control(4); - control_allocator_status.allocated_thrust[2] = allocated_control(5); + // Allocated control + const matrix::Vector &allocated_control = _control_allocation[matrix_index]->getAllocatedControl(); + control_allocator_status.allocated_torque[0] = allocated_control(0); + control_allocator_status.allocated_torque[1] = allocated_control(1); + control_allocator_status.allocated_torque[2] = allocated_control(2); + control_allocator_status.allocated_thrust[0] = allocated_control(3); + control_allocator_status.allocated_thrust[1] = allocated_control(4); + control_allocator_status.allocated_thrust[2] = allocated_control(5); - // Unallocated control - const matrix::Vector unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() - - allocated_control; - control_allocator_status.unallocated_torque[0] = unallocated_control(0); - control_allocator_status.unallocated_torque[1] = unallocated_control(1); - control_allocator_status.unallocated_torque[2] = unallocated_control(2); - control_allocator_status.unallocated_thrust[0] = unallocated_control(3); - control_allocator_status.unallocated_thrust[1] = unallocated_control(4); - control_allocator_status.unallocated_thrust[2] = unallocated_control(5); + // Unallocated control + const matrix::Vector unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() - + allocated_control; + control_allocator_status.unallocated_torque[0] = unallocated_control(0); + control_allocator_status.unallocated_torque[1] = unallocated_control(1); + control_allocator_status.unallocated_torque[2] = unallocated_control(2); + control_allocator_status.unallocated_thrust[0] = unallocated_control(3); + control_allocator_status.unallocated_thrust[1] = unallocated_control(4); + control_allocator_status.unallocated_thrust[2] = unallocated_control(5); - // Allocation success flags - control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1), - unallocated_control(2)).norm_squared() < 1e-6f); - control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4), - unallocated_control(5)).norm_squared() < 1e-6f); - } + // override control_allocator_status in customized saturation logic for certain effectiveness types + _actuator_effectiveness->getAllocatedAndUnallocatedControl(matrix_index, control_allocator_status); + // Allocation success flags + control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0], + control_allocator_status.unallocated_torque[1], + control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f); + control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0], + control_allocator_status.unallocated_thrust[1], + control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f); // Actuator saturation const matrix::Vector &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint(); From 83e906e2e9d9b5eb096ebb8eb7afebf32f7446ef Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 31 Oct 2022 12:20:47 +0100 Subject: [PATCH 27/43] Control_allocator_status.msg: remove allocated_ fields It's enough that the setpoints and the unallocated values are logged, from these the allocated values can be calculated if required. Signed-off-by: Silvan Fuhrer --- msg/ControlAllocatorStatus.msg | 2 -- .../ActuatorEffectiveness/ActuatorEffectiveness.hpp | 4 ++-- .../ActuatorEffectivenessHelicopter.cpp | 3 +-- .../ActuatorEffectivenessHelicopter.hpp | 2 +- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 3 +-- .../ActuatorEffectivenessTiltrotorVTOL.hpp | 2 +- src/modules/control_allocator/ControlAllocator.cpp | 8 +------- 7 files changed, 7 insertions(+), 17 deletions(-) diff --git a/msg/ControlAllocatorStatus.msg b/msg/ControlAllocatorStatus.msg index 3fbbe844ef..2d7b088322 100644 --- a/msg/ControlAllocatorStatus.msg +++ b/msg/ControlAllocatorStatus.msg @@ -1,12 +1,10 @@ uint64 timestamp # time since system start (microseconds) 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 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/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 344c468a16..952179c81f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -199,10 +199,10 @@ public: virtual uint32_t getStoppedMotors() const { return 0; } /** - * Fill in the allocated and unallocated torque and thrust, customized by effectiveness type. + * Fill in the unallocated torque and thrust, customized by effectiveness type. * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead. */ - virtual void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} + virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index a0ca1808d8..f5a7aaedbb 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -217,8 +217,7 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit } } -void ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(int matrix_index, - control_allocator_status_s &status) +void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) { // Note: the values '-1', '1' and '0' are just to indicate a negative, diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 83d28d2330..3cd64b2f36 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -79,7 +79,7 @@ public: ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, const matrix::Vector &actuator_max) override; - void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; private: float throttleSpoolupProgress(); bool mainMotorEnaged(); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index aee4f78644..bd9e1db0fc 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -186,8 +186,7 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh } } -void ActuatorEffectivenessTiltrotorVTOL::getAllocatedAndUnallocatedControl(int matrix_index, - control_allocator_status_s &status) +void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) { // only handle matrix 0 (motors and tilts) if (matrix_index == 1) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 7ad8477e02..5686d2ebda 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -82,7 +82,7 @@ public: uint32_t getStoppedMotors() const override { return _stopped_motors; } - void getAllocatedAndUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; protected: bool _collective_tilt_updated{true}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index dee3cb8e6a..c487b4bc4b 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -591,12 +591,6 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) // Allocated control const matrix::Vector &allocated_control = _control_allocation[matrix_index]->getAllocatedControl(); - control_allocator_status.allocated_torque[0] = allocated_control(0); - control_allocator_status.allocated_torque[1] = allocated_control(1); - control_allocator_status.allocated_torque[2] = allocated_control(2); - control_allocator_status.allocated_thrust[0] = allocated_control(3); - control_allocator_status.allocated_thrust[1] = allocated_control(4); - control_allocator_status.allocated_thrust[2] = allocated_control(5); // Unallocated control const matrix::Vector unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() - @@ -609,7 +603,7 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) control_allocator_status.unallocated_thrust[2] = unallocated_control(5); // override control_allocator_status in customized saturation logic for certain effectiveness types - _actuator_effectiveness->getAllocatedAndUnallocatedControl(matrix_index, control_allocator_status); + _actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status); // Allocation success flags control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0], From ab3fe543d420fd91ebdc00ef271983bdaf6fcf94 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 2 Nov 2022 09:29:28 +0100 Subject: [PATCH 28/43] libevents: update submodule --- src/lib/events/libevents | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/events/libevents b/src/lib/events/libevents index a89808bffd..179f86a8fc 160000 --- a/src/lib/events/libevents +++ b/src/lib/events/libevents @@ -1 +1 @@ -Subproject commit a89808bffd1efb0543e66f17a7fa12dfce5e6bf0 +Subproject commit 179f86a8fc7fd74cb80630e77b1b9435d3c5b748 From 0996e5319f6015c0f4bcccb6cc1c39098d735cc1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 2 Nov 2022 09:32:24 +0100 Subject: [PATCH 29/43] commander: add preflight check for hardfault files on SD card --- .../checks/sdcardCheck.cpp | 89 ++++++++++++++----- .../checks/sdcardCheck.hpp | 8 +- src/modules/commander/commander_params.c | 11 +++ 3 files changed, 87 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp index e056b80061..8be0c6961b 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp @@ -32,6 +32,8 @@ ****************************************************************************/ #include "sdcardCheck.hpp" +#include +#include #ifdef __PX4_DARWIN #include @@ -42,38 +44,85 @@ void SdCardChecks::checkAndReport(const Context &context, Report &reporter) { - if (_param_com_arm_sdcard.get() <= 0) { - return; - } + if (_param_com_arm_sdcard.get() > 0) { - struct statfs statfs_buf; + struct statfs statfs_buf; - if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) { - // on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted - _sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0); - } - - if (!_sdcard_detected) { - NavModes affected_modes{NavModes::None}; - - if (_param_com_arm_sdcard.get() == 2) { - // disallow arming without sd card - affected_modes = NavModes::All; + if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) { + // on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted + _sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0); } + if (!_sdcard_detected) { + NavModes affected_modes{NavModes::None}; + + if (_param_com_arm_sdcard.get() == 2) { + // disallow arming without sd card + affected_modes = NavModes::All; + } + + /* EVENT + * @description + * Insert an SD Card to the autopilot and reboot the system. + * + * + * This check can be configured via COM_ARM_SDCARD parameter. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::system, + events::ID("check_missing_fmu_sdcard"), + events::Log::Error, "Missing FMU SD Card"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card"); + } + } + } + +#ifdef __PX4_NUTTX + // Check for hardfault files + + if (!_hardfault_checked_once && _param_com_arm_hardfault_check.get()) { + _hardfault_checked_once = true; + + DIR *dp = opendir(PX4_STORAGEDIR); + + if (dp != nullptr) { + + struct dirent *result; + + while ((result = readdir(dp)) && !_hardfault_file_present) { + + // Check for pattern fault_*.log + if (strncmp("fault_", result->d_name, 6) == 0 && strcmp(result->d_name + strlen(result->d_name) - 4, ".log") == 0) { + _hardfault_file_present = true; + } + } + + closedir(dp); + } + } + + if (_hardfault_file_present && _param_com_arm_hardfault_check.get()) { /* EVENT * @description - * Insert an SD Card to the autopilot and reboot the system. + * The SD card contains crash dump files, service the vehicle before continuing to fly. * * - * This check can be configured via COM_ARM_SDCARD parameter. + * Check how to debug hardfaults on https://docs.px4.io/main/en/debug/gdb_debugging.html#hard-fault-debugging. + * When completed, remove the files 'fault_*.log' on the SD card. + * + * This check can be configured via COM_ARM_HFLT_CHK parameter. * */ - reporter.armingCheckFailure(affected_modes, health_component_t::system, events::ID("check_missing_fmu_sdcard"), - events::Log::Error, "Missing FMU SD Card"); + reporter.healthFailure(NavModes::All, health_component_t::system, + events::ID("check_hardfault_present"), + events::Log::Error, "Crash dumps present on SD card"); if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card"); + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD, vehicle needs service"); } } + +#endif /* __PX4_NUTTX */ } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp index 6870f648ad..5f46a6ff38 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp @@ -46,7 +46,13 @@ public: private: bool _sdcard_detected{false}; +#ifdef __PX4_NUTTX + bool _hardfault_checked_once {false}; + bool _hardfault_file_present{false}; +#endif + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamInt) _param_com_arm_sdcard + (ParamInt) _param_com_arm_sdcard, + (ParamBool) _param_com_arm_hardfault_check ) }; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 74a4e85b1f..3b779a7885 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1010,6 +1010,17 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1); */ PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1); +/** + * Enable FMU SD card hardfault detection check + * + * This check detects if there are hardfault files present on the + * SD card. If so, and the parameter is enabled, arming is prevented. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1); + /** * Enforced delay between arming and further navigation * From 298cc61e07d9900aac99bcf4e102d4d638957506 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 30 Oct 2022 21:38:52 -0400 Subject: [PATCH 30/43] ekf2: push fuse beta config into backend --- src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/control.cpp | 41 +++++++++++----------- src/modules/ekf2/EKF/estimator_interface.h | 3 -- src/modules/ekf2/EKF2.cpp | 4 +-- src/modules/ekf2/EKF2.hpp | 2 +- 5 files changed, 24 insertions(+), 27 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index dbc5b55680..e1ae0dc117 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -336,6 +336,7 @@ struct parameters { float arsp_thr{2.0f}; ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion // synthetic sideslip fusion + int32_t beta_fusion_enabled{0}; float beta_innov_gate{5.0f}; ///< synthetic sideslip innovation consistency gate size in standard deviation (STD) float beta_noise{0.3f}; ///< synthetic sideslip noise (rad) const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index fc259a51b1..0235dbdcd3 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -570,30 +570,31 @@ void Ekf::controlAirDataFusion() void Ekf::controlBetaFusion() { - if (_control_status.flags.fake_pos) { - return; - } + _control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing + && _control_status.flags.in_air && !_control_status.flags.fake_pos; - // Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally: - const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us); + if (_control_status.flags.fuse_beta) { - if (beta_fusion_time_triggered && - _control_status.flags.fuse_beta && - _control_status.flags.in_air) { - updateSideslip(_aid_src_sideslip); - _innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected; + // Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally: + const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us); - // If starting wind state estimation, reset the wind states and covariances before fusing any data - if (!_control_status.flags.wind) { - // activate the wind states - _control_status.flags.wind = true; - // reset the timeout timers to prevent repeated resets - _aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us; - resetWind(); - } + if (beta_fusion_time_triggered) { - if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) { - fuseSideslip(_aid_src_sideslip); + updateSideslip(_aid_src_sideslip); + _innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected; + + // If starting wind state estimation, reset the wind states and covariances before fusing any data + if (!_control_status.flags.wind) { + // activate the wind states + _control_status.flags.wind = true; + // reset the timeout timers to prevent repeated resets + _aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us; + resetWind(); + } + + if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) { + fuseSideslip(_aid_src_sideslip); + } } } } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 69cb788907..23bee11ca0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -132,9 +132,6 @@ public: // set vehicle is fixed wing status void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; } - // set flag if synthetic sideslip measurement should be fused - void set_fuse_beta_flag(bool fuse_beta) { _control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air); } - // set flag if static pressure rise due to ground effect is expected // use _params.gnd_effect_deadzone to adjust for expected rise in static pressure // flag will clear after GNDEFFECT_TIMEOUT uSec diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index b5ef696ad1..bf4de3456f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -145,6 +145,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_ev_pos_y(_params->ev_pos_body(1)), _param_ekf2_ev_pos_z(_params->ev_pos_body(2)), _param_ekf2_arsp_thr(_params->arsp_thr), + _param_ekf2_fuse_beta(_params->beta_fusion_enabled), _param_ekf2_tau_vel(_params->vel_Tau), _param_ekf2_tau_pos(_params->pos_Tau), _param_ekf2_gbias_init(_params->switch_on_gyro_bias), @@ -532,9 +533,6 @@ void EKF2::Run() if (_status_sub.copy(&vehicle_status)) { const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); - // only fuse synthetic sideslip measurements if conditions are met - _ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1)); - // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) _ekf.set_is_fixed_wing(is_fixed_wing); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 652e3f6b6f..08bf9d30b1 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -569,7 +569,7 @@ private: // control of airspeed and sideslip fusion (ParamExtFloat) _param_ekf2_arsp_thr, ///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec) - (ParamInt) + (ParamExtInt) _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables // output predictor filter time constants From 6511866408855b6803161f5f1a702eaeb1453acd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 2 Nov 2022 08:50:09 +0100 Subject: [PATCH 31/43] fix commander: check that offboard_control_mode data is recent This is a regression from https://github.com/PX4/PX4-Autopilot/pull/20172 --- .../checks/offboardCheck.cpp | 16 +++++----------- .../checks/offboardCheck.hpp | 4 +--- 2 files changed, 6 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp index 1609da7af4..3a3e9e3d50 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp @@ -35,11 +35,6 @@ using namespace time_literals; -OffboardChecks::OffboardChecks() -{ - _offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s); -} - void OffboardChecks::checkAndReport(const Context &context, Report &reporter) { reporter.failsafeFlags().offboard_control_signal_lost = true; @@ -47,9 +42,10 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter) offboard_control_mode_s offboard_control_mode; if (_offboard_control_mode_sub.copy(&offboard_control_mode)) { - bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity - || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate - || offboard_control_mode.actuator; + bool data_is_recent = hrt_absolute_time() < offboard_control_mode.timestamp + _param_com_of_loss_t.get() * 1_s; + bool offboard_available = (offboard_control_mode.position || offboard_control_mode.velocity + || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate + || offboard_control_mode.actuator) && data_is_recent; if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) { offboard_available = false; @@ -62,9 +58,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter) offboard_available = false; } - _offboard_available.set_state_and_update(offboard_available, hrt_absolute_time()); - // This is a mode requirement, no need to report - reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state(); + reporter.failsafeFlags().offboard_control_signal_lost = !offboard_available; } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp index 5d29bc75c2..6b15e24999 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp @@ -37,19 +37,17 @@ #include #include -#include class OffboardChecks : public HealthAndArmingCheckBase { public: - OffboardChecks(); + OffboardChecks() = default; ~OffboardChecks() = default; void checkAndReport(const Context &context, Report &reporter) override; private: uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; - systemlib::Hysteresis _offboard_available{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamFloat) _param_com_of_loss_t From 1fc1a81d8fc574001d120dff8516e92369bce1d1 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Wed, 26 Oct 2022 09:49:45 +0200 Subject: [PATCH 32/43] UCANS32K146 Add support for 2nd PWM --- boards/nxp/ucans32k146/src/board_config.h | 4 ++-- boards/nxp/ucans32k146/src/timer_config.cpp | 13 ++++++++++++- .../src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h | 4 ++-- 3 files changed, 16 insertions(+), 5 deletions(-) diff --git a/boards/nxp/ucans32k146/src/board_config.h b/boards/nxp/ucans32k146/src/board_config.h index 3e97c8d76b..ea591a9f03 100644 --- a/boards/nxp/ucans32k146/src/board_config.h +++ b/boards/nxp/ucans32k146/src/board_config.h @@ -99,14 +99,14 @@ __BEGIN_DECLS /* Timer I/O PWM and capture * - * ?? PWM outputs are configured. + * 2 PWM outputs are configured. * ?? Timer inputs are configured. * * Pins: * Defined in board.h */ -#define DIRECT_PWM_OUTPUT_CHANNELS 1 +#define DIRECT_PWM_OUTPUT_CHANNELS 2 #define BOARD_HAS_LED_PWM 1 diff --git a/boards/nxp/ucans32k146/src/timer_config.cpp b/boards/nxp/ucans32k146/src/timer_config.cpp index 104936e7a6..7106489b39 100644 --- a/boards/nxp/ucans32k146/src/timer_config.cpp +++ b/boards/nxp/ucans32k146/src/timer_config.cpp @@ -105,11 +105,13 @@ #define rPWMLOAD(t) REG(t,S32K1XX_FTM_PWMLOAD_OFFSET) constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::FTM1), initIOTimer(Timer::FTM2), }; constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::FTM2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::FTM1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin1}), }; constexpr io_timers_channel_mapping_t io_timers_channel_mapping = @@ -154,17 +156,26 @@ void ucans32k_timer_initialize(void) regval |= FTM_SC_CLKS_FTM; _REG(S32K1XX_FTM0_SC) = regval; + regval = _REG(S32K1XX_FTM1_SC); + regval &= ~(FTM_SC_CLKS_MASK); + regval |= FTM_SC_CLKS_FTM; + _REG(S32K1XX_FTM1_SC) = regval; + regval = _REG(S32K1XX_FTM2_SC); regval &= ~(FTM_SC_CLKS_MASK); regval |= FTM_SC_CLKS_FTM; _REG(S32K1XX_FTM2_SC) = regval; - /* Enabled System Clock Gating Control for FTM0, and FTM2 */ + /* Enabled System Clock Gating Control for FTM0, FTM1 and FTM2 */ regval = _REG(S32K1XX_PCC_FTM0); regval |= PCC_CGC; _REG(S32K1XX_PCC_FTM0) = regval; + regval = _REG(S32K1XX_PCC_FTM1); + regval |= PCC_CGC; + _REG(S32K1XX_PCC_FTM1) = regval; + regval = _REG(S32K1XX_PCC_FTM2); regval |= PCC_CGC; _REG(S32K1XX_PCC_FTM2) = regval; diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h index 71a026b5c5..c62ee30e58 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h @@ -45,8 +45,8 @@ #pragma once __BEGIN_DECLS /* configuration limits */ -#define MAX_IO_TIMERS 1 -#define MAX_TIMER_IO_CHANNELS 1 +#define MAX_IO_TIMERS 2 +#define MAX_TIMER_IO_CHANNELS 2 #define MAX_LED_TIMERS 1 #define MAX_TIMER_LED_CHANNELS 3 From a9542baf3c23f8c0f74b172ed239045996fb4a44 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Mon, 7 Nov 2022 15:29:14 +0100 Subject: [PATCH 33/43] Enable motor controls for tailsitter VTOLs in fixed wing mode (enable Quad Tailsitters) (#20511) * Enable motor controls for fixed wing mode in tailsitters This commit enable motor controls in fixed wing mode for tailsitters This is needed for enabling quad tailsitters * VTOL: differential thrust in FW: adapt params to be generic for all axes Until now only suppoted on yaw axis. Not to be supported also on Roll and Pitch. - VT_FW_DIFTHR_EN: make bitmask for all three axes independently. First bit is Yaw, sucht that existing meaning of VT_FW_DIFTHR_EN doesn't change. - VT_FW_DIFTHR_SC: rename to VT_FW_DIFTHR_S_Y and add same params for roll (_R) and pitch (_P). Signed-off-by: Silvan Fuhrer * Integrate differential control bits to three axis control Signed-off-by: Silvan Fuhrer --- .../init.d-posix/airframes/10042_xvert | 2 +- .../init.d-posix/airframes/1041_tailsitter | 2 +- .../airframes/1102_tailsitter_duo_sih.hil | 2 +- src/lib/parameters/param_translation.cpp | 9 ++++ src/modules/vtol_att_control/tailsitter.cpp | 19 +++++-- src/modules/vtol_att_control/tiltrotor.cpp | 6 +-- .../vtol_att_control_params.c | 50 ++++++++++++++++--- src/modules/vtol_att_control/vtol_type.h | 13 ++++- 8 files changed, 85 insertions(+), 18 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert index c87491c413..45c5d7ba73 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert @@ -14,7 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert} param set-default VT_ELEV_MC_LOCK 0 param set-default VT_TYPE 0 param set-default VT_FW_DIFTHR_EN 1 -param set-default VT_FW_DIFTHR_SC 0.3 +param set-default VT_FW_DIFTHR_S_Y 0.3 param set-default MPC_MAN_Y_MAX 60 param set-default MC_PITCH_P 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter index d72ea64da2..1b9bb77115 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter @@ -70,7 +70,7 @@ param set-default MPC_XY_VEL_D_ACC 0.1 param set-default NAV_ACC_RAD 5 param set-default VT_FW_DIFTHR_EN 1 -param set-default VT_FW_DIFTHR_SC 0.5 +param set-default VT_FW_DIFTHR_S_Y 0.5 param set-default VT_F_TRANS_DUR 1.5 param set-default VT_F_TRANS_THR 0.7 param set-default VT_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil index c883b1f2bb..9ba9822367 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil @@ -22,7 +22,7 @@ param set-default VT_ELEV_MC_LOCK 0 param set-default VT_MOT_COUNT 2 param set-default VT_TYPE 0 param set-default VT_FW_DIFTHR_EN 1 -param set-default VT_FW_DIFTHR_SC 0.3 +param set-default VT_FW_DIFTHR_S_Y 0.3 param set-default MPC_MAN_Y_MAX 60 param set-default MC_PITCH_P 5 diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 0bb35fbf58..e0c4b4d6ad 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -250,5 +250,14 @@ bool param_modify_on_import(bson_node_t node) } } + // 2022-07-18: translate VT_FW_DIFTHR_SC->VT_FW_DIFTHR_S_Y + { + if (strcmp("VT_FW_DIFTHR_SC", node->name) == 0) { + strcpy(node->name, "VT_FW_DIFTHR_S_Y"); + PX4_INFO("copying %s -> %s", "VT_FW_DIFTHR_SC", "VT_FW_DIFTHR_S_Y"); + return true; + } + } + return false; } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 2d6ef23f46..fd47faadef 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -343,9 +343,22 @@ void Tailsitter::fill_actuator_outputs() _thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE]; /* allow differential thrust if enabled */ - if (_param_vt_fw_difthr_en.get()) { - mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ; - _torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ; + if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { + float yaw_control = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get(); + mc_out[actuator_controls_s::INDEX_ROLL] = yaw_control; + _torque_setpoint_0->xyz[0] = yaw_control; + } + + if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::PITCH_BIT)) { + float pitch_control = fw_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get(); + mc_out[actuator_controls_s::INDEX_PITCH] = pitch_control; + _torque_setpoint_0->xyz[1] = pitch_control; + } + + if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::ROLL_BIT)) { + float roll_control = -fw_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get(); + mc_out[actuator_controls_s::INDEX_YAW] = roll_control; + _torque_setpoint_0->xyz[2] = roll_control; } } else { diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 3d2c2e2aa3..ab20579b79 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -481,9 +481,9 @@ void Tiltrotor::fill_actuator_outputs() _thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE]; /* allow differential thrust if enabled */ - if (_param_vt_fw_difthr_en.get()) { - mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ; - _torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ; + if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { + mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; + _torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; } } else { diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 2fefc5c5bd..a6e408fc9f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -254,27 +254,63 @@ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f); /** * Differential thrust in forwards flight. * - * Set to 1 to enable differential thrust in fixed-wing flight. + * Enable differential thrust seperately for roll, pitch, yaw in forward (fixed-wing) mode. + * The effectiveness of differential thrust around the corresponding axis can be + * tuned by setting VT_FW_DIFTHR_S_R / VT_FW_DIFTHR_S_P / VT_FW_DIFTHR_S_Y. * * @min 0 - * @max 1 - * @decimal 0 + * @max 7 + * @bit 0 Yaw + * @bit 1 Roll + * @bit 2 Pitch * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0); /** - * Differential thrust scaling factor + * Roll differential thrust factor in forward flight * - * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight. + * Maps the roll control output in forward flight to the actuator differential thrust output. + * + * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN. * * @min 0.0 - * @max 1.0 + * @max 2.0 * @decimal 2 * @increment 0.1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f); +PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_R, 1.f); + +/** + * Pitch differential thrust factor in forward flight + * + * Maps the pitch control output in forward flight to the actuator differential thrust output. + * + * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN. + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_P, 1.f); + +/** + * Yaw differential thrust factor in forward flight + * + * Maps the yaw control output in forward flight to the actuator differential thrust output. + * + * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN. + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @increment 0.1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_Y, 0.1f); /** * Backtransition deceleration setpoint to pitch feedforward gain. diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 326373e760..8bc1a27b80 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -77,6 +77,13 @@ enum VtolForwardActuationMode { ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND }; +// enum for bitmask of VT_FW_DIFTHR_EN parameter options +enum class VtFwDifthrEnBits : int32_t { + YAW_BIT = (1 << 0), + ROLL_BIT = (1 << 1), + PITCH_BIT = (1 << 2), +}; + class VtolAttitudeControl; class VtolType : public ModuleParams @@ -239,8 +246,10 @@ protected: (ParamBool) _param_fw_arsp_mode, (ParamFloat) _param_vt_trans_timeout, (ParamFloat) _param_mpc_xy_cruise, - (ParamBool) _param_vt_fw_difthr_en, - (ParamFloat) _param_vt_fw_difthr_sc, + (ParamInt) _param_vt_fw_difthr_en, + (ParamFloat) _param_vt_fw_difthr_s_y, + (ParamFloat) _param_vt_fw_difthr_s_p, + (ParamFloat) _param_vt_fw_difthr_s_r, (ParamFloat) _param_vt_b_dec_ff, (ParamFloat) _param_vt_b_dec_i, (ParamFloat) _param_vt_b_dec_mss, From 41903531929df6f6b5cd6c7ea665d9ba73b36c8f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 7 Nov 2022 09:31:17 -0500 Subject: [PATCH 34/43] vehicle_magnetometer: fix standalone mag bias est factored into in flight mag cal - preflight mag bias estimate is in the vehicle body frame and removed from the raw mag data after it's rotated, calibrated, etc - in flight mag bias (from ekf2) is in the vehicle body frame, but stored as a sensor frame offset immediately --- .../sensors/vehicle_magnetometer/VehicleMagnetometer.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 494af2c2b6..7d87f705a7 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -280,9 +280,8 @@ void VehicleMagnetometer::UpdateMagCalibration() _mag_cal[i].device_id = estimator_sensor_bias.mag_device_id; - // readd estimated bias that was removed before publishing vehicle_magnetometer - _mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias) + - _calibration_estimator_bias[mag_index]; + // readd estimated bias that was removed before publishing vehicle_magnetometer (_calibration_estimator_bias) + _mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias + _calibration_estimator_bias[mag_index]); _mag_cal[i].variance = bias_variance; From dac9f0dac4553e70dac98b21617add948ea71d86 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 7 Nov 2022 09:34:26 -0500 Subject: [PATCH 35/43] boards: px4_fmu-v6x_default include systemcmds/gpio --- boards/px4/fmu-v6x/default.px4board | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index ef2ff976cb..60bb22729c 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -74,6 +74,7 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_GPIO=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_LED_CONTROL=y From 3e35f948d8122fde384e5f5047bb62bb5375bff9 Mon Sep 17 00:00:00 2001 From: Junwoo Hwang Date: Wed, 26 Oct 2022 15:39:29 +0200 Subject: [PATCH 36/43] autopilot_tester: Make `wait_until_ready` also wait until vehicle can arm - Previously, due to the way MAVSDK's `health_all_ok` was implemented, vehicle often didn't have a valid global position estimate (although function returned true), and it wouldn't arm, and the SITL would fail - Also sometimes as vehicle didn't have manual control, it entered weird states where it wasn't able to arm as well - This adds a check to make sure vehicle is able to arm, directly from the Health struct --- test/mavsdk_tests/autopilot_tester.cpp | 27 +++++++++++--------------- test/mavsdk_tests/autopilot_tester.h | 12 +++++++++++- 2 files changed, 22 insertions(+), 17 deletions(-) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 504165804b..c58dd28bf6 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -80,27 +80,22 @@ void AutopilotTester::connect(const std::string uri) void AutopilotTester::wait_until_ready() { - std::cout << time_str() << "Waiting for system to be ready" << std::endl; + std::cout << time_str() << "Waiting for system to be ready (system health ok & able to arm)" << std::endl; + + // Wiat until the system is healthy CHECK(poll_condition_with_timeout( [this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(30))); - // FIXME: workaround to prevent race between PX4 switching to Hold mode - // and us trying to arm and take off. If PX4 is not in Hold mode yet, - // our arming presumably triggers a failsafe in manual mode. - std::this_thread::sleep_for(std::chrono::seconds(1)); -} + // Note: There is a known bug in MAVSDK (https://github.com/mavlink/MAVSDK/issues/1852), + // where `health_all_ok()` returning true doesn't actually mean vehicle is ready to accept + // global position estimate as valid (due to hysteresis). This needs to be fixed properly. -void AutopilotTester::wait_until_ready_local_position_only() -{ - std::cout << time_str() << "Waiting for system to be ready" << std::endl; + // However, this is mitigated by the `is_armable` check below as a side effect, since + // when the vehicle considers global position to be valid, it will then allow arming + + // Wait until we can arm CHECK(poll_condition_with_timeout( - [this]() { - return - (_telemetry->health().is_gyrometer_calibration_ok && - _telemetry->health().is_accelerometer_calibration_ok && - _telemetry->health().is_magnetometer_calibration_ok && - _telemetry->health().is_local_position_ok); - }, std::chrono::seconds(20))); + [this]() { return _telemetry->health().is_armable; }, std::chrono::seconds(20))); } void AutopilotTester::store_home() diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index a36a70f7bf..bb4b826c0e 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -95,8 +95,12 @@ public: ~AutopilotTester(); void connect(const std::string uri); + + /** + * @brief Wait until vehicle's system status is healthy & is able to arm + */ void wait_until_ready(); - void wait_until_ready_local_position_only(); + void store_home(); void check_home_within(float acceptance_radius_m); void check_home_not_within(float min_distance_m); @@ -204,6 +208,12 @@ private: void report_speed_factor(); + /** + * @brief Continue polling until condition returns true or we have a timeout + * + * @param fun Boolean returning function. When true, the polling terminates. + * @param duration Timeout for polling in `std::chrono::` time unit + */ template bool poll_condition_with_timeout( std::function fun, std::chrono::duration duration) From ae606488bd81101609db7ad1a5f91939a87aa82a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 7 Nov 2022 17:26:14 +0100 Subject: [PATCH 37/43] MulticopterRateControl: set timestamp last before publishing torque and thrust setpoints --- src/modules/mc_rate_control/MulticopterRateControl.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 02a54c9155..6eccec7809 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -296,22 +296,20 @@ MulticopterRateControl::Run() void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime ×tamp_sample) { vehicle_torque_setpoint_s vehicle_torque_setpoint{}; - vehicle_torque_setpoint.timestamp = hrt_absolute_time(); vehicle_torque_setpoint.timestamp_sample = timestamp_sample; vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f; vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f; vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f; - + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); } void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample) { vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; - vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); vehicle_thrust_setpoint.timestamp_sample = timestamp_sample; _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); - + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); } From f1b6f22bacbcade156156881bf557b25bd8c2f37 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 7 Nov 2022 17:54:00 +0100 Subject: [PATCH 38/43] AngularVelocityController: set timestamps on line before publishing --- .../AngularVelocityController.cpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index 979e22b388..b54ad9031a 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -218,10 +218,10 @@ AngularVelocityController::Run() // 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.timestamp = hrt_absolute_time(); _rate_ctrl_status_pub.publish(rate_ctrl_status); // publish controller output @@ -238,14 +238,12 @@ 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; - + v_angular_accel_sp.timestamp = hrt_absolute_time(); _vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp); } @@ -253,14 +251,12 @@ 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; - + v_torque_sp.timestamp = hrt_absolute_time(); _vehicle_torque_setpoint_pub.publish(v_torque_sp); } @@ -268,12 +264,11 @@ 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; - + v_thrust_sp.timestamp = hrt_absolute_time(); _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); } From 6df2b68d72cdf0b916d4f7ee8d3ab05672d2d526 Mon Sep 17 00:00:00 2001 From: Chris Seto Date: Sat, 5 Nov 2022 01:26:41 -0500 Subject: [PATCH 39/43] Clean msp_osd help string and debug message --- src/drivers/osd/msp_osd/msp_osd.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/osd/msp_osd/msp_osd.cpp b/src/drivers/osd/msp_osd/msp_osd.cpp index 1bb62e5612..e279ac7ff0 100644 --- a/src/drivers/osd/msp_osd/msp_osd.cpp +++ b/src/drivers/osd/msp_osd/msp_osd.cpp @@ -121,7 +121,7 @@ MspOsd::MspOsd(const char *device) : strcpy(_device, device); // _is_initialized = true; - PX4_INFO("MSP OSD prepared to run on %s", _device); + PX4_INFO("MSP OSD running on %s", _device); } MspOsd::~MspOsd() @@ -504,10 +504,10 @@ int MspOsd::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Msp OSD! +MSP telemetry streamer ### Implementation -Does the things for the DJI Air Unit OSD +Converts uORB messages to MSP telemetry packets ### Examples CLI usage example: From 2eed5306c0c1d4e828c8bab6591ebb631a435cb3 Mon Sep 17 00:00:00 2001 From: tanja Date: Mon, 7 Nov 2022 10:00:21 +0100 Subject: [PATCH 40/43] SPI: use official HW_VER_REV function --- platforms/common/spi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp index ca95ada5fe..a2033ff555 100644 --- a/platforms/common/spi.cpp +++ b/platforms/common/spi.cpp @@ -47,7 +47,7 @@ void px4_set_spi_buses_from_hw_version() #if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) int hw_version_revision = board_get_hw_version(); #else - int hw_version_revision = (board_get_hw_version() << 16) | board_get_hw_revision(); + int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision()); #endif From 5910f8982abc4372ca68213590df7fcabccbab4e Mon Sep 17 00:00:00 2001 From: Damien SIX Date: Tue, 8 Nov 2022 09:46:21 +0100 Subject: [PATCH 41/43] fix warning (mavlink): connection to gcs --- .../HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp index 1088dbc0ac..b9ae7a8036 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp @@ -119,7 +119,7 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte events::ID("check_rc_dl_no_dllink"), log_level, "No connection to the ground control station"); - if (reporter.mavlink_log_pub()) { + if (gcs_connection_required && reporter.mavlink_log_pub()) { mavlink_log_warning(reporter.mavlink_log_pub(), "Preflight Fail: No connection to the ground control station\t"); } From 688dae110834cf591c7255174daef402b55dc97a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 27 Oct 2022 10:13:28 -0400 Subject: [PATCH 42/43] ekf2: add new EKF2_EV_QMIN parameter --- src/modules/ekf2/EKF/common.h | 2 ++ src/modules/ekf2/EKF2.cpp | 3 ++- src/modules/ekf2/EKF2.hpp | 1 + src/modules/ekf2/ekf2_params.c | 13 +++++++++++++ 4 files changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index e1ae0dc117..f9a4a0595a 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -231,6 +231,7 @@ struct extVisionSample { float angVar{}; ///< angular heading variance (rad**2) VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD; uint8_t reset_counter{}; + int8_t quality{}; ///< quality indicator between 0 and 100 }; struct dragSample { @@ -357,6 +358,7 @@ struct parameters { float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check // vision position fusion + int32_t ev_quality_minimum{0}; ///< vision minimum acceptable quality integer float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz)) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bf4de3456f..deeec9fc9b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -123,6 +123,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), _param_ekf2_rng_k_gate(_params->range_kin_consistency_gate), + _param_ekf2_ev_qmin(_params->ev_quality_minimum), _param_ekf2_evv_gate(_params->ev_vel_innov_gate), _param_ekf2_evp_gate(_params->ev_pos_innov_gate), _param_ekf2_of_n_min(_params->flow_noise), @@ -1846,7 +1847,7 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo // use timestamp from external computer, clocks are synchronized when using MAVROS ev_data.time_us = ev_odom.timestamp_sample; ev_data.reset_counter = ev_odom.reset_counter; - //ev_data.quality = ev_odom.quality; + ev_data.quality = ev_odom.quality; if (new_ev_odom) { _ekf.setExtVisionData(ev_data); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 08bf9d30b1..6aea8e4dfb 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -522,6 +522,7 @@ private: // vision estimate fusion (ParamInt) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise + (ParamExtInt) _param_ekf2_ev_qmin, (ParamFloat) _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) (ParamFloat) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 0a69cd8f8c..36242b190a 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -779,6 +779,19 @@ PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); */ PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0); +/** + * External vision (EV) minimum quality (optional) + * + * External vision will only be started and fused if the quality metric is above this threshold. + * The quality metric is a completely optional field provided by some VIO systems. + * + * @group EKF2 + * @min 0 + * @max 100 + * @decimal 1 + */ +PARAM_DEFINE_INT32(EKF2_EV_QMIN, 0); + /** * Measurement noise for vision position observations used to lower bound or replace the uncertainty included in the message * From 5239993c882f4803deaa8ed251cc623dbae57a6b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 27 Oct 2022 11:30:19 -0400 Subject: [PATCH 43/43] ekf2: move EV vel to new state machine, introduce EKF2_EV_CTRL param --- .../init.d-posix/airframes/1013_iris_vision | 1 + src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/CMakeLists.txt | 1 + src/modules/ekf2/EKF/common.h | 21 +- src/modules/ekf2/EKF/control.cpp | 64 ++--- src/modules/ekf2/EKF/ekf.h | 9 +- src/modules/ekf2/EKF/ekf_helper.cpp | 76 +----- src/modules/ekf2/EKF/estimator_interface.cpp | 2 +- src/modules/ekf2/EKF/ev_height_control.cpp | 24 +- src/modules/ekf2/EKF/ev_vel_control.cpp | 231 ++++++++++++++++++ src/modules/ekf2/EKF2.cpp | 90 ++++--- src/modules/ekf2/EKF2.hpp | 5 +- src/modules/ekf2/ekf2_params.c | 31 ++- .../test/sensor_simulator/ekf_wrapper.cpp | 4 +- .../ekf2/test/sensor_simulator/vio.cpp | 16 +- src/modules/ekf2/test/sensor_simulator/vio.h | 4 +- src/modules/ekf2/test/test_EKF_airspeed.cpp | 1 + .../ekf2/test/test_EKF_externalVision.cpp | 9 +- 18 files changed, 406 insertions(+), 184 deletions(-) create mode 100644 src/modules/ekf2/EKF/ev_vel_control.cpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision b/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision index b2f1078cd1..0420919e7a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision @@ -10,6 +10,7 @@ # EKF2: Vision position and heading param set-default EKF2_AID_MASK 24 param set-default EKF2_EV_DELAY 5 +param set-default EKF2_EV_CTRL 15 param set-default EKF2_GPS_CTRL 0 # LPE: Vision + baro diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 9da2f0c3ba..7d040abc33 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -92,6 +92,7 @@ px4_add_module( EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp EKF/ev_height_control.cpp + EKF/ev_vel_control.cpp EKF/fake_height_control.cpp EKF/fake_pos_control.cpp EKF/gnss_height_control.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 5c1814c6da..ad14129159 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -43,6 +43,7 @@ add_library(ecl_EKF EKFGSF_yaw.cpp estimator_interface.cpp ev_height_control.cpp + ev_vel_control.cpp fake_height_control.cpp fake_pos_control.cpp gnss_height_control.cpp diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index f9a4a0595a..a9c329c8a7 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -79,8 +79,9 @@ using math::Utilities::updateYawInRotMat; #define GNDEFFECT_TIMEOUT 10E6 ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec) enum class VelocityFrame : uint8_t { - LOCAL_FRAME_FRD = 0, - BODY_FRAME_FRD = 1 + LOCAL_FRAME_NED = 0, + LOCAL_FRAME_FRD = 1, + BODY_FRAME_FRD = 2 }; enum GeoDeclinationMask : uint8_t { @@ -126,6 +127,13 @@ enum RngCtrl : uint8_t { ENABLED = 2 }; +enum class EvCtrl : uint8_t { + HPOS = (1<<0), + VPOS = (1<<1), + VEL = (1<<2), + YAW = (1<<3) +}; + enum SensorFusionMask : uint16_t { // Bit locations for fusion_mode DEPRECATED_USE_GPS = (1<<0), ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl) @@ -136,7 +144,7 @@ enum SensorFusionMask : uint16_t { USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl) - USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data + DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data }; struct gpsMessage { @@ -227,8 +235,8 @@ struct extVisionSample { Vector3f vel{}; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis Quatf quat{}; ///< quaternion defining rotation from body to earth frame Vector3f posVar{}; ///< XYZ position variances (m**2) - Vector3f velVar{}; ///< XYZ velocity variances ((m/sec)**2) - float angVar{}; ///< angular heading variance (rad**2) + Vector3f velocity_var{}; ///< XYZ velocity variances ((m/sec)**2) + Vector3f orientation_var{}; ///< orientation variance (rad**2) VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD; uint8_t reset_counter{}; int8_t quality{}; ///< quality indicator between 0 and 100 @@ -266,6 +274,7 @@ struct parameters { int32_t baro_ctrl{1}; int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL}; int32_t rng_ctrl{RngCtrl::CONDITIONAL}; + int32_t ev_ctrl{0}; int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator @@ -358,6 +367,8 @@ struct parameters { float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check // vision position fusion + float ev_vel_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec) + float ev_att_noise{0.1f}; ///< minimum allowed observation noise for EV attitude fusion (rad/sec) int32_t ev_quality_minimum{0}; ///< vision minimum acceptable quality integer float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 0235dbdcd3..f2674aea52 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -217,7 +217,7 @@ void Ekf::controlExternalVisionFusion() // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames // needs to be calculated and the observations rotated into the EKF frame of reference if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) && ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS) - || (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL)) && !_control_status.flags.ev_yaw) { + || (_params.ev_ctrl & static_cast(EvCtrl::VEL))) && !_control_status.flags.ev_yaw) { // rotate EV measurements into the EKF Navigation frame calcExtVisRotMat(); @@ -232,11 +232,6 @@ void Ekf::controlExternalVisionFusion() if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) { startEvPosFusion(); } - - // turn on use of external vision measurements for velocity - if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL && !_control_status.flags.ev_vel) { - startEvVelFusion(); - } } } @@ -331,15 +326,6 @@ void Ekf::controlExternalVisionFusion() // check if we have been deadreckoning too long if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) { - // only reset velocity if we have no another source of aiding constraining it - if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) && - isTimedOut(_time_last_hor_vel_fuse, (uint64_t)1E6)) { - - if (_control_status.flags.ev_vel) { - resetVelocityToVision(); - } - } - resetHorizontalPositionToVision(); } } @@ -354,38 +340,10 @@ void Ekf::controlExternalVisionFusion() fuseHorizontalPosition(_aid_src_ev_pos); } - // determine if we should use the velocity observations - if (_control_status.flags.ev_vel) { - if (reset && _control_status_prev.flags.ev_vel) { - resetVelocityToVision(); - } - - // check if we have been deadreckoning too long - if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) { - // only reset velocity if we have no another source of aiding constraining it - if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) && - isTimedOut(_time_last_hor_pos_fuse, (uint64_t)1E6)) { - resetVelocityToVision(); - } - } - - resetEstimatorAidStatus(_aid_src_ev_vel); - - const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f)); - - const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f); - - updateVelocityAidSrcStatus(_ev_sample_delayed.time_us, getVisionVelocityInEkfFrame(), obs_var, innov_gate, _aid_src_ev_vel); - - _aid_src_ev_vel.fusion_enabled = true; - - fuseVelocity(_aid_src_ev_vel); - } - // determine if we should use the yaw observation resetEstimatorAidStatus(_aid_src_ev_yaw); const float measured_hdg = getEulerYaw(_ev_sample_delayed.quat); - const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f); + const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.orientation_var(2), 1.e-4f); if (PX4_ISFINITE(measured_hdg)) { _aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us; @@ -414,12 +372,28 @@ void Ekf::controlExternalVisionFusion() } } + + bool ev_reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter); + + // determine if we should use the horizontal position observations + bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (_ev_sample_delayed.quality >= _params.ev_quality_minimum); + + bool starting_conditions_passing = quality_sufficient + && ((_ev_sample_delayed.time_us - _ev_sample_delayed_prev.time_us) < EV_MAX_INTERVAL) + && ((_params.ev_quality_minimum <= 0) || (_ev_sample_delayed_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient + && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient + && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL); + + // EV velocity + controlEvVelFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); + + // record observation and estimate for use next time _ev_sample_delayed_prev = _ev_sample_delayed; _hpos_pred_prev = _state.pos.xy(); _yaw_pred_prev = getEulerYaw(_state.quat_nominal); - } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw) + } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw) && !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) { // Turn off EV fusion mode if no data has been received diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 0ee5ebdace..788d00f5f9 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -504,6 +504,8 @@ private: uint64_t _time_last_healthy_rng_data{0}; uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source + uint8_t _nb_ev_vel_reset_available{0}; + Vector3f _last_known_pos{}; ///< last known local position vector (m) uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec) @@ -700,7 +702,6 @@ private: void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var); void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, float vel_var) { resetHorizontalVelocityTo(new_horz_vel, Vector2f(vel_var, vel_var)); } - void resetVelocityToVision(); void resetHorizontalVelocityToZero(); void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var); @@ -784,10 +785,6 @@ private: // update the rotation matrix which transforms EV navigation frame measurements into NED void calcExtVisRotMat(); - Vector3f getVisionVelocityInEkfFrame() const; - - Vector3f getVisionVelocityVarianceInEkfFrame() const; - // matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24> // that is optimized by exploring the sparsity in H template @@ -872,6 +869,7 @@ private: // control fusion of external vision observations void controlExternalVisionFusion(); + void controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, bool quality_sufficient, estimator_aid_source3d_s& aid_src); // control fusion of optical flow observations void controlOpticalFlowFusion(); @@ -1033,7 +1031,6 @@ private: void stopGpsYawFusion(); void startEvPosFusion(); - void startEvVelFusion(); void startEvYawFusion(); void stopEvFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 1cbdf456ad..db39044439 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -44,13 +44,6 @@ #include #include -void Ekf::resetVelocityToVision() -{ - _information_events.flags.reset_vel_to_vision = true; - ECL_INFO("reset to vision velocity"); - resetVelocityTo(getVisionVelocityInEkfFrame(), getVisionVelocityVarianceInEkfFrame()); -} - void Ekf::resetHorizontalVelocityToZero() { _information_events.flags.reset_vel_to_zero = true; @@ -297,7 +290,7 @@ bool Ekf::resetMagHeading() bool Ekf::resetYawToEv() { const float yaw_new = getEulerYaw(_ev_sample_delayed.quat); - const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)); + const float yaw_new_variance = fmaxf(_ev_sample_delayed.orientation_var(2), sq(1.0e-2f)); resetQuatStateYaw(yaw_new, yaw_new_variance); _R_ev_to_ekf.setIdentity(); @@ -1156,56 +1149,6 @@ void Ekf::updateGroundEffect() } } -Vector3f Ekf::getVisionVelocityInEkfFrame() const -{ - Vector3f vel; - // correct velocity for offset relative to IMU - const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; - - // rotate measurement into correct earth frame if required - switch (_ev_sample_delayed.vel_frame) { - case VelocityFrame::BODY_FRAME_FRD: - vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body); - break; - - case VelocityFrame::LOCAL_FRAME_FRD: - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - - if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) { - vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth; - - } else { - vel = _ev_sample_delayed.vel - vel_offset_earth; - } - - break; - } - - return vel; -} - -Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const -{ - Matrix3f ev_vel_cov = matrix::diag(_ev_sample_delayed.velVar); - - // rotate measurement into correct earth frame if required - switch (_ev_sample_delayed.vel_frame) { - case VelocityFrame::BODY_FRAME_FRD: - ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose(); - break; - - case VelocityFrame::LOCAL_FRAME_FRD: - if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) { - ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose(); - } - - break; - } - - return ev_vel_cov.diag(); -} - // update the rotation matrix which rotates EV measurements into the EKF's navigation frame void Ekf::calcExtVisRotMat() { @@ -1368,14 +1311,6 @@ void Ekf::startEvPosFusion() ECL_INFO("starting vision pos fusion"); } -void Ekf::startEvVelFusion() -{ - _control_status.flags.ev_vel = true; - resetVelocityToVision(); - _information_events.flags.starting_vision_vel_fusion = true; - ECL_INFO("starting vision vel fusion"); -} - void Ekf::startEvYawFusion() { // turn on fusion of external vision yaw measurements and disable all magnetometer fusion @@ -1405,15 +1340,6 @@ void Ekf::stopEvPosFusion() } } -void Ekf::stopEvVelFusion() -{ - if (_control_status.flags.ev_vel) { - ECL_INFO("stopping EV vel fusion"); - _control_status.flags.ev_vel = false; - resetEstimatorAidStatus(_aid_src_ev_vel); - } -} - void Ekf::stopEvYawFusion() { if (_control_status.flags.ev_yaw) { diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 42b2380859..81f229c1af 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -496,7 +496,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms); } - if (_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW | SensorFusionMask::USE_EXT_VIS_VEL)) { + if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl & static_cast(EvCtrl::VEL))) { max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms); } diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index d360e99abd..01b08e8f89 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -94,8 +94,28 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample) bias_est.setBias(-_state.pos(2) + measurement); // reset vertical velocity - if (PX4_ISFINITE(ev_sample.vel(2)) && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)) { - resetVerticalVelocityTo(ev_sample.vel(2), ev_sample.velVar(2)); + if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast(EvCtrl::VEL))) { + + // correct velocity for offset relative to IMU + const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; + const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + + switch (ev_sample.vel_frame) { + case VelocityFrame::LOCAL_FRAME_NED: + case VelocityFrame::LOCAL_FRAME_FRD: { + const Vector3f reset_vel = ev_sample.vel - vel_offset_earth; + resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise))); + } + break; + + case VelocityFrame::BODY_FRAME_FRD: { + const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body); + const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose(); + resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise))); + } + break; + } } else { resetVerticalVelocityToZero(); diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp new file mode 100644 index 0000000000..979e951512 --- /dev/null +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -0,0 +1,231 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 ev_vel_control.cpp + * Control functions for ekf external vision velocity fusion + */ + +#include "ekf.h" + +void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, + bool quality_sufficient, estimator_aid_source3d_s &aid_src) +{ + static constexpr const char *AID_SRC_NAME = "EV velocity"; + + const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw) + || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); + + // determine if we should use EV velocity aiding + bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VEL)) + && _control_status.flags.tilt_align + && ev_sample.vel.isAllFinite(); + + // correct velocity for offset relative to IMU + const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; + const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + + // rotate measurement into correct earth frame if required + Vector3f vel{NAN, NAN, NAN}; + Matrix3f vel_cov{}; + + switch (ev_sample.vel_frame) { + case VelocityFrame::LOCAL_FRAME_NED: + if (_control_status.flags.yaw_align) { + vel = ev_sample.vel - vel_offset_earth; + vel_cov = matrix::diag(ev_sample.velocity_var); + + } else { + continuing_conditions_passing = false; + } + + break; + + case VelocityFrame::LOCAL_FRAME_FRD: + if (_control_status.flags.ev_yaw) { + // using EV frame + vel = ev_sample.vel - vel_offset_earth; + vel_cov = matrix::diag(ev_sample.velocity_var); + + } else { + // rotate EV to the EKF reference frame + const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); + const Dcmf R_ev_to_ekf = Dcmf(q_error); + + vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth; + vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose(); + + // increase minimum variance to include EV orientation variance + // TODO: do this properly + const float orientation_var_max = ev_sample.orientation_var.max(); + + for (int i = 0; i < 2; i++) { + vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max); + } + } + + break; + + case VelocityFrame::BODY_FRAME_FRD: + vel = _R_to_earth * (ev_sample.vel - vel_offset_body); + vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose(); + break; + + default: + continuing_conditions_passing = false; + break; + } + + // increase minimum variance if GPS active (position reference) + if (_control_status.flags.gps) { + for (int i = 0; i < 2; i++) { + vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise)); + } + } + + const Vector3f measurement{vel}; + + const Vector3f measurement_var{ + math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)), + math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)), + math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f)) + }; + + const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite(); + + updateVelocityAidSrcStatus(ev_sample.time_us, + measurement, // observation + measurement_var, // observation variance + math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate + aid_src); + + if (!measurement_valid) { + continuing_conditions_passing = false; + } + + starting_conditions_passing &= continuing_conditions_passing + && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); + + if (_control_status.flags.ev_vel) { + aid_src.fusion_enabled = true; + + if (continuing_conditions_passing) { + + if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) { + + if (quality_sufficient) { + ECL_INFO("reset to %s", AID_SRC_NAME); + _information_events.flags.reset_vel_to_vision = true; + resetVelocityTo(measurement, measurement_var); + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + } else { + // EV has reset, but quality isn't sufficient + // we have no choice but to stop EV and try to resume once quality is acceptable + stopEvVelFusion(); + return; + } + + } else if (quality_sufficient) { + fuseVelocity(aid_src); + + } else { + aid_src.innovation_rejected = true; + } + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); // 1 second + + if (is_fusion_failing) { + + if ((_nb_ev_vel_reset_available > 0) && quality_sufficient) { + // Data seems good, attempt a reset + _information_events.flags.reset_vel_to_vision = true; + ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); + resetVelocityTo(measurement, measurement_var); + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + if (_control_status.flags.in_air) { + _nb_ev_vel_reset_available--; + } + + } else if (starting_conditions_passing) { + // Data seems good, but previous reset did not fix the issue + // something else must be wrong, declare the sensor faulty and stop the fusion + //_control_status.flags.ev_vel_fault = true; + ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); + stopEvVelFusion(); + + } else { + // A reset did not fix the issue but all the starting checks are not passing + // This could be a temporary issue, stop the fusion without declaring the sensor faulty + ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); + stopEvVelFusion(); + } + } + + } else { + // Stop fusion but do not declare it faulty + ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME); + stopEvVelFusion(); + } + + } else { + if (starting_conditions_passing) { + // activate fusion, only reset if necessary + if (!isHorizontalAidingActive() || yaw_alignment_changed) { + ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); + _information_events.flags.reset_vel_to_vision = true; + resetVelocityTo(measurement, measurement_var); + + } else { + ECL_INFO("starting %s fusion", AID_SRC_NAME); + } + + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + _nb_ev_vel_reset_available = 5; + _information_events.flags.starting_vision_vel_fusion = true; + _control_status.flags.ev_vel = true; + } + } +} + +void Ekf::stopEvVelFusion() +{ + if (_control_status.flags.ev_vel) { + resetEstimatorAidStatus(_aid_src_ev_vel); + + _control_status.flags.ev_vel = false; + } +} diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index deeec9fc9b..cf1aacd056 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -123,7 +123,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), _param_ekf2_rng_k_gate(_params->range_kin_consistency_gate), + _param_ekf2_ev_ctrl(_params->ev_ctrl), _param_ekf2_ev_qmin(_params->ev_quality_minimum), + _param_ekf2_evv_noise(_params->ev_vel_noise), + _param_ekf2_eva_noise(_params->ev_att_noise), _param_ekf2_evv_gate(_params->ev_vel_innov_gate), _param_ekf2_evp_gate(_params->ev_pos_innov_gate), _param_ekf2_of_n_min(_params->flow_noise), @@ -706,6 +709,23 @@ void EKF2::VerifyParams() events::send(events::ID("ekf2_hgt_ref_gps"), events::Log::Warning, "GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get()); } + + // EV EKF2_AID_MASK -> EKF2_EV_CTRL + if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) { + + _param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast(EvCtrl::VEL)); + _param_ekf2_ev_ctrl.commit(); + + _param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)); + _param_ekf2_aid_mask.commit(); + + mavlink_log_critical(&_mavlink_log_pub, "EKF2 EV use EKF2_EV_CTRL instead of EKF2_AID_MASK\n"); + /* EVENT + * @description EKF2_AID_MASK is set to {1:.0}. + */ + events::send(events::ID("ekf2_aid_mask_ev"), events::Log::Warning, + "Use EKF2_EV_CTRL instead", _param_ekf2_aid_mask.get()); + } } void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) @@ -1737,45 +1757,44 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo ev_data.vel.setNaN(); ev_data.quat.setNaN(); - // if error estimates are unavailable, use parameter defined defaults - // check for valid velocity data - if (Vector3f(ev_odom.velocity).isAllFinite()) { - bool velocity_valid = true; + const Vector3f ev_odom_vel(ev_odom.velocity); + const Vector3f ev_odom_vel_var(ev_odom.velocity_variance); + + if (ev_odom_vel.isAllFinite()) { + bool velocity_frame_valid = false; switch (ev_odom.velocity_frame) { - // case vehicle_odometry_s::VELOCITY_FRAME_NED: - // ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED; - // break; + case vehicle_odometry_s::VELOCITY_FRAME_NED: + ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED; + velocity_frame_valid = true; + break; case vehicle_odometry_s::VELOCITY_FRAME_FRD: ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; + velocity_frame_valid = true; break; case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD: ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD; - break; - - default: - velocity_valid = false; + velocity_frame_valid = true; break; } - if (velocity_valid) { - ev_data.vel(0) = ev_odom.velocity[0]; - ev_data.vel(1) = ev_odom.velocity[1]; - ev_data.vel(2) = ev_odom.velocity[2]; + if (velocity_frame_valid) { + ev_data.vel = ev_odom_vel; const float evv_noise_var = sq(_param_ekf2_evv_noise.get()); // velocity measurement error from ev_data or parameters - if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.velocity_variance).isAllFinite()) { - ev_data.velVar(0) = fmaxf(evv_noise_var, ev_odom.velocity_variance[0]); - ev_data.velVar(1) = fmaxf(evv_noise_var, ev_odom.velocity_variance[1]); - ev_data.velVar(2) = fmaxf(evv_noise_var, ev_odom.velocity_variance[2]); + if ((_param_ekf2_ev_noise_md.get() == 0) && ev_odom_vel_var.isAllFinite()) { + + ev_data.velocity_var(0) = fmaxf(evv_noise_var, ev_odom_vel_var(0)); + ev_data.velocity_var(1) = fmaxf(evv_noise_var, ev_odom_vel_var(1)); + ev_data.velocity_var(2) = fmaxf(evv_noise_var, ev_odom_vel_var(2)); } else { - ev_data.velVar.setAll(evv_noise_var); + ev_data.velocity_var.setAll(evv_noise_var); } new_ev_odom = true; @@ -1822,23 +1841,34 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo } // check for valid orientation data - if ((Quatf(ev_odom.q).isAllFinite()) - && ((fabsf(ev_odom.q[0]) > 0.f) || (fabsf(ev_odom.q[1]) > 0.f) - || (fabsf(ev_odom.q[2]) > 0.f) || (fabsf(ev_odom.q[3]) > 0.f)) - ) { - ev_data.quat = Quatf(ev_odom.q); + const Quatf ev_odom_q(ev_odom.q); + const Vector3f ev_odom_q_var(ev_odom.orientation_variance); + const bool non_zero = (fabsf(ev_odom_q(0)) > 0.f) || (fabsf(ev_odom_q(1)) > 0.f) + || (fabsf(ev_odom_q(2)) > 0.f) || (fabsf(ev_odom_q(3)) > 0.f); + const float eps = 1e-5f; + const bool no_element_larger_than_one = (fabsf(ev_odom_q(0)) <= 1.f + eps) + && (fabsf(ev_odom_q(1)) <= 1.f + eps) + && (fabsf(ev_odom_q(2)) <= 1.f + eps) + && (fabsf(ev_odom_q(3)) <= 1.f + eps); + const bool norm_in_tolerance = fabsf(1.f - ev_odom_q.norm()) <= eps; + + const bool orientation_valid = ev_odom_q.isAllFinite() && non_zero && no_element_larger_than_one && norm_in_tolerance; + + if (orientation_valid) { + ev_data.quat = ev_odom_q; ev_data.quat.normalize(); // orientation measurement error from ev_data or parameters const float eva_noise_var = sq(_param_ekf2_eva_noise.get()); - if (!_param_ekf2_ev_noise_md.get() && - PX4_ISFINITE(ev_odom.orientation_variance[2]) - ) { - ev_data.angVar = fmaxf(eva_noise_var, ev_odom.orientation_variance[2]); + if ((_param_ekf2_ev_noise_md.get() == 0) && ev_odom_q_var.isAllFinite()) { + + ev_data.orientation_var(0) = fmaxf(eva_noise_var, ev_odom_q_var(0)); + ev_data.orientation_var(1) = fmaxf(eva_noise_var, ev_odom_q_var(1)); + ev_data.orientation_var(2) = fmaxf(eva_noise_var, ev_odom_q_var(2)); } else { - ev_data.angVar = eva_noise_var; + ev_data.orientation_var.setAll(eva_noise_var); } new_ev_odom = true; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 6aea8e4dfb..48627e79a0 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -520,14 +520,15 @@ private: _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) // vision estimate fusion + (ParamExtInt) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection (ParamInt) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise (ParamExtInt) _param_ekf2_ev_qmin, (ParamFloat) _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) - (ParamFloat) + (ParamExtFloat) _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s) - (ParamFloat) + (ParamExtFloat) _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad) (ParamExtFloat) _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 36242b190a..e5d29b7361 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -608,7 +608,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * 5 : Set to true to enable multi-rotor drag specific force fusion * 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used * 7 : Deprecated, use EKF2_GPS_CTRL instead - * 8 : Set to true to enable vision velocity fusion + * 3 : Deprecated, use EKF2_EV_CTRL instead * * @group EKF2 * @min 0 @@ -621,7 +621,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * @bit 5 multi-rotor drag fusion * @bit 6 rotate external vision * @bit 7 unused - * @bit 8 vision velocity fusion + * @bit 8 unused * @reboot_required true */ PARAM_DEFINE_INT32(EKF2_AID_MASK, 0); @@ -654,6 +654,25 @@ PARAM_DEFINE_INT32(EKF2_HGT_REF, 1); */ PARAM_DEFINE_INT32(EKF2_BARO_CTRL, 1); +/** + * External vision (EV) sensor aiding + * + * Set bits in the following positions to enable: + * 0 : Horizontal position fusion + * 1 : Vertical position fusion + * 2 : 3D velocity fusion + * 3 : Yaw + * + * @group EKF2 + * @min 0 + * @max 15 + * @bit 0 Horizontal position + * @bit 1 Vertical position + * @bit 2 3D velocity + * @bit 3 Yaw + */ +PARAM_DEFINE_INT32(EKF2_EV_CTRL, 15); + /** * GNSS sensor aiding * @@ -770,11 +789,13 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f); PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); /** - * Whether to set the external vision observation noise from the parameter or from vision message + * External vision (EV) noise mode * - * If set to true the observation noise is set from the parameters directly, if set to false the measurement noise is taken from the vision message and the parameter are used as a lower bound. + * If set to 0 (default) the measurement noise is taken from the vision message and the EV noise parameters are used as a lower bound. + * If set to 1 the observation noise is set from the parameters directly, * - * @boolean + * @value 0 EV reported variance (parameter lower bound) + * @value 1 EV noise parameters * @group EKF2 */ PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0); diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 3f4cc8bffc..3215dbb5b0 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -152,12 +152,12 @@ bool EkfWrapper::isIntendingExternalVisionPositionFusion() const void EkfWrapper::enableExternalVisionVelocityFusion() { - _ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_VEL; + _ekf_params->ev_ctrl |= static_cast(EvCtrl::VEL); } void EkfWrapper::disableExternalVisionVelocityFusion() { - _ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_VEL; + _ekf_params->ev_ctrl &= ~static_cast(EvCtrl::VEL); } bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const diff --git a/src/modules/ekf2/test/sensor_simulator/vio.cpp b/src/modules/ekf2/test/sensor_simulator/vio.cpp index 42723306e0..ab26808ece 100644 --- a/src/modules/ekf2/test/sensor_simulator/vio.cpp +++ b/src/modules/ekf2/test/sensor_simulator/vio.cpp @@ -7,6 +7,7 @@ namespace sensor Vio::Vio(std::shared_ptr ekf): Sensor(ekf) { + _vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; } Vio::~Vio() @@ -26,7 +27,7 @@ void Vio::setData(const extVisionSample &vio_data) void Vio::setVelocityVariance(const Vector3f &velVar) { - _vio_data.velVar = velVar; + _vio_data.velocity_var = velVar; } void Vio::setPositionVariance(const Vector3f &posVar) @@ -36,7 +37,7 @@ void Vio::setPositionVariance(const Vector3f &posVar) void Vio::setAngularVariance(float angVar) { - _vio_data.angVar = angVar; + _vio_data.orientation_var(2) = angVar; } void Vio::setVelocity(const Vector3f &vel) @@ -59,11 +60,16 @@ void Vio::setVelocityFrameToBody() _vio_data.vel_frame = VelocityFrame::BODY_FRAME_FRD; } -void Vio::setVelocityFrameToLocal() +void Vio::setVelocityFrameToLocalFRD() { _vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; } +void Vio::setVelocityFrameToLocalNED() +{ + _vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED; +} + extVisionSample Vio::dataAtRest() { extVisionSample vio_data; @@ -71,8 +77,8 @@ extVisionSample Vio::dataAtRest() vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};; vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f}; vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f}; - vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f}; - vio_data.angVar = 0.05f; + vio_data.velocity_var = Vector3f{0.1f, 0.1f, 0.1f}; + vio_data.orientation_var(2) = 0.05f; vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; return vio_data; } diff --git a/src/modules/ekf2/test/sensor_simulator/vio.h b/src/modules/ekf2/test/sensor_simulator/vio.h index 2000865e22..50b81e60c0 100644 --- a/src/modules/ekf2/test/sensor_simulator/vio.h +++ b/src/modules/ekf2/test/sensor_simulator/vio.h @@ -58,7 +58,9 @@ public: void setVelocity(const Vector3f &vel); void setPosition(const Vector3f &pos); void setOrientation(const Quatf &quat); - void setVelocityFrameToLocal(); + + void setVelocityFrameToLocalNED(); + void setVelocityFrameToLocalFRD(); void setVelocityFrameToBody(); extVisionSample dataAtRest(); diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index 7a3b2e003e..ff73948bd3 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -83,6 +83,7 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation) const Vector2f airspeed_body(2.4f, 0.0f); _ekf_wrapper.enableExternalVisionVelocityFusion(); _sensor_simulator._vio.setVelocity(simulated_velocity_earth); + _sensor_simulator._vio.setVelocityFrameToLocalNED(); _sensor_simulator.startExternalVision(); _ekf->set_in_air_status(true); diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 24744f9e84..50a52a9828 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -162,9 +162,9 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment) const Vector3f estimated_velocity_in_ekf_frame = _ekf->getVelocity(); EXPECT_TRUE(isEqual(estimated_velocity_in_ekf_frame, simulated_velocity_in_ekf_frame, 0.01f)); // And: the frame offset should be estimated correctly - Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion(); - EXPECT_TRUE(matrix::isEqual(vision_to_ekf.canonical(), - estimatedExternalVisionFrameOffset.canonical())); + // Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion(); + // EXPECT_TRUE(matrix::isEqual(vision_to_ekf.canonical(), + // estimatedExternalVisionFrameOffset.canonical())); // AND: the reset in velocity should be saved correctly reset_logging_checker.capturePostResetState(); @@ -212,7 +212,6 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment) EXPECT_TRUE(isEqual(estimated_position_in_ekf_frame, simulated_position_in_ekf_frame, 1e-2f)); } - TEST_F(EkfExternalVisionTest, visionVarianceCheck) { _sensor_simulator.runSeconds(_tilt_align_time); @@ -308,7 +307,7 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal) // WHEN: measurement is given in LOCAL-FRAME and // x variance is bigger than y variance - _sensor_simulator._vio.setVelocityFrameToLocal(); + _sensor_simulator._vio.setVelocityFrameToLocalNED(); const Vector3f vel_cov_earth{2.f, 0.01f, 0.01f}; const Vector3f vel_earth(1.0f, 0.0f, 0.0f);