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/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/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 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 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/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 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/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/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/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/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/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 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 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..020a44bfec 100644 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ b/platforms/qurt/src/px4/CMakeLists.txt @@ -33,9 +33,12 @@ # Placeholder set(QURT_LAYER_SRCS + drv_hrt.cpp tasks.cpp ) add_library(px4_layer ${QURT_LAYER_SRCS} ) + +target_link_libraries(px4_layer PRIVATE work_queue) 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(); +} 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; } 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 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 { 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) { } 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: 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 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}; 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/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); } 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/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 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"); } 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_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); 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 * diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 8a1f0df1f4..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. - * Should return false if not filled in and the effectivenes matrix should be used instead + * 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 bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const { return false; } + 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 8cf7a9cb5d..f5a7aaedbb 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -217,48 +217,36 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit } } -bool ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const +void ActuatorEffectivenessHelicopter::getUnallocatedControl(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..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; - bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const 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/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..bd9e1db0fc 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; } } } @@ -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,23 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh break; } } + +void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(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 7f242915fe..5686d2ebda 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -81,8 +81,11 @@ public: const char *name() const override { return "VTOL Tiltrotor"; } uint32_t getStoppedMotors() const override { return _stopped_motors; } + + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + protected: - bool _combined_tilt_updated{true}; + bool _collective_tilt_updated{true}; ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; ActuatorEffectivenessTilts _tilts; @@ -93,8 +96,15 @@ 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)}; + + 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..c487b4bc4b 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -589,33 +589,29 @@ 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(); - // 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->getUnallocatedControl(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(); diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 5e565b8a70..ded4c01d52 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -359,6 +359,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 b8dcf1e092..866e790cb2 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -332,30 +332,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 3cd695099b..dc63000df6 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 9ca8b45f87..e91c500a01 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -150,6 +150,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), @@ -581,9 +582,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 054b2c18f4..13df4237ca 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -571,7 +571,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 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/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..6741e45c4f 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()); + // Check if we are moving vertically. + // 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. - 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; + float vertical_velocity_threshold = _param_lndmc_z_vel_max.get(); + + if (_landed_hysteresis.get_state()) { + vertical_velocity_threshold *= 2.5f; } - _vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate); + _vertical_movement = (fabsf(_vehicle_local_position.vz) > vertical_velocity_threshold); } 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] >= 1.1f * _param_lndmc_z_vel_max.get()); } // ground contact requires commanded descent until landed @@ -278,21 +263,21 @@ 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 - if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { - landThresholdFactor = 2.5f; + // 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; } - // 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.xy().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 @@ -306,14 +291,6 @@ 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(); - } - // 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 1bbd7c7315..1e473eff83 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 @@ -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; @@ -83,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 landed. */ - static constexpr hrt_abstime LAND_DETECTOR_LAND_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; @@ -94,8 +92,6 @@ private: param_t minThrottle; param_t hoverThrottle; param_t minManThrottle; - param_t landSpeed; - param_t crawlSpeed; param_t useHoverThrustEstimate; } _paramHandle{}; @@ -103,8 +99,6 @@ private: float minThrottle; float hoverThrottle; float minManThrottle; - float landSpeed; - float crawlSpeed; bool useHoverThrustEstimate; } _params{}; @@ -126,11 +120,11 @@ 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 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 diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 3c979ac767..889085e79e 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -48,16 +48,19 @@ PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f); /** - * Multicopter max climb rate + * Multicopter vertical velocity threshold * - * Maximum vertical velocity allowed in the landed state + * 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 * * @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/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 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? } 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..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,10 +448,12 @@ 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 */ 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); } 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; 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, 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, 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); } } 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)