From 089c962d9208b3616ba505ab927fa399554035d4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 16 Aug 2021 13:10:52 -0400 Subject: [PATCH] px4io: moving mixing to FMU side MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Using mixers on the IO side had a remote benefit of being able to override all control surfaces with a radio remote on a fixed wing. This ended up not being used that much and since the original design 10 years ago (2011) we have been able to convince ourselves that the overall system stability is at a level where this marginal benefit, which is not present on multicopters, is not worth the hazzle. Co-authored-by: Beat Küng Co-authored-by: Daniel Agar --- ROMFS/px4fmu_common/init.d-posix/rcS | 2 - ROMFS/px4fmu_common/init.d/CMakeLists.txt | 1 - .../init.d/airframes/1002_standard_vtol.hil | 1 - .../init.d/airframes/4030_3dr_solo | 1 - ROMFS/px4fmu_common/init.d/rc.interface | 30 +- ROMFS/px4fmu_common/init.d/rc.io | 23 - ROMFS/px4fmu_test/init.d/rcS | 17 +- .../crazyflie/syslink/syslink_main.cpp | 3 +- msg/input_rc.msg | 1 + msg/manual_control_switches.msg | 7 - msg/px4io_status.msg | 45 +- msg/rc_channels.msg | 53 +- msg/safety.msg | 2 - msg/vehicle_control_mode.msg | 1 - .../px4_work_queue/WorkQueueManager.hpp | 22 +- platforms/posix/cmake/sitl_tests.cmake | 1 - posix-configs/SITL/init/test/cmd_template.in | 1 - .../SITL/init/test/test_imu_filtering | 1 - posix-configs/SITL/init/test/test_mavlink | 1 - posix-configs/SITL/init/test/test_shutdown | 1 - posix-configs/SITL/init/test/tests_all | 1 - src/drivers/drv_pwm_output.h | 3 - src/drivers/drv_rc_input.h | 80 - src/drivers/px4io/CMakeLists.txt | 2 +- src/drivers/px4io/px4io.cpp | 2767 +++++------------ src/drivers/px4io/px4io_params.c | 7 +- src/drivers/px4io/px4io_serial.cpp | 26 +- src/drivers/px4io/px4io_uploader.cpp | 8 +- src/drivers/rc_input/RCInput.cpp | 4 +- src/drivers/rc_input/RCInput.hpp | 1 - .../snapdragon_spektrum_rc/spektrum_rc.cpp | 1 - src/lib/mixer_module/CMakeLists.txt | 2 +- src/lib/mixer_module/mixer_module.cpp | 6 +- src/lib/rc/sbus.cpp | 12 +- src/lib/systemlib/system_params.c | 16 - src/modules/commander/Commander.cpp | 119 +- src/modules/commander/Commander.hpp | 8 +- src/modules/mavlink/mavlink_receiver.cpp | 13 +- .../mavlink/streams/MANUAL_CONTROL.hpp | 3 - src/modules/px4iofirmware/CMakeLists.txt | 2 - src/modules/px4iofirmware/controls.c | 208 +- src/modules/px4iofirmware/mixer.cpp | 468 +-- src/modules/px4iofirmware/mixer.h | 42 - src/modules/px4iofirmware/protocol.h | 199 +- src/modules/px4iofirmware/px4io.c | 29 - src/modules/px4iofirmware/px4io.h | 42 +- src/modules/px4iofirmware/registers.c | 443 +-- src/modules/rc_update/params.c | 244 +- src/modules/rc_update/params_deprecated.c | 146 + src/modules/rc_update/rc_update.cpp | 101 +- src/modules/rc_update/rc_update.h | 14 +- src/modules/simulator/simulator.h | 2 +- src/systemcmds/tests/CMakeLists.txt | 1 - src/systemcmds/tests/test_conv.cpp | 74 - src/systemcmds/tests/test_mixer.cpp | 6 +- src/systemcmds/tests/test_ppm_loopback.c | 2 +- src/systemcmds/tests/test_rc.c | 2 +- src/systemcmds/tests/tests_main.c | 1 - src/systemcmds/tests/tests_main.h | 1 - 59 files changed, 1150 insertions(+), 4170 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.io delete mode 100644 src/drivers/drv_rc_input.h delete mode 100644 src/modules/px4iofirmware/mixer.h delete mode 100644 src/systemcmds/tests/test_conv.cpp diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 7d3784c151..343fcf4283 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -120,8 +120,6 @@ then param set SENS_BOARD_X_OFF 0.000001 param set SENS_DPRES_OFF 0.001 - - param set SYS_RESTART_TYPE 2 fi param set-default BAT1_N_CELLS 4 diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index ef190544ae..543b47e25b 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -42,7 +42,6 @@ px4_add_romfs_files( rc.fw_apps rc.fw_defaults rc.interface - rc.io rc.logging rc.mc_apps rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index 636a1cd94a..c2b3679db3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -56,7 +56,6 @@ param set-default RTL_DESCEND_ALT 10 param set-default RTL_RETURN_ALT 30 param set-default SDLOG_DIRS_MAX 7 -param set-default SYS_RESTART_TYPE 2 param set-default VT_F_TRANS_THR 0.75 param set-default VT_MOT_ID 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo index 96f78e8e1a..f1e581dea0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo +++ b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo @@ -48,7 +48,6 @@ param set-default CBRK_IO_SAFETY 22027 param set-default CBRK_SUPPLY_CHK 894281 # RC configuration -param set-default RC_MAP_MODE_SW 5 param set-default RC_MAP_PITCH 2 param set-default RC_MAP_ROLL 1 param set-default RC_MAP_THROTTLE 3 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 2faae54c77..129c4ca321 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -17,6 +17,10 @@ set OUTPUT_DEV none set OUTPUT_AUX_DEV /dev/pwm_output1 set OUTPUT_EXTRA_DEV /dev/pwm_output0 +# set these before starting the modules +param set PWM_AUX_OUT ${PWM_AUX_OUT} +param set PWM_MAIN_OUT ${PWM_OUT} + # # If mount (gimbal) control is enabled and output mode is AUX, set the aux # mixer to mount (override the airframe-specific MIXER_AUX setting). @@ -78,9 +82,16 @@ then fi fi - if [ $OUTPUT_MODE = io ] + # + # Start IO for PWM output or RC input if needed. + # + if [ $IO_PRESENT = yes ] then - . ${R}etc/init.d/rc.io + if ! px4io start + then + echo "PX4IO start failed" + tune_control play -t 18 # PROG_PX4IO_ERR + fi fi if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ] @@ -91,17 +102,6 @@ then tune_control play error fi fi - - # - # Start IO for RC input if needed. - # - if [ $IO_PRESENT = yes ] - then - if [ $OUTPUT_MODE != io ] - then - . ${R}etc/init.d/rc.io - fi - fi fi if [ $MIXER != none -a $MIXER != skip ] @@ -212,8 +212,6 @@ then fi fi -param set PWM_AUX_OUT ${PWM_AUX_OUT} - if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ] then if [ $PWM_OUT != none ] @@ -226,8 +224,6 @@ then fi fi -param set PWM_MAIN_OUT ${PWM_OUT} - if [ $EXTRA_MIXER_MODE != none ] then diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io deleted file mode 100644 index 132b2fd513..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ /dev/null @@ -1,23 +0,0 @@ -#!/bin/sh -# -# PX4IO interface init script. -# - -# If $OUTPUT_MODE indicated Hardware-int-the-loop simulation, px4io should not publish actuator_outputs, -# instead, pwm_out_sim will publish that uORB -if [ $OUTPUT_MODE = hil ] -then - set HIL_ARG $OUTPUT_MODE -fi - -if [ $IO_PRESENT = yes ] -then - if px4io start $HIL_ARG - then - # Allow PX4IO to recover from midair restarts. - px4io recovery - else - echo "PX4IO start failed" - tune_control play -t 18 # PROG_PX4IO_ERR - fi -fi diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index dfd74cd828..de7022c56a 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -70,14 +70,6 @@ then set io_file /etc/extras/px4_io-v2_default.bin fi -if px4io start -then - echo "PX4IO OK" -else - set unit_test_failure 1 - set unit_test_failure_list "${unit_test_failure_list} px4io_start" -fi - if px4io checkcrc $io_file then echo "PX4IO CRC OK" @@ -104,6 +96,15 @@ else fi fi +if px4io start +then + echo "PX4IO OK" +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} px4io_start" +fi + + # # The presence of this file suggests we're running a mount stress test # diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index daf314e3b9..f6993aa8b0 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -53,7 +53,6 @@ #include #include -#include #include #include @@ -96,7 +95,7 @@ Syslink::Syslink() : _fd(0), _queue(2, sizeof(syslink_message_t)), _writebuffer(16, sizeof(crtp_message_t)), - _rssi(RC_INPUT_RSSI_MAX), + _rssi(input_rc_s::RSSI_MAX), _bstate(BAT_DISCHARGING) { px4_sem_init(&memory_sem, 0, 0); diff --git a/msg/input_rc.msg b/msg/input_rc.msg index 8f333e7f66..d26c31ea0f 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -23,6 +23,7 @@ uint64 timestamp_last_signal # last valid reception time uint8 channel_count # number of channels actually being seen +int8 RSSI_MAX = 100 int32 rssi # receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception bool rc_failsafe # explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly. diff --git a/msg/manual_control_switches.msg b/msg/manual_control_switches.msg index 444120e595..d140743761 100644 --- a/msg/manual_control_switches.msg +++ b/msg/manual_control_switches.msg @@ -26,11 +26,4 @@ uint8 kill_switch # throttle kill: _NORMAL_, KILL uint8 gear_switch # landing gear switch: _DOWN_, UP uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT -# legacy "advanced" switch configuration (will be removed soon) -uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO -uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL -uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO -uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED -uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL - uint32 switch_changes # number of switch changes diff --git a/msg/px4io_status.msg b/msg/px4io_status.msg index 8d830b21f4..df072e71c2 100644 --- a/msg/px4io_status.msg +++ b/msg/px4io_status.msg @@ -6,49 +6,38 @@ float32 voltage_v # Servo rail voltage in volts float32 rssi_v # RSSI pin voltage in volts # PX4IO status flags (PX4IO_P_STATUS_FLAGS) -bool status_outputs_armed -bool status_override -bool status_rc_ok -bool status_rc_ppm -bool status_rc_dsm -bool status_rc_sbus -bool status_fmu_ok -bool status_raw_pwm -bool status_mixer_ok bool status_arm_sync -bool status_init_ok bool status_failsafe -bool status_safety_off bool status_fmu_initialized +bool status_fmu_ok +bool status_init_ok +bool status_outputs_armed +bool status_raw_pwm +bool status_rc_ok +bool status_rc_dsm +bool status_rc_ppm +bool status_rc_sbus bool status_rc_st24 bool status_rc_sumd +bool status_safety_off # PX4IO alarms (PX4IO_P_STATUS_ALARMS) -bool alarm_vbatt_low -bool alarm_temperature -bool alarm_servo_current -bool alarm_acc_current -bool alarm_fmu_lost -bool alarm_rc_lost bool alarm_pwm_error -bool alarm_vservo_fault +bool alarm_rc_lost # PX4IO arming (PX4IO_P_SETUP_ARMING) -bool arming_io_arm_ok +bool arming_failsafe_custom bool arming_fmu_armed bool arming_fmu_prearmed -bool arming_manual_override_ok -bool arming_failsafe_custom -bool arming_inair_restart_ok -bool arming_always_pwm_enable -bool arming_rc_handling_disabled -bool arming_lockdown bool arming_force_failsafe +bool arming_io_arm_ok +bool arming_lockdown bool arming_termination_failsafe -bool arming_override_immediate +uint16[8] pwm +uint16[8] pwm_disarmed +uint16[8] pwm_failsafe -int16[8] actuators -uint16[8] servos +uint16[8] pwm_rate_hz uint16[18] raw_inputs diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 55d022cabb..e44812d70d 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -4,41 +4,36 @@ uint8 FUNCTION_THROTTLE = 0 uint8 FUNCTION_ROLL = 1 uint8 FUNCTION_PITCH = 2 uint8 FUNCTION_YAW = 3 -uint8 FUNCTION_MODE = 4 -uint8 FUNCTION_RETURN = 5 -uint8 FUNCTION_POSCTL = 6 -uint8 FUNCTION_LOITER = 7 -uint8 FUNCTION_OFFBOARD = 8 -uint8 FUNCTION_ACRO = 9 -uint8 FUNCTION_FLAPS = 10 -uint8 FUNCTION_AUX_1 = 11 -uint8 FUNCTION_AUX_2 = 12 -uint8 FUNCTION_AUX_3 = 13 -uint8 FUNCTION_AUX_4 = 14 -uint8 FUNCTION_AUX_5 = 15 -uint8 FUNCTION_PARAM_1 = 16 -uint8 FUNCTION_PARAM_2 = 17 -uint8 FUNCTION_PARAM_3_5 = 18 -uint8 FUNCTION_KILLSWITCH = 19 -uint8 FUNCTION_TRANSITION = 20 -uint8 FUNCTION_GEAR = 21 -uint8 FUNCTION_ARMSWITCH = 22 -uint8 FUNCTION_STAB = 23 -uint8 FUNCTION_AUX_6 = 24 -uint8 FUNCTION_MAN = 25 -uint8 FUNCTION_FLTBTN_SLOT_1 = 26 -uint8 FUNCTION_FLTBTN_SLOT_2 = 27 -uint8 FUNCTION_FLTBTN_SLOT_3 = 28 -uint8 FUNCTION_FLTBTN_SLOT_4 = 39 -uint8 FUNCTION_FLTBTN_SLOT_5 = 30 -uint8 FUNCTION_FLTBTN_SLOT_6 = 31 +uint8 FUNCTION_RETURN = 4 +uint8 FUNCTION_LOITER = 5 +uint8 FUNCTION_OFFBOARD = 6 +uint8 FUNCTION_FLAPS = 7 +uint8 FUNCTION_AUX_1 = 8 +uint8 FUNCTION_AUX_2 = 9 +uint8 FUNCTION_AUX_3 = 10 +uint8 FUNCTION_AUX_4 = 11 +uint8 FUNCTION_AUX_5 = 12 +uint8 FUNCTION_AUX_6 = 13 +uint8 FUNCTION_PARAM_1 = 14 +uint8 FUNCTION_PARAM_2 = 15 +uint8 FUNCTION_PARAM_3_5 = 16 +uint8 FUNCTION_KILLSWITCH = 17 +uint8 FUNCTION_TRANSITION = 18 +uint8 FUNCTION_GEAR = 19 +uint8 FUNCTION_ARMSWITCH = 20 +uint8 FUNCTION_FLTBTN_SLOT_1 = 21 +uint8 FUNCTION_FLTBTN_SLOT_2 = 22 +uint8 FUNCTION_FLTBTN_SLOT_3 = 23 +uint8 FUNCTION_FLTBTN_SLOT_4 = 24 +uint8 FUNCTION_FLTBTN_SLOT_5 = 25 +uint8 FUNCTION_FLTBTN_SLOT_6 = 26 uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6 uint64 timestamp_last_valid # Timestamp of last valid RC signal float32[18] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[32] function # Functions mapping +int8[27] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout uint32 frame_drop_count # Number of dropped frames diff --git a/msg/safety.msg b/msg/safety.msg index 62046c71b7..429b76b726 100644 --- a/msg/safety.msg +++ b/msg/safety.msg @@ -1,5 +1,3 @@ uint64 timestamp # time since system start (microseconds) bool safety_switch_available # Set to true if a safety switch is connected bool safety_off # Set to true if safety is off -bool override_available # Set to true if external override system is connected -bool override_enabled # Set to true if override is engaged diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 85ee40229d..1f4034369d 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -1,7 +1,6 @@ uint64 timestamp # time since system start (microseconds) bool flag_armed # synonym for actuator_armed.armed -bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing bool flag_multicopter_position_control_enabled bool flag_control_manual_enabled # true if manual input is mixed in diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 167ae858ee..3d8fa3be0c 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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 @@ -77,16 +77,16 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; -static constexpr wq_config_t UART0{"wq:UART0", 1536, -21}; -static constexpr wq_config_t UART1{"wq:UART1", 1536, -22}; -static constexpr wq_config_t UART2{"wq:UART2", 1536, -23}; -static constexpr wq_config_t UART3{"wq:UART3", 1536, -24}; -static constexpr wq_config_t UART4{"wq:UART4", 1536, -25}; -static constexpr wq_config_t UART5{"wq:UART5", 1536, -26}; -static constexpr wq_config_t UART6{"wq:UART6", 1536, -27}; -static constexpr wq_config_t UART7{"wq:UART7", 1536, -28}; -static constexpr wq_config_t UART8{"wq:UART8", 1536, -29}; -static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1536, -30}; +static constexpr wq_config_t UART0{"wq:UART0", 1632, -21}; +static constexpr wq_config_t UART1{"wq:UART1", 1632, -22}; +static constexpr wq_config_t UART2{"wq:UART2", 1632, -23}; +static constexpr wq_config_t UART3{"wq:UART3", 1632, -24}; +static constexpr wq_config_t UART4{"wq:UART4", 1632, -25}; +static constexpr wq_config_t UART5{"wq:UART5", 1632, -26}; +static constexpr wq_config_t UART6{"wq:UART6", 1632, -27}; +static constexpr wq_config_t UART7{"wq:UART7", 1632, -28}; +static constexpr wq_config_t UART8{"wq:UART8", 1632, -29}; +static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1632, -30}; static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50}; diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index 74abff2abc..44194d0b0d 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -8,7 +8,6 @@ set(tests bezier bitset bson - conv dataman file2 float diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index 702483c198..2214e4406f 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -5,7 +5,6 @@ param load param set CBRK_SUPPLY_CHK 894281 -param set SYS_RESTART_TYPE 0 dataman start diff --git a/posix-configs/SITL/init/test/test_imu_filtering b/posix-configs/SITL/init/test/test_imu_filtering index 9590e39124..1af06e4b9e 100644 --- a/posix-configs/SITL/init/test/test_imu_filtering +++ b/posix-configs/SITL/init/test/test_imu_filtering @@ -5,7 +5,6 @@ param load param set CBRK_SUPPLY_CHK 894281 -param set SYS_RESTART_TYPE 0 dataman start diff --git a/posix-configs/SITL/init/test/test_mavlink b/posix-configs/SITL/init/test/test_mavlink index d75544238a..ad02885bd6 100644 --- a/posix-configs/SITL/init/test/test_mavlink +++ b/posix-configs/SITL/init/test/test_mavlink @@ -5,7 +5,6 @@ param load param set CBRK_SUPPLY_CHK 894281 -param set SYS_RESTART_TYPE 0 dataman start diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index f88e5f376c..2eceb11f8d 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -9,7 +9,6 @@ param set BAT1_N_CELLS 3 param set CBRK_SUPPLY_CHK 894281 param set MAV_TYPE 22 param set VT_TYPE 2 -param set SYS_RESTART_TYPE 0 dataman start diff --git a/posix-configs/SITL/init/test/tests_all b/posix-configs/SITL/init/test/tests_all index 063dee50da..1017bb6df7 100644 --- a/posix-configs/SITL/init/test/tests_all +++ b/posix-configs/SITL/init/test/tests_all @@ -5,7 +5,6 @@ param load param set CBRK_SUPPLY_CHK 894281 -param set SYS_RESTART_TYPE 0 dataman start diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index e1090ac1bb..dd98e9180f 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -215,9 +215,6 @@ typedef uint16_t servo_position_t; /** force safety switch on (to enable use of safety switch) */ #define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 28) -/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ -#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _PX4_IOC(_PWM_SERVO_BASE, 32) - /** set auxillary output mode */ #define PWM_SERVO_ENTER_TEST_MODE 18 #define PWM_SERVO_EXIT_TEST_MODE 19 diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h deleted file mode 100644 index 6ab1d0b49f..0000000000 --- a/src/drivers/drv_rc_input.h +++ /dev/null @@ -1,80 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file drv_rc_input.h - * - * R/C input interface. - */ - -#ifndef _DRV_RC_INPUT_H -#define _DRV_RC_INPUT_H - -#include -#include -#include - -#include "drv_orb_dev.h" - -/** - * Path for the default R/C input device. - * - * Note that on systems with more than one R/C input path (e.g. - * PX4FMU with PX4IO connected) there may be other devices that - * respond to this protocol. - * - * Input data may be obtained by subscribing to the input_rc - * object, or by poll/reading from the device. - */ -#define RC_INPUT0_DEVICE_PATH "/dev/input_rc0" - -/** - * Maximum RSSI value - */ -#define RC_INPUT_RSSI_MAX 100 - -/** - * Input signal type, value is a control position from zero to 100 - * percent. - */ -typedef uint16_t rc_input_t; - -#define _RC_INPUT_BASE 0x2b00 - -/** Enable RSSI input via ADC */ -#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1) - -/** Enable RSSI input via PWM signal */ -#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2) - -#endif /* _DRV_RC_INPUT_H */ diff --git a/src/drivers/px4io/CMakeLists.txt b/src/drivers/px4io/CMakeLists.txt index d4af0602c8..d3320ca2f0 100644 --- a/src/drivers/px4io/CMakeLists.txt +++ b/src/drivers/px4io/CMakeLists.txt @@ -42,7 +42,7 @@ px4_add_module( DEPENDS arch_px4io_serial circuit_breaker - mixer + mixer_module ) # include the px4io binary in ROMFS diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a50b569d8e..8dc09e4b46 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -37,65 +37,45 @@ * * PX4IO is connected via DMA enabled high-speed UART. */ - +#include #include -#include -#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include - - #include -#include -#include -#include #include #include #include +#include +#include -#include - +#include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include +#include #include +#include #include #include -#include +#include #include -#include #include #include -#include #include #include -#include -#include #include @@ -108,12 +88,10 @@ #include "px4io_driver.h" #define PX4IO_SET_DEBUG _IOC(0xff00, 0) -#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) -#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2) -#define PX4IO_CHECK_CRC _IOC(0xff00, 3) +#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 1) +#define PX4IO_CHECK_CRC _IOC(0xff00, 2) -#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz -#define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz +static constexpr unsigned MIN_TOPIC_UPDATE_INTERVAL = 2500; // 2.5 ms -> 400 Hz using namespace time_literals; @@ -122,7 +100,7 @@ using namespace time_literals; * * Encapsulates PX4FMU to PX4IO communications modeled as file operations. */ -class PX4IO : public cdev::CDev +class PX4IO : public cdev::CDev, public ModuleBase, public OutputModuleInterface { public: /** @@ -130,38 +108,24 @@ public: * * Initialize all class variables. */ - PX4IO(device::Device *interface); + PX4IO() = delete; + explicit PX4IO(device::Device *interface); - /** - * Destructor. - * - * Wait for worker thread to terminate. - */ - virtual ~PX4IO(); + ~PX4IO() override; /** * Initialize the PX4IO class. * * Retrieve relevant initial system parameters. Initialize PX4IO registers. */ - virtual int init(); - - /** - * Initialize the PX4IO class. - * - * Retrieve relevant initial system parameters. Initialize PX4IO registers. - * - * @param disable_rc_handling set to true to forbid override / RC handling on IO - * @param hitl_mode set to suppress publication of actuator_outputs - instead defer to pwm_out_sim - */ - int init(bool disable_rc_handling, bool hitl_mode); + int init() override; /** * Detect if a PX4IO is connected. * * Only validate if there is a PX4IO to talk to. */ - virtual int detect(); + int detect(); /** * IO Control handler. @@ -172,21 +136,21 @@ public: * @param[in] cmd the IOCTL command * @param[in] the IOCTL command parameter (optional) */ - virtual int ioctl(file *filp, int cmd, unsigned long arg); - - /** - * Disable RC input handling - */ - int disable_rc_handling(); + int ioctl(file *filp, int cmd, unsigned long arg) override; /** * Print IO status. * * Print all relevant IO status information * - * @param extended_status Shows more verbose information (in particular RC config) */ - void print_status(bool extended_status); + int print_status(); + + static int custom_command(int argc, char *argv[]); + + static int print_usage(const char *reason = nullptr); + + static int task_spawn(int argc, char *argv[]); /** * Fetch and print debug console output. @@ -198,35 +162,46 @@ public: * * @param is_fail true for failure condition, false for normal operation. */ - void test_fmu_fail(bool is_fail) - { - _test_fmu_fail = is_fail; - }; + void test_fmu_fail(bool is_fail) { _test_fmu_fail = is_fail; }; - inline uint16_t system_status() const {return _status;} + uint16_t system_status() const { return _status; } + + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) override; private: - device::Device *_interface; + void Run() override; - unsigned _hardware; ///< Hardware revision - unsigned _max_actuators; ///< Maximum # of actuators supported by PX4IO - unsigned _max_controls; ///< Maximum # of controls supported by PX4IO - unsigned _max_rc_input; ///< Maximum receiver channels supported by PX4IO - unsigned _max_relays; ///< Maximum relays supported by PX4IO - unsigned _max_transfer; ///< Maximum number of I2C transfers supported by PX4IO + void updateDisarmed(); + void updateFailsafe(); - bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values - unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels - uint64_t _rc_last_valid; ///< last valid timestamp + static int checkcrc(int argc, char *argv[]); + static int bind(int argc, char *argv[]); + static int lockdown(int argc, char *argv[]); + static int monitor(); - volatile int _task; ///< worker task id - volatile bool _task_should_exit; ///< worker terminate flag + static constexpr int PX4IO_MAX_ACTUATORS = 8; - orb_advert_t _mavlink_log_pub; ///< mavlink log pub + device::Device *const _interface; - perf_counter_t _perf_update; ///< local performance counter for status updates - perf_counter_t _perf_write; ///< local performance counter for PWM control writes - perf_counter_t _perf_sample_latency; ///< total system latency (based on passed-through timestamp) + unsigned _hardware{0}; ///< Hardware revision + unsigned _max_actuators{0}; ///< Maximum # of actuators supported by PX4IO + unsigned _max_controls{0}; ///< Maximum # of controls supported by PX4IO + unsigned _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO + unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO + + uint64_t _rc_last_valid{0}; ///< last valid timestamp + + int _class_instance{-1}; + + hrt_abstime _poll_last{0}; + + orb_advert_t _mavlink_log_pub{nullptr}; ///< mavlink log pub + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _interface_read_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": interface read")}; + perf_counter_t _interface_write_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": interface write")}; /* cached IO state */ uint16_t _status{0}; ///< Various IO status flags @@ -235,55 +210,32 @@ private: uint16_t _last_written_arming_s{0}; ///< the last written arming state reg uint16_t _last_written_arming_c{0}; ///< the last written arming state reg - /* subscribed topics */ - int _t_actuator_controls_0; ///< actuator controls group 0 topic - - uORB::Subscription _t_actuator_controls_1{ORB_ID(actuator_controls_1)}; ///< actuator controls group 1 topic - uORB::Subscription _t_actuator_controls_2{ORB_ID(actuator_controls_2)};; ///< actuator controls group 2 topic - uORB::Subscription _t_actuator_controls_3{ORB_ID(actuator_controls_3)};; ///< actuator controls group 3 topic uORB::Subscription _t_actuator_armed{ORB_ID(actuator_armed)}; ///< system armed control topic - uORB::Subscription _t_vehicle_control_mode{ORB_ID(vehicle_control_mode)}; ///< vehicle control mode topic uORB::Subscription _t_vehicle_command{ORB_ID(vehicle_command)}; ///< vehicle command topic uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; hrt_abstime _last_status_publish{0}; - bool _param_update_force; ///< force a parameter update + bool _param_update_force{true}; ///< force a parameter update /* advertised topics */ - uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; - uORB::PublicationMulti _to_outputs{ORB_ID(actuator_outputs)}; - uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits)}; - uORB::Publication _px4io_status_pub{ORB_ID(px4io_status)}; - uORB::Publication _to_safety{ORB_ID(safety)}; + uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; + uORB::PublicationMulti _to_safety{ORB_ID(safety)}; + uORB::Publication _px4io_status_pub{ORB_ID(px4io_status)}; safety_s _safety{}; - bool _primary_pwm_device; ///< true if we are the default PWM output - bool _lockdown_override; ///< allow to override the safety lockdown - bool _armed; ///< wether the system is armed - bool _override_available; ///< true if manual reversion mode is enabled + bool _lockdown_override{false}; ///< override the safety lockdown - bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled - bool _in_esc_calibration_mode; ///< do not send control outputs to IO (used for esc calibration) + int32_t _thermal_control{-1}; ///< thermal control state + bool _analog_rc_rssi_stable{false}; ///< true when analog RSSI input is stable + float _analog_rc_rssi_volt{-1.f}; ///< analog RSSI voltage - int32_t _rssi_pwm_chan; ///< RSSI PWM input channel - int32_t _rssi_pwm_max; ///< max RSSI input on PWM channel - int32_t _rssi_pwm_min; ///< min RSSI input on PWM channel - int32_t _thermal_control; ///< thermal control state - bool _analog_rc_rssi_stable; ///< true when analog RSSI input is stable - float _analog_rc_rssi_volt; ///< analog RSSI voltage + bool _test_fmu_fail{false}; ///< To test what happens if IO loses FMU + bool _in_test_mode{false}; ///< true if PWM_SERVO_ENTER_TEST_MODE is active - bool _test_fmu_fail; ///< To test what happens if IO loses FMU - - struct MotorTest { - uORB::Subscription test_motor_sub{ORB_ID(test_motor)}; - bool in_test_mode{false}; - hrt_abstime timeout{0}; - }; - MotorTest _motor_test; - bool _hitl_mode; ///< Hardware-in-the-loop simulation mode - don't publish actuator_outputs + MixingOutput _mixing_output{PX4IO_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true}; bool _pwm_min_configured{false}; bool _pwm_max_configured{false}; @@ -291,36 +243,11 @@ private: bool _pwm_dis_configured{false}; bool _pwm_rev_configured{false}; - /** - * Trampoline to the worker task - */ - static int task_main_trampoline(int argc, char *argv[]); - - /** - * worker task - */ - void task_main(); - - /** - * Send controls for one group to IO - */ - int io_set_control_state(unsigned group); - - /** - * Send all controls to IO - */ - int io_set_control_groups(); - /** * Update IO's arming-related state */ int io_set_arming_state(); - /** - * Push RC channel configuration to IO. - */ - int io_set_rc_config(); - /** * Fetch status and alarms from IO * @@ -328,29 +255,14 @@ private: */ int io_get_status(); - /** - * Disable RC input handling - */ - int io_disable_rc_handling(); - /** * Fetch RC inputs from IO. * * @param input_rc Input structure to populate. * @return OK if data was returned. */ - int io_get_raw_rc_input(input_rc_s &input_rc); - - /** - * Fetch and publish raw RC input data. - */ int io_publish_raw_rc(); - /** - * Fetch and publish the PWM servo outputs. - */ - int io_publish_pwm_outputs(); - /** * write register(s) * @@ -434,107 +346,51 @@ private: void update_params(); - /** - * check and handle test_motor topic updates - */ - void handle_motor_test(); - - /* do not allow to copy this class due to ptr data members */ - PX4IO(const PX4IO &); - PX4IO operator=(const PX4IO &); + DEFINE_PARAMETERS( + (ParamInt) _param_pwm_sbus_mode, + (ParamInt) _param_rc_rssi_pwm_chan, + (ParamInt) _param_rc_rssi_pwm_max, + (ParamInt) _param_rc_rssi_pwm_min, + (ParamInt) _param_sens_en_themal, + (ParamInt) _param_sys_hitl, + (ParamInt) _param_sys_use_io + ) }; -namespace -{ -PX4IO *g_dev = nullptr; -} - #define PX4IO_DEVICE_PATH "/dev/px4io" PX4IO::PX4IO(device::Device *interface) : CDev(PX4IO_DEVICE_PATH), - _interface(interface), - _hardware(0), - _max_actuators(0), - _max_controls(0), - _max_rc_input(0), - _max_relays(0), - _max_transfer(16), /* sensible default */ - _rc_handling_disabled(false), - _rc_chan_count(0), - _rc_last_valid(0), - _task(-1), - _task_should_exit(false), - _mavlink_log_pub(nullptr), - _perf_update(perf_alloc(PC_ELAPSED, "io update")), - _perf_write(perf_alloc(PC_ELAPSED, "io write")), - _perf_sample_latency(perf_alloc(PC_ELAPSED, "io control latency")), - _t_actuator_controls_0(-1), - _param_update_force(false), - _primary_pwm_device(false), - _lockdown_override(false), - _armed(false), - _override_available(false), - _cb_flighttermination(true), - _in_esc_calibration_mode(false), - _rssi_pwm_chan(0), - _rssi_pwm_max(0), - _rssi_pwm_min(0), - _thermal_control(-1), - _analog_rc_rssi_stable(false), - _analog_rc_rssi_volt(-1.0f), - _test_fmu_fail(false), - _hitl_mode(false) + OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(PX4IO_SERIAL_DEVICE)), + _interface(interface) { - /* we need this potentially before it could be set in task_main */ - g_dev = this; + _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); + _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); } PX4IO::~PX4IO() { - /* tell the task we want it to go away */ - _task_should_exit = true; + delete _interface; - /* spin waiting for the task to stop */ - for (unsigned i = 0; (i < 10) && (_task != -1); i++) { - /* give it another 100ms */ - px4_usleep(100000); - } - - /* well, kill it anyway, though this will probably crash */ - if (_task != -1) { - task_delete(_task); - } - - if (_interface != nullptr) { - delete _interface; - } - - /* In the case the task did not exit - * clean up the alternate device node - */ - if (_primary_pwm_device) { - unregister_driver(PWM_OUTPUT0_DEVICE_PATH); - _primary_pwm_device = false; + /* clean up the alternate device node */ + if (_class_instance >= 0) { + unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); } /* deallocate perfs */ - perf_free(_perf_update); - perf_free(_perf_write); - perf_free(_perf_sample_latency); - - g_dev = nullptr; + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_interface_read_perf); + perf_free(_interface_write_perf); } int PX4IO::detect() { - int ret; - - if (_task == -1) { + if (!is_running()) { /* do regular cdev init */ - ret = CDev::init(); + int ret = CDev::init(); if (ret != OK) { return ret; @@ -563,37 +419,26 @@ PX4IO::detect() return 0; } -int -PX4IO::init(bool rc_handling_disabled, bool hitl_mode) +bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) { - _rc_handling_disabled = rc_handling_disabled; - _hitl_mode = hitl_mode; - return init(); -} + SmartLock lock_guard(_lock); -int -PX4IO::init() -{ - int ret; - param_t sys_restart_param; - int32_t sys_restart_val = DM_INIT_REASON_VOLATILE; - - sys_restart_param = param_find("SYS_RESTART_TYPE"); - - if (sys_restart_param != PARAM_INVALID) { - /* Indicate restart type is unknown */ - int32_t prev_val; - param_get(sys_restart_param, &prev_val); - - if (prev_val != DM_INIT_REASON_POWER_ON) { - param_set_no_notification(sys_restart_param, &sys_restart_val); - } + if (!_test_fmu_fail && !_in_test_mode) { + /* output to the servos */ + io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs); } + return true; +} + +int PX4IO::init() +{ /* do regular cdev init */ - ret = CDev::init(); + int ret = CDev::init(); if (ret != OK) { + PX4_ERR("init failed %d", ret); return ret; } @@ -624,12 +469,10 @@ PX4IO::init() _hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION); _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); - _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); - if ((_max_actuators < 1) || (_max_actuators > 16) || - (_max_relays > 32) || + if ((_max_actuators < 1) || (_max_actuators > PX4IO_MAX_ACTUATORS) || (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { @@ -656,18 +499,7 @@ PX4IO::init() _max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS; } - param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); - param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); - param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); - - /* - * Check for IO flight state - if FMU was flagged to be in - * armed state, FMU is recovering from an in-air reset. - * Read back status and request the commander to arm - * in this case. - */ - - uint16_t reg; + uint16_t reg = 0; /* get IO's last seen FMU state */ ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); @@ -676,567 +508,207 @@ PX4IO::init() return ret; } - /* - * in-air restart is only tried if the IO board reports it is - * already armed, and has been configured for in-air restart - */ - if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && - (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - - /* get a status update from IO */ - io_get_status(); - - mavlink_log_emergency(&_mavlink_log_pub, "RECOVERING FROM FMU IN-AIR RESTART\t"); - events::send(events::ID("px4io_inair_restart_recovery"), events::Log::Alert, - "Recovering from in-air restart"); - - /* WARNING: COMMANDER app/vehicle status must be initialized. - * If this fails (or the app is not started), worst-case IO - * remains untouched (so manual override is still available). - */ - - uORB::Subscription actuator_armed_sub{ORB_ID(actuator_armed)}; - - /* fill with initial values, clear updated flag */ - actuator_armed_s actuator_armed{}; - uint64_t try_start_time = hrt_absolute_time(); - - /* keep checking for an update, ensure we got a arming information, - not something that was published a long time ago. */ - do { - if (actuator_armed_sub.update(&actuator_armed)) { - // updated data, exit loop - break; - } - - /* wait 10 ms */ - px4_usleep(10000); - - /* abort after 3s */ - if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) { - mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (1), abort\t"); - events::send(events::ID("px4io_inair_restart_recovery_failed"), events::Log::Emergency, - "Failed to recover from in-air restart, aborting initialization"); - return 1; - } - - } while (true); - - /* send this to itself */ - param_t sys_id_param = param_find("MAV_SYS_ID"); - param_t comp_id_param = param_find("MAV_COMP_ID"); - - int32_t sys_id; - int32_t comp_id; - - if (param_get(sys_id_param, &sys_id)) { - errx(1, "PRM SYSID"); - } - - if (param_get(comp_id_param, &comp_id)) { - errx(1, "PRM CMPID"); - } - - /* prepare vehicle command */ - vehicle_command_s vcmd = {}; - vcmd.target_system = (uint8_t)sys_id; - vcmd.target_component = (uint8_t)comp_id; - vcmd.source_system = (uint8_t)sys_id; - vcmd.source_component = (uint8_t)comp_id; - vcmd.confirmation = true; /* ask to confirm command */ - - if (reg & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) { - mavlink_log_emergency(&_mavlink_log_pub, "IO is in failsafe, force failsafe\t"); - events::send(events::ID("px4io_init_in_failsafe"), events::Log::Emergency, - "IO is in failsafe, forcing flight termination"); - /* send command to terminate flight via command API */ - vcmd.timestamp = hrt_absolute_time(); - vcmd.param1 = 1.0f; /* request flight termination */ - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION; - - /* send command once */ - uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; - vcmd_pub.publish(vcmd); - - /* spin here until IO's state has propagated into the system */ - do { - actuator_armed_sub.update(&actuator_armed); - - /* wait 50 ms */ - px4_usleep(50000); - - /* abort after 2s */ - if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) { - mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (3), abort\t"); - events::send(events::ID("px4io_inair_restart_recovery_failed3"), events::Log::Emergency, - "Failed to recover from in-air restart, aborting initialization"); - return 1; - } - - /* re-send if necessary */ - if (!actuator_armed.force_failsafe) { - vcmd_pub.publish(vcmd); - PX4_WARN("re-sending flight termination cmd"); - } - - /* keep waiting for state change for 2 s */ - } while (!actuator_armed.force_failsafe); - } - - /* send command to arm system via command API */ - vcmd.timestamp = hrt_absolute_time(); - vcmd.param1 = 1.0f; /* request arming */ - vcmd.param3 = 1234.f; /* mark the command coming from IO (for in-air restoring) */ - vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; - - /* send command once */ - uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; - vcmd_pub.publish(vcmd); - - /* spin here until IO's state has propagated into the system */ - do { - actuator_armed_sub.update(&actuator_armed); - - /* wait 50 ms */ - px4_usleep(50000); - - /* abort after 2s */ - if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) { - mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (2), abort\t"); - events::send(events::ID("px4io_inair_restart_recovery_failed2"), events::Log::Emergency, - "Failed to recover from in-air restart, aborting initialization"); - return 1; - } - - /* re-send if necessary */ - if (!actuator_armed.armed) { - vcmd_pub.publish(vcmd); - PX4_WARN("re-sending arm cmd"); - } - - /* keep waiting for state change for 2 s */ - } while (!actuator_armed.armed); - - /* Indicate restart type is in-flight */ - sys_restart_val = DM_INIT_REASON_IN_FLIGHT; - int32_t prev_val; - param_get(sys_restart_param, &prev_val); - - if (prev_val != sys_restart_val) { - param_set(sys_restart_param, &sys_restart_val); - } - - /* regular boot, no in-air restart, init IO */ - - } else { - - /* dis-arm IO before touching anything */ - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, - PX4IO_P_SETUP_ARMING_FMU_ARMED | - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | - PX4IO_P_SETUP_ARMING_LOCKDOWN, 0); - - if (_rc_handling_disabled) { - ret = io_disable_rc_handling(); - - if (ret != OK) { - PX4_ERR("failed disabling RC handling"); - return ret; - } - - } else { - /* publish RC config to IO */ - ret = io_set_rc_config(); - - if (ret != OK) { - mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail\t"); - events::send(events::ID("px4io_io_rc_config_upload_failed"), events::Log::Critical, - "IO RC config upload failed, aborting initialization"); - return ret; - } - } - - /* Indicate restart type is power on */ - sys_restart_val = DM_INIT_REASON_POWER_ON; - int32_t prev_val; - param_get(sys_restart_param, &prev_val); - - if (prev_val != sys_restart_val) { - param_set(sys_restart_param, &sys_restart_val); - } + /* dis-arm IO before touching anything */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_LOCKDOWN, + 0); + if (ret != OK) { + mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail\t"); + events::send(events::ID("px4io_io_rc_config_upload_failed"), events::Log::Critical, + "IO RC config upload failed, aborting initialization"); + return ret; } /* set safety to off if circuit breaker enabled */ if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) { - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - ret = register_driver(PWM_OUTPUT0_DEVICE_PATH, &fops, 0666, (void *)this); + if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) { + _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); + _mixing_output.setDriverInstance(_class_instance); - if (ret == OK) { - PX4_INFO("default PWM output device"); - _primary_pwm_device = true; + _mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL); } - /* ensure PWM limits are applied before any other module starts */ update_params(); - /* start the IO interface task */ - _task = px4_task_spawn_cmd("px4io", - SCHED_DEFAULT, - SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 1536, - (px4_main_t)&PX4IO::task_main_trampoline, - nullptr); - - if (_task < 0) { - PX4_ERR("task start failed: %d", errno); - return -errno; - } + ScheduleNow(); return OK; } -int -PX4IO::task_main_trampoline(int argc, char *argv[]) +void PX4IO::updateDisarmed() { - g_dev->task_main(); - return 0; + pwm_output_values pwm{}; + + for (unsigned i = 0; i < _max_actuators; i++) { + pwm.values[i] = _mixing_output.disarmedValue(i); + } + + io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm.values, _max_actuators); } -void -PX4IO::task_main() +void PX4IO::updateFailsafe() { - hrt_abstime poll_last = 0; - hrt_abstime orb_check_last = 0; + pwm_output_values pwm{}; - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0)); - orb_set_interval(_t_actuator_controls_0, 2); /* default to 500Hz */ - - if (_t_actuator_controls_0 < 0) { - PX4_ERR("actuator subscription failed"); - goto out; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm.values[i] = _mixing_output.failsafeValue(i); } - /* Fetch initial flight termination circuit breaker state */ - _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, _max_actuators); +} - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _t_actuator_controls_0; - fds[0].events = POLLIN; +void PX4IO::Run() +{ + if (should_exit()) { + ScheduleClear(); + _mixing_output.unregister(); - _param_update_force = true; - - /* lock against the ioctl handler */ - lock(); - - /* loop talking to IO */ - while (!_task_should_exit) { - - /* sleep waiting for topic updates, but no more than 20ms */ - unlock(); - int ret = ::poll(fds, 1, 20); - lock(); - - /* this would be bad... */ - if (ret < 0) { - warnx("poll error %d", errno); - continue; - } - - perf_begin(_perf_update); - hrt_abstime now = hrt_absolute_time(); - - /* if we have new control data from the ORB, handle it */ - if (fds[0].revents & POLLIN) { - - /* we're not nice to the lower-priority control groups and only check them - when the primary group updated (which is now). */ - (void)io_set_control_groups(); - } - - if (!_armed && !_lockdown_override) { - handle_motor_test(); - - } else { - _motor_test.in_test_mode = false; - } - - if (now >= poll_last + IO_POLL_INTERVAL) { - /* run at 50-250Hz */ - poll_last = now; - - /* pull status and alarms from IO */ - io_get_status(); - - /* get raw R/C input from IO */ - io_publish_raw_rc(); - - /* fetch PWM outputs from IO */ - io_publish_pwm_outputs(); - - /* check updates on uORB topics and handle it */ - bool updated = false; - - /* arming state */ - updated = _t_actuator_armed.updated(); - - if (!updated) { - updated = _t_vehicle_control_mode.updated(); - } - - if (updated) { - io_set_arming_state(); - } - } - - if (!_armed && (now >= orb_check_last + ORB_CHECK_INTERVAL)) { - /* run at 5Hz */ - orb_check_last = now; - - /* vehicle command */ - if (_t_vehicle_command.updated()) { - vehicle_command_s cmd{}; - _t_vehicle_command.copy(&cmd); - - // Check for a DSM pairing command - if (((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { - int bind_arg; - - switch ((int)cmd.param2) { - case 0: - bind_arg = DSM2_BIND_PULSES; - break; - - case 1: - bind_arg = DSMX_BIND_PULSES; - break; - - case 2: - default: - bind_arg = DSMX8_BIND_PULSES; - break; - } - - int dsm_ret = dsm_bind_ioctl(bind_arg); - - /* publish ACK */ - if (dsm_ret == OK) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - - } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); - } - } - } - - /* - * If parameters have changed, re-send RC mappings to IO - * - * XXX this may be a bit spammy - */ - - // check for parameter updates - if (_parameter_update_sub.updated() || _param_update_force) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - _param_update_force = false; - - if (!_rc_handling_disabled) { - /* re-upload RC input config as it may have changed */ - io_set_rc_config(); - } - - /* send RC throttle failsafe value to IO */ - int32_t failsafe_param_val; - param_t failsafe_param = param_find("RC_FAILS_THR"); - - if (failsafe_param != PARAM_INVALID) { - - param_get(failsafe_param, &failsafe_param_val); - - if (failsafe_param_val > 0) { - - uint16_t failsafe_thr = failsafe_param_val; - int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); - - if (pret != OK) { - mavlink_log_critical(&_mavlink_log_pub, "failsafe upload failed, FS: %d us\t", (int)failsafe_thr); - events::send(events::ID("px4io_failsafe_upload_failed"), events::Log::Critical, - "Failsafe upload failed, threshold: {1} us", failsafe_thr); - } - } - } - - /* Check if the IO safety circuit breaker has been updated */ - bool circuit_breaker_io_safety_enabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY); - /* Bypass IO safety switch logic by setting FORCE_SAFETY_OFF */ - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, circuit_breaker_io_safety_enabled); - - /* Check if the flight termination circuit breaker has been updated */ - _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); - /* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */ - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION, !_cb_flighttermination); - - param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); - param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); - param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); - - param_t thermal_param = param_find("SENS_EN_THERMAL"); - - if (thermal_param != PARAM_INVALID) { - - int32_t thermal_p; - param_get(thermal_param, &thermal_p); - - if (thermal_p != _thermal_control || _param_update_force) { - - _thermal_control = thermal_p; - /* set power management state for thermal */ - uint16_t tctrl; - - if (_thermal_control < 0) { - tctrl = PX4IO_THERMAL_IGNORE; - - } else { - tctrl = PX4IO_THERMAL_OFF; - } - - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, tctrl); - } - } - - float param_val; - param_t parm_handle; - - parm_handle = param_find("TRIM_ROLL"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(param_val)); - } - - parm_handle = param_find("TRIM_PITCH"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(param_val)); - } - - parm_handle = param_find("TRIM_YAW"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(param_val)); - } - - parm_handle = param_find("FW_MAN_R_SC"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_ROLL, FLOAT_TO_REG(param_val)); - } - - parm_handle = param_find("FW_MAN_P_SC"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_PITCH, FLOAT_TO_REG(param_val)); - } - - parm_handle = param_find("FW_MAN_Y_SC"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_YAW, FLOAT_TO_REG(param_val)); - } - - /* S.BUS output */ - int32_t sbus_mode; - parm_handle = param_find("PWM_SBUS_MODE"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, &sbus_mode); - - if (sbus_mode == 1) { - /* enable S.BUS 1 */ - (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); - - } else if (sbus_mode == 2) { - /* enable S.BUS 2 */ - (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); - - } else { - /* disable S.BUS */ - (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, - (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); - } - } - - /* thrust to pwm modelling factor */ - parm_handle = param_find("THR_MDL_FAC"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THR_MDL_FAC, FLOAT_TO_REG(param_val)); - } - - /* maximum motor pwm slew rate */ - parm_handle = param_find("MOT_SLEW_MAX"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_MOTOR_SLEW_MAX, FLOAT_TO_REG(param_val)); - } - - /* air-mode */ - parm_handle = param_find("MC_AIRMODE"); - - if (parm_handle != PARAM_INVALID) { - int32_t param_val_int; - param_get(parm_handle, ¶m_val_int); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_AIRMODE, SIGNED_TO_REG(param_val_int)); - } - - update_params(); - } - - } - - perf_end(_perf_update); + exit_and_cleanup(); + return; } - unlock(); + perf_begin(_cycle_perf); + perf_count(_interval_perf); -out: - PX4_DEBUG("exiting"); + // schedule minimal update rate if there are no actuator controls + ScheduleDelayed(20_ms); - /* clean up the alternate device node */ - if (_primary_pwm_device) { - unregister_driver(PWM_OUTPUT0_DEVICE_PATH); - _primary_pwm_device = false; + /* if we have new control data from the ORB, handle it */ + if (_param_sys_hitl.get() <= 0) { + _mixing_output.update(); } - /* tell the dtor that we are exiting */ - _task = -1; - _exit(0); + SmartLock lock_guard(_lock); + + if (hrt_elapsed_time(&_poll_last) >= 20_ms) { + /* run at 50 */ + _poll_last = hrt_absolute_time(); + + /* pull status and alarms from IO */ + io_get_status(); + + /* get raw R/C input from IO */ + io_publish_raw_rc(); + } + + if (_param_sys_hitl.get() <= 0) { + /* check updates on uORB topics and handle it */ + if (_t_actuator_armed.updated()) { + io_set_arming_state(); + + // TODO: throttle + } + } + + if (!_mixing_output.armed().armed) { + /* vehicle command */ + if (_t_vehicle_command.updated()) { + vehicle_command_s cmd{}; + _t_vehicle_command.copy(&cmd); + + // Check for a DSM pairing command + if (((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { + int bind_arg; + + switch ((int)cmd.param2) { + case 0: + bind_arg = DSM2_BIND_PULSES; + break; + + case 1: + bind_arg = DSMX_BIND_PULSES; + break; + + case 2: + default: + bind_arg = DSMX8_BIND_PULSES; + break; + } + + int dsm_ret = dsm_bind_ioctl(bind_arg); + + /* publish ACK */ + if (dsm_ret == OK) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + } + } + } + + /* + * If parameters have changed, re-send RC mappings to IO + */ + + // check for parameter updates + if (_parameter_update_sub.updated() || _param_update_force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + _param_update_force = false; + + ModuleParams::updateParams(); + + update_params(); + + /* Check if the IO safety circuit breaker has been updated */ + bool circuit_breaker_io_safety_enabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY); + /* Bypass IO safety switch logic by setting FORCE_SAFETY_OFF */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, circuit_breaker_io_safety_enabled); + + /* Check if the flight termination circuit breaker has been updated */ + bool disable_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + /* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION, !disable_flighttermination); + + if (_param_sens_en_themal.get() != _thermal_control || _param_update_force) { + + _thermal_control = _param_sens_en_themal.get(); + /* set power management state for thermal */ + uint16_t tctrl; + + if (_thermal_control < 0) { + tctrl = PX4IO_THERMAL_IGNORE; + + } else { + tctrl = PX4IO_THERMAL_OFF; + } + + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, tctrl); + } + + /* S.BUS output */ + if (_param_pwm_sbus_mode.get() == 1) { + /* enable S.BUS 1 */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + + } else if (_param_pwm_sbus_mode.get() == 2) { + /* enable S.BUS 2 */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + + } else { + /* disable S.BUS */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, + (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + } + } + } + + _mixing_output.updateSubscriptions(false, true); + + perf_end(_cycle_perf); } void PX4IO::update_params() { - // skip update when armed - if (_armed) { + // skip update when armed or PWM disabled + if (_mixing_output.armed().armed || _class_instance == -1) { return; } @@ -1266,120 +738,107 @@ void PX4IO::update_params() // PWM_MAIN_MINx if (!_pwm_min_configured) { - pwm_output_values pwm{}; - pwm.channel_count = _max_actuators; - for (unsigned i = 0; i < _max_actuators; i++) { sprintf(str, "%s_MIN%u", prefix, i + 1); int32_t pwm_min = -1; if (param_get(param_find(str), &pwm_min) == PX4_OK) { if (pwm_min >= 0) { - pwm.values[i] = math::constrain(pwm_min, static_cast(PWM_LOWEST_MIN), static_cast(PWM_HIGHEST_MIN)); + _mixing_output.minValue(i) = math::constrain(pwm_min, static_cast(PWM_LOWEST_MIN), + static_cast(PWM_HIGHEST_MIN)); - if (pwm_min != pwm.values[i]) { - int32_t pwm_min_new = pwm.values[i]; + if (pwm_min != _mixing_output.minValue(i)) { + int32_t pwm_min_new = _mixing_output.minValue(i); param_set(param_find(str), &pwm_min_new); } } else if (pwm_default_channel_mask & 1 << i) { - pwm.values[i] = pwm_min_default; + _mixing_output.minValue(i) = pwm_min_default; } } } - if (io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm.values, pwm.channel_count) == OK) { - _pwm_min_configured = true; - } + _pwm_min_configured = true; } // PWM_MAIN_MAXx if (!_pwm_max_configured) { - pwm_output_values pwm{}; - pwm.channel_count = _max_actuators; - for (unsigned i = 0; i < _max_actuators; i++) { sprintf(str, "%s_MAX%u", prefix, i + 1); int32_t pwm_max = -1; if (param_get(param_find(str), &pwm_max) == PX4_OK) { if (pwm_max >= 0) { - pwm.values[i] = math::constrain(pwm_max, static_cast(PWM_LOWEST_MAX), static_cast(PWM_HIGHEST_MAX)); + _mixing_output.maxValue(i) = math::constrain(pwm_max, static_cast(PWM_LOWEST_MAX), + static_cast(PWM_HIGHEST_MAX)); - if (pwm_max != pwm.values[i]) { - int32_t pwm_max_new = pwm.values[i]; + if (pwm_max != _mixing_output.maxValue(i)) { + int32_t pwm_max_new = _mixing_output.maxValue(i); param_set(param_find(str), &pwm_max_new); } } else if (pwm_default_channel_mask & 1 << i) { - pwm.values[i] = pwm_max_default; + _mixing_output.maxValue(i) = pwm_max_default; } } } - if (io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm.values, pwm.channel_count) == OK) { - _pwm_max_configured = true; - } + _pwm_max_configured = true; } // PWM_MAIN_FAILx if (!_pwm_fail_configured) { - pwm_output_values pwm{}; - pwm.channel_count = _max_actuators; - for (unsigned i = 0; i < _max_actuators; i++) { sprintf(str, "%s_FAIL%u", prefix, i + 1); int32_t pwm_fail = -1; if (param_get(param_find(str), &pwm_fail) == PX4_OK) { if (pwm_fail >= 0) { - pwm.values[i] = math::constrain(pwm_fail, static_cast(0), static_cast(PWM_HIGHEST_MAX)); + _mixing_output.failsafeValue(i) = math::constrain(pwm_fail, static_cast(0), + static_cast(PWM_HIGHEST_MAX)); - if (pwm_fail != pwm.values[i]) { - int32_t pwm_fail_new = pwm.values[i]; + if (pwm_fail != _mixing_output.failsafeValue(i)) { + int32_t pwm_fail_new = _mixing_output.failsafeValue(i); param_set(param_find(str), &pwm_fail_new); } } } } - if (io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, pwm.channel_count) == OK) { - _pwm_fail_configured = true; - } + _pwm_fail_configured = true; + updateFailsafe(); } // PWM_MAIN_DISx if (!_pwm_dis_configured) { - pwm_output_values pwm{}; - pwm.channel_count = _max_actuators; - for (unsigned i = 0; i < _max_actuators; i++) { sprintf(str, "%s_DIS%u", prefix, i + 1); int32_t pwm_dis = -1; if (param_get(param_find(str), &pwm_dis) == PX4_OK) { if (pwm_dis >= 0) { - pwm.values[i] = math::constrain(pwm_dis, static_cast(0), static_cast(PWM_HIGHEST_MAX)); + _mixing_output.disarmedValue(i) = math::constrain(pwm_dis, static_cast(0), + static_cast(PWM_HIGHEST_MAX)); - if (pwm_dis != pwm.values[i]) { - int32_t pwm_dis_new = pwm.values[i]; + if (pwm_dis != _mixing_output.disarmedValue(i)) { + int32_t pwm_dis_new = _mixing_output.disarmedValue(i); param_set(param_find(str), &pwm_dis_new); } } else if (pwm_default_channel_mask & 1 << i) { - pwm.values[i] = pwm_disarmed_default; + _mixing_output.disarmedValue(i) = pwm_disarmed_default; } } } - if (io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm.values, pwm.channel_count) == OK) { - _pwm_dis_configured = true; - } + _pwm_dis_configured = true; + updateDisarmed(); } // PWM_MAIN_REVx if (!_pwm_rev_configured) { - int16_t reverse_pwm_mask = 0; + uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); + reverse_pwm_mask = 0; for (unsigned i = 0; i < _max_actuators; i++) { sprintf(str, "%s_REV%u", prefix, i + 1); @@ -1393,113 +852,28 @@ void PX4IO::update_params() } } - if (io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, reverse_pwm_mask) == OK) { - _pwm_rev_configured = true; - } + _pwm_rev_configured = true; } // PWM_MAIN_TRIMx - { - uint16_t values[8] {}; + if (_mixing_output.mixers()) { + int16_t values[8] {}; for (unsigned i = 0; i < _max_actuators; i++) { sprintf(str, "%s_TRIM%u", prefix, i + 1); float pwm_trim = 0.f; if (param_get(param_find(str), &pwm_trim) == PX4_OK) { - values[i] = (int16_t)(10000 * pwm_trim); + values[i] = roundf(10000 * pwm_trim); } } // copy the trim values to the mixer offsets - io_reg_set(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, values, _max_actuators); + _mixing_output.mixers()->set_trims(values, _max_actuators); } } -int -PX4IO::io_set_control_groups() -{ - int ret = io_set_control_state(0); - - /* send auxiliary control groups */ - (void)io_set_control_state(1); - (void)io_set_control_state(2); - (void)io_set_control_state(3); - - return ret; -} - -int -PX4IO::io_set_control_state(unsigned group) -{ - actuator_controls_s controls{}; ///< actuator outputs - - /* get controls */ - bool changed = false; - - switch (group) { - case 0: { - orb_check(_t_actuator_controls_0, &changed); - - if (changed) { - orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls); - perf_set_elapsed(_perf_sample_latency, hrt_elapsed_time(&controls.timestamp_sample)); - } - } - break; - - case 1: - changed = _t_actuator_controls_1.update(&controls); - break; - - case 2: - changed = _t_actuator_controls_2.update(&controls); - break; - - case 3: - changed = _t_actuator_controls_3.update(&controls); - break; - - } - - if (!changed && (!_in_esc_calibration_mode || group != 0)) { - return -1; - - } else if (_in_esc_calibration_mode && group == 0) { - /* modify controls to get max pwm (full thrust) on every esc */ - memset(&controls, 0, sizeof(controls)); - - /* set maximum thrust */ - controls.control[3] = 1.0f; - } - - uint16_t regs[sizeof(controls.control) / sizeof(controls.control[0])] = {}; - - for (unsigned i = 0; (i < _max_controls) && (i < sizeof(controls.control) / sizeof(controls.control[0])); i++) { - /* ensure FLOAT_TO_REG does not produce an integer overflow */ - const float ctrl = math::constrain(controls.control[i], -1.0f, 1.0f); - - if (!isfinite(ctrl)) { - regs[i] = INT16_MAX; - - } else { - regs[i] = FLOAT_TO_REG(ctrl); - } - - } - - if (!_test_fmu_fail && !_motor_test.in_test_mode) { - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, math::min(_max_controls, - sizeof(controls.control) / sizeof(controls.control[0]))); - - } else { - return OK; - } -} - -void -PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result) +void PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result) { /* publish ACK */ uORB::Publication vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; @@ -1512,74 +886,6 @@ PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result) vehicle_command_ack_pub.publish(command_ack); } -void -PX4IO::handle_motor_test() -{ - test_motor_s test_motor; - - while (_motor_test.test_motor_sub.update(&test_motor)) { - if (test_motor.driver_instance != 0 || - hrt_elapsed_time(&test_motor.timestamp) > 100_ms) { - continue; - } - - bool in_test_mode = test_motor.action == test_motor_s::ACTION_RUN; - - if (in_test_mode != _motor_test.in_test_mode) { - // reset all outputs to disarmed on state change - pwm_output_values pwm_disarmed; - - if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) { - for (unsigned i = 0; i < _max_actuators; ++i) { - io_reg_set(PX4IO_PAGE_DIRECT_PWM, i, pwm_disarmed.values[i]); - } - } - } - - if (in_test_mode) { - unsigned idx = test_motor.motor_number; - - if (idx < _max_actuators) { - if (test_motor.value < 0.f) { - pwm_output_values pwm_disarmed; - - if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) { - io_reg_set(PX4IO_PAGE_DIRECT_PWM, idx, pwm_disarmed.values[idx]); - } - - } else { - pwm_output_values pwm_min; - pwm_output_values pwm_max; - - if (io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm_min.values, _max_actuators) == 0 && - io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm_max.values, _max_actuators) == 0) { - - uint16_t value = math::constrain(pwm_min.values[idx] + - static_cast(((pwm_max.values[idx] - pwm_min.values[idx]) * test_motor.value)), - pwm_min.values[idx], pwm_max.values[idx]); - io_reg_set(PX4IO_PAGE_DIRECT_PWM, idx, value); - } - } - } - - if (test_motor.timeout_ms > 0) { - _motor_test.timeout = test_motor.timestamp + test_motor.timeout_ms * 1000; - - } else { - _motor_test.timeout = 0; - } - } - - _motor_test.in_test_mode = in_test_mode; - } - - // check for timeouts - if (_motor_test.timeout != 0 && hrt_absolute_time() > _motor_test.timeout) { - _motor_test.in_test_mode = false; - _motor_test.timeout = 0; - } -} - int PX4IO::io_set_arming_state() { @@ -1589,17 +895,13 @@ PX4IO::io_set_arming_state() actuator_armed_s armed; if (_t_actuator_armed.copy(&armed)) { - _in_esc_calibration_mode = armed.in_esc_calibration_mode; - - if (armed.armed || _in_esc_calibration_mode) { + if (armed.armed || armed.in_esc_calibration_mode) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - _armed = armed.armed; - if (armed.prearmed) { set |= PX4IO_P_SETUP_ARMING_FMU_PREARMED; @@ -1639,19 +941,6 @@ PX4IO::io_set_arming_state() } } - vehicle_control_mode_s control_mode; - - if (_t_vehicle_control_mode.copy(&control_mode)) { - if (control_mode.flag_external_manual_override_ok) { - set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; - _override_available = true; - - } else { - clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; - _override_available = false; - } - } - if (_last_written_arming_s != set || _last_written_arming_c != clear) { _last_written_arming_s = set; _last_written_arming_c = clear; @@ -1661,183 +950,7 @@ PX4IO::io_set_arming_state() return 0; } -int -PX4IO::disable_rc_handling() -{ - _rc_handling_disabled = true; - return io_disable_rc_handling(); -} - -int -PX4IO::io_disable_rc_handling() -{ - uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED; - uint16_t clear = 0; - - return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); -} - -int -PX4IO::io_set_rc_config() -{ - unsigned offset = 0; - int input_map[_max_rc_input]; - int32_t ichan; - int ret = OK; - - /* - * Generate the input channel -> control channel mapping table; - * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical - * controls. - */ - - /* fill the mapping with an error condition triggering value */ - for (unsigned i = 0; i < _max_rc_input; i++) { - input_map[i] = UINT8_MAX; - } - - /* - * NOTE: The indices for mapped channels are 1-based - * for compatibility reasons with existing - * autopilots / GCS'. - */ - - /* ROLL */ - param_get(param_find("RC_MAP_ROLL"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 0; - } - - /* PITCH */ - param_get(param_find("RC_MAP_PITCH"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 1; - } - - /* YAW */ - param_get(param_find("RC_MAP_YAW"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 2; - } - - /* THROTTLE */ - param_get(param_find("RC_MAP_THROTTLE"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 3; - } - - /* FLAPS */ - param_get(param_find("RC_MAP_FLAPS"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 4; - } - - /* AUX 1*/ - param_get(param_find("RC_MAP_AUX1"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 5; - } - - /* AUX 2*/ - param_get(param_find("RC_MAP_AUX2"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 6; - } - - /* AUX 3*/ - param_get(param_find("RC_MAP_AUX3"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 7; - } - - /* MAIN MODE SWITCH */ - param_get(param_find("RC_MAP_MODE_SW"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - /* use out of normal bounds index to indicate special channel */ - input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; - } - - /* - * Iterate all possible RC inputs. - */ - for (unsigned i = 0; i < _max_rc_input; i++) { - uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; - char pname[16]; - float fval; - - /* - * RC params are floats, but do only - * contain integer values. Do not scale - * or cast them, let the auto-typeconversion - * do its job here. - * Channels: 500 - 2500 - * Inverted flag: -1 (inverted) or 1 (normal) - */ - - sprintf(pname, "RC%u_MIN", i + 1); - param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_MIN] = fval; - - sprintf(pname, "RC%u_TRIM", i + 1); - param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_CENTER] = fval; - - sprintf(pname, "RC%u_MAX", i + 1); - param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_MAX] = fval; - - sprintf(pname, "RC%u_DZ", i + 1); - param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_DEADZONE] = fval; - - regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = input_map[i]; - - regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - sprintf(pname, "RC%u_REV", i + 1); - param_get(param_find(pname), &fval); - - /* - * This has been taken for the sake of compatibility - * with APM's setup / mission planner: normal: 1, - * inverted: -1 - */ - if (fval < 0) { - regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; - } - - /* send channel config to IO */ - ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); - - if (ret != OK) { - PX4_ERR("rc config upload failed"); - break; - } - - /* check the IO initialisation flag */ - if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) { - mavlink_log_critical(&_mavlink_log_pub, "config for RC%u rejected by IO\t", i + 1); - events::send(events::ID("px4io_config_rc_rejected"), events::Log::Error, - "Config for RC{1} rejected by IO", i + 1); - break; - } - - offset += PX4IO_P_RC_CONFIG_STRIDE; - } - - return ret; -} - -int -PX4IO::io_handle_status(uint16_t status) +int PX4IO::io_handle_status(uint16_t status) { int ret = 1; /** @@ -1873,18 +986,13 @@ PX4IO::io_handle_status(uint16_t status) * Get and handle the safety status */ const bool safety_off = status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF; - const bool override_enabled = status & PX4IO_P_STATUS_FLAGS_OVERRIDE; // publish immediately on change, otherwise at 1 Hz if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s) - || (_safety.safety_off != safety_off) - || (_safety.override_available != _override_available) - || (_safety.override_enabled != override_enabled)) { + || (_safety.safety_off != safety_off)) { _safety.safety_switch_available = true; _safety.safety_off = safety_off; - _safety.override_available = _override_available; - _safety.override_enabled = override_enabled; _safety.timestamp = hrt_absolute_time(); _to_safety.publish(_safety); @@ -1893,8 +1001,7 @@ PX4IO::io_handle_status(uint16_t status) return ret; } -int -PX4IO::dsm_bind_ioctl(int dsmMode) +int PX4IO::dsm_bind_ioctl(int dsmMode) { // Do not bind if invalid pulses are provided if (dsmMode != DSM2_BIND_PULSES && @@ -1951,8 +1058,7 @@ PX4IO::dsm_bind_ioctl(int dsmMode) return ret; } -int -PX4IO::io_get_status() +int PX4IO::io_get_status() { /* get * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT, @@ -2001,50 +1107,52 @@ PX4IO::io_get_status() // PX4IO_P_STATUS_FLAGS status.status_outputs_armed = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; - status.status_override = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_OVERRIDE; status.status_rc_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_OK; status.status_rc_ppm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_PPM; status.status_rc_dsm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_DSM; status.status_rc_sbus = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SBUS; + status.status_rc_st24 = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_ST24; + status.status_rc_sumd = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SUMD; + status.status_fmu_initialized = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; status.status_fmu_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_OK; status.status_raw_pwm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RAW_PWM; - status.status_mixer_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_MIXER_OK; status.status_arm_sync = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_ARM_SYNC; status.status_init_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_INIT_OK; status.status_failsafe = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FAILSAFE; status.status_safety_off = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_OFF; - status.status_fmu_initialized = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; - status.status_rc_st24 = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_ST24; - status.status_rc_sumd = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SUMD; // PX4IO_P_STATUS_ALARMS - status.alarm_vbatt_low = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_VBATT_LOW; - status.alarm_temperature = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_TEMPERATURE; - status.alarm_servo_current = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT; - status.alarm_acc_current = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_ACC_CURRENT; - status.alarm_fmu_lost = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_FMU_LOST; status.alarm_rc_lost = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST; status.alarm_pwm_error = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_PWM_ERROR; - status.alarm_vservo_fault = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT; // PX4IO_P_SETUP_ARMING status.arming_io_arm_ok = SETUP_ARMING & PX4IO_P_SETUP_ARMING_IO_ARM_OK; status.arming_fmu_armed = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_ARMED; status.arming_fmu_prearmed = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_PREARMED; - status.arming_manual_override_ok = SETUP_ARMING & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; status.arming_failsafe_custom = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; - status.arming_inair_restart_ok = SETUP_ARMING & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK; - status.arming_always_pwm_enable = SETUP_ARMING & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; - status.arming_rc_handling_disabled = SETUP_ARMING & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED; status.arming_lockdown = SETUP_ARMING & PX4IO_P_SETUP_ARMING_LOCKDOWN; status.arming_force_failsafe = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; status.arming_termination_failsafe = SETUP_ARMING & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - status.arming_override_immediate = SETUP_ARMING & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE; - for (unsigned i = 0; i < _max_actuators; i++) { - status.actuators[i] = static_cast(io_reg_get(PX4IO_PAGE_ACTUATORS, i)); - status.servos[i] = io_reg_get(PX4IO_PAGE_SERVOS, i); + status.pwm[i] = io_reg_get(PX4IO_PAGE_SERVOS, i); + status.pwm_disarmed[i] = io_reg_get(PX4IO_PAGE_DISARMED_PWM, i); + status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i); + } + + // PWM rates, 0 = low rate, 1 = high rate + const uint16_t pwm_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); + + const int pwm_low_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); + const int pwm_high_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); + + for (unsigned i = 0; i < _max_actuators; i++) { + if (pwm_rate & (1 << i)) { + status.pwm_rate_hz[i] = pwm_high_rate; + + } else { + status.pwm_rate_hz[i] = pwm_low_rate; + } } uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); @@ -2065,11 +1173,12 @@ PX4IO::io_get_status() return ret; } -int -PX4IO::io_get_raw_rc_input(input_rc_s &input_rc) +int PX4IO::io_publish_raw_rc() { - uint32_t channel_count; - int ret; + input_rc_s input_rc{}; + + /* set the RC status flag ORDER MATTERS! */ + input_rc.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK); /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; @@ -2082,7 +1191,7 @@ PX4IO::io_get_raw_rc_input(input_rc_s &input_rc) * * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); + int ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); if (ret != OK) { return ret; @@ -2092,15 +1201,13 @@ PX4IO::io_get_raw_rc_input(input_rc_s &input_rc) * Get the channel count any any extra channels. This is no more expensive than reading the * channel count once. */ - channel_count = regs[PX4IO_P_RAW_RC_COUNT]; + uint32_t channel_count = regs[PX4IO_P_RAW_RC_COUNT]; /* limit the channel count */ if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; } - _rc_chan_count = channel_count; - input_rc.timestamp = hrt_absolute_time(); input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; @@ -2129,7 +1236,7 @@ PX4IO::io_get_raw_rc_input(input_rc_s &input_rc) input_rc.channel_count = channel_count; /* rc_lost has to be set before the call to this function */ - if (!input_rc.rc_lost && !input_rc.rc_failsafe) { + if ((channel_count > 0) && !input_rc.rc_lost && !input_rc.rc_failsafe) { _rc_last_valid = input_rc.timestamp; } @@ -2157,48 +1264,31 @@ PX4IO::io_get_raw_rc_input(input_rc_s &input_rc) } /* get RSSI from input channel */ - if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { - int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) / - (_rssi_pwm_max - _rssi_pwm_min); - rssi = rssi > 100 ? 100 : rssi; - rssi = rssi < 0 ? 0 : rssi; - input_rc.rssi = rssi; - } + if (_param_rc_rssi_pwm_chan.get() > 0 && _param_rc_rssi_pwm_chan.get() <= input_rc_s::RC_INPUT_MAX_CHANNELS) { + const auto &min = _param_rc_rssi_pwm_min.get(); + const auto &max = _param_rc_rssi_pwm_max.get(); - return ret; -} - -int -PX4IO::io_publish_raw_rc() -{ - - /* fetch values from IO */ - input_rc_s rc_val; - - /* set the RC status flag ORDER MATTERS! */ - rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK); - - int ret = io_get_raw_rc_input(rc_val); - - if (ret != OK) { - return ret; + if (max - min != 0) { + int rssi = ((input_rc.values[_param_rc_rssi_pwm_chan.get() - 1] - min) * 100) / (max - min); + input_rc.rssi = math::constrain(rssi, 0, 100); + } } /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) { - rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24; } else { - rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; /* only keep publishing RC input if we ever got a valid input */ if (_rc_last_valid == 0) { @@ -2207,59 +1297,12 @@ PX4IO::io_publish_raw_rc() } } - _to_input_rc.publish(rc_val); + _to_input_rc.publish(input_rc); - return OK; + return ret; } -int -PX4IO::io_publish_pwm_outputs() -{ - if (_hitl_mode) { - return OK; - } - - /* get servo values from IO */ - uint16_t ctl[_max_actuators]; - int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); - - if (ret != OK) { - return ret; - } - - actuator_outputs_s outputs = {}; - outputs.timestamp = hrt_absolute_time(); - outputs.noutputs = _max_actuators; - - /* convert from register format to float */ - for (unsigned i = 0; i < _max_actuators; i++) { - outputs.output[i] = ctl[i]; - } - - _to_outputs.publish(outputs); - - /* get mixer status flags from IO */ - MultirotorMixer::saturation_status saturation_status; - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &saturation_status.value, 1); - - if (ret != OK) { - return ret; - } - - /* publish mixer status */ - if (saturation_status.flags.valid) { - multirotor_motor_limits_s motor_limits{}; - motor_limits.timestamp = hrt_absolute_time(); - motor_limits.saturation_status = saturation_status.value; - - _to_mixer_status.publish(motor_limits); - } - - return OK; -} - -int -PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { @@ -2267,7 +1310,9 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned return -EINVAL; } - int ret = _interface->write((page << 8) | offset, (void *)values, num_values); + perf_begin(_interface_write_perf); + int ret = _interface->write((page << 8) | offset, (void *)values, num_values); + perf_end(_interface_write_perf); if (ret != (int)num_values) { PX4_DEBUG("io_reg_set(%" PRIu8 ",%" PRIu8 ",%u): error %d", page, offset, num_values, ret); @@ -2277,14 +1322,12 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned return OK; } -int -PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) +int PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) { return io_reg_set(page, offset, &value, 1); } -int -PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { @@ -2292,7 +1335,9 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v return -EINVAL; } + perf_begin(_interface_read_perf); int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); + perf_end(_interface_read_perf); if (ret != (int)num_values) { PX4_DEBUG("io_reg_get(%" PRIu8 ",%" PRIu8 ",%u): data error %d", page, offset, num_values, ret); @@ -2302,8 +1347,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v return OK; } -uint32_t -PX4IO::io_reg_get(uint8_t page, uint8_t offset) +uint32_t PX4IO::io_reg_get(uint8_t page, uint8_t offset) { uint16_t value; @@ -2314,13 +1358,10 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset) return value; } -int -PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) +int PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) { - int ret; - uint16_t value; - - ret = io_reg_get(page, offset, &value, 1); + uint16_t value = 0; + int ret = io_reg_get(page, offset, &value, 1); if (ret != OK) { return ret; @@ -2376,125 +1417,7 @@ PX4IO::print_debug() } -int -PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) -{ - /* get debug level */ - int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG); - - uint8_t frame[_max_transfer]; - - do { - - px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; - unsigned max_len = _max_transfer - sizeof(px4io_mixdata); - - msg->f2i_mixer_magic = F2I_MIXER_MAGIC; - msg->action = F2I_MIXER_ACTION_RESET; - - do { - unsigned count = buflen; - - if (count > max_len) { - count = max_len; - } - - if (count > 0) { - memcpy(&msg->text[0], buf, count); - buf += count; - buflen -= count; - - } else { - continue; - } - - /* - * We have to send an even number of bytes. This - * will only happen on the very last transfer of a - * mixer, and we are guaranteed that there will be - * space left to round up as _max_transfer will be - * even. - */ - unsigned total_len = sizeof(px4io_mixdata) + count; - - if (total_len % 2) { - msg->text[count] = '\0'; - total_len++; - } - - int ret; - - for (int i = 0; i < 30; i++) { - /* failed, but give it a 2nd shot */ - ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); - - if (ret) { - px4_usleep(333); - - } else { - break; - } - } - - /* print mixer chunk */ - if (debuglevel > 5 || ret) { - - warnx("fmu sent: \"%s\"", msg->text); - - /* read IO's output */ - print_debug(); - } - - if (ret) { - PX4_ERR("mixer send error %d", ret); - return ret; - } - - msg->action = F2I_MIXER_ACTION_APPEND; - - } while (buflen > 0); - - int ret; - - /* send the closing newline */ - msg->text[0] = '\n'; - msg->text[1] = '\0'; - - for (int i = 0; i < 30; i++) { - /* failed, but give it a 2nd shot */ - ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); - - if (ret) { - px4_usleep(333); - - } else { - break; - } - } - - if (ret == 0) { - /* success, exit */ - break; - } - - retries--; - - } while (retries > 0); - - if (retries == 0) { - mavlink_log_info(&_mavlink_log_pub, "[IO] mixer upload fail\t"); - events::send(events::ID("px4io_mixer_upload_failed"), events::Log::Error, "IO mixer upload failed"); - /* load must have failed for some reason */ - return -EINVAL; - - } else { - /* all went well, set the mixer ok flag */ - return io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_MIXER_OK); - } -} - -void -PX4IO::print_status(bool extended_status) +int PX4IO::print_status() { /* basic configuration */ printf("protocol %" PRIu32 " hardware %" PRIu32 " bootloader %" PRIu32 " buffer %" PRIu32 "B crc 0x%04" PRIu32 "%04" @@ -2505,13 +1428,12 @@ PX4IO::print_status(bool extended_status) io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC + 1)); - printf("%" PRIu32 " controls %" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs %" PRIu32 - " relays\n", + + printf("%" PRIu32 " controls %" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT)); /* status */ uORB::SubscriptionData status_sub{ORB_ID(px4io_status)}; @@ -2522,23 +1444,7 @@ PX4IO::print_status(bool extended_status) /* now clear alarms */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0x0000); - uint16_t pwm_invert_mask = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE); - printf("\n"); - printf("reversed outputs: ["); - - for (unsigned i = 0; i < _max_actuators; i++) { - printf("%s", (pwm_invert_mask & (1 << i)) ? "x" : "_"); - } - - printf("]"); - - float trim_roll = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL)); - float trim_pitch = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH)); - float trim_yaw = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW)); - - printf(" trims: r: %8.4f p: %8.4f y: %8.4f\n", - (double)trim_roll, (double)trim_pitch, (double)trim_yaw); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%" PRIu16 " raw R/C inputs", raw_inputs); @@ -2547,36 +1453,6 @@ PX4IO::print_status(bool extended_status) printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); } - printf("\n"); - - uint16_t io_status_flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - uint16_t flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); - printf("R/C flags: 0x%04" PRIx16 "%s%s%s%s%s\n", flags, - (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "") - ); - - if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { - int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); - printf("RC data (PPM frame len) %d us\n", frame_len); - - if ((frame_len - raw_inputs * 2000 - 3000) < 0) { - printf("WARNING WARNING WARNING! This RC receiver does not allow safe frame detection.\n"); - } - } - - uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); - printf("mapped R/C inputs 0x%04" PRIx16, mapped_inputs); - - for (unsigned i = 0; i < _max_rc_input; i++) { - if (mapped_inputs & (1 << i)) { - printf(" %u:%" PRId16, i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); - } - } - printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); printf("ADC inputs"); @@ -2589,60 +1465,16 @@ PX4IO::print_status(bool extended_status) /* setup and state */ uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); - printf("features 0x%04" PRIx16 "%s%s%s%s\n", features, + printf("features 0x%04" PRIx16 "%s%s%s\n", features, ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), - ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") ); - printf("rates 0x%04" PRIx32 " default %" PRIu32 " alt %" PRIu32 " sbus %" PRIu32 "\n", - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE)); + printf("sbus rate %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE)); + printf("debuglevel %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - for (unsigned group = 0; group < 4; group++) { - printf("controls %u:", group); - - for (unsigned i = 0; i < _max_controls; i++) { - printf(" %" PRIu16, (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); - } - - printf("\n"); - } - - if (extended_status) { - for (unsigned i = 0; i < _max_rc_input; i++) { - unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; - uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); - printf("input %u min %" PRIu32 " center %" PRIu32 " max %" PRIu32 " deadzone %" PRIu32 " assigned %" PRIu32 - " options 0x%04" PRIx16 "%s%s\n", - i, - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), - options, - ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), - ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); - } - } - - printf("failsafe"); - - for (unsigned i = 0; i < _max_actuators; i++) { - printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - } - - printf("\ndisarmed values"); - - for (unsigned i = 0; i < _max_actuators; i++) { - printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); - } - /* IMU heater (Pixhawk 2.1) */ uint16_t heater_level = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL); @@ -2655,15 +1487,13 @@ PX4IO::print_status(bool extended_status) } } - if (_hitl_mode) { - printf("\nHITL Mode"); - } - printf("\n"); + + _mixing_output.printStatus(); + return 0; } -int -PX4IO::ioctl(file *filep, int cmd, unsigned long arg) +int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { SmartLock lock_guard(_lock); int ret = OK; @@ -2671,41 +1501,49 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* regular ioctl? */ switch (cmd) { case PWM_SERVO_ARM: + PX4_DEBUG("PWM_SERVO_ARM"); /* set the 'armed' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED); break; case PWM_SERVO_SET_ARM_OK: + PX4_DEBUG("PWM_SERVO_SET_ARM_OK"); /* set the 'OK to arm' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK); break; case PWM_SERVO_CLEAR_ARM_OK: + PX4_DEBUG("PWM_SERVO_CLEAR_ARM_OK"); /* clear the 'OK to arm' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0); break; case PWM_SERVO_DISARM: + PX4_DEBUG("PWM_SERVO_DISARM"); /* clear the 'armed' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); break; case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + PX4_DEBUG("PWM_SERVO_GET_DEFAULT_UPDATE_RATE"); /* get the default update rate */ *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); break; case PWM_SERVO_SET_UPDATE_RATE: + PX4_DEBUG("PWM_SERVO_SET_UPDATE_RATE"); /* set the requested alternate rate */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); break; case PWM_SERVO_GET_UPDATE_RATE: + PX4_DEBUG("PWM_SERVO_GET_UPDATE_RATE"); /* get the alternative update rate */ *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); break; case PWM_SERVO_SET_SELECT_UPDATE_RATE: { + PX4_DEBUG("PWM_SERVO_SET_SELECT_UPDATE_RATE"); /* blindly clear the PWM update alarm - might be set for some other reason */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); @@ -2726,11 +1564,12 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } case PWM_SERVO_GET_SELECT_UPDATE_RATE: - + PX4_DEBUG("PWM_SERVO_GET_SELECT_UPDATE_RATE"); *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); break; case PWM_SERVO_SET_FAILSAFE_PWM: { + PX4_DEBUG("PWM_SERVO_SET_FAILSAFE_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; if (pwm->channel_count > _max_actuators) @@ -2739,12 +1578,19 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) return -E2BIG; } - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] != 0) { + _mixing_output.failsafeValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MAX); + } + } + + updateFailsafe(); + break; } case PWM_SERVO_GET_FAILSAFE_PWM: { + PX4_DEBUG("PWM_SERVO_GET_FAILSAFE_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; pwm->channel_count = _max_actuators; @@ -2758,122 +1604,140 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } case PWM_SERVO_SET_DISARMED_PWM: { + PX4_DEBUG("PWM_SERVO_SET_DISARMED_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - if (pwm->channel_count > _max_actuators) + if (pwm->channel_count > _max_actuators) { /* fail with error */ - { return -E2BIG; } - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] != 0) { + _mixing_output.disarmedValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MAX); + } + } + + updateDisarmed(); + break; } case PWM_SERVO_GET_DISARMED_PWM: { + PX4_DEBUG("PWM_SERVO_GET_DISARMED_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, _max_actuators); - - if (ret != OK) { - ret = -EIO; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _mixing_output.disarmedValue(i); } break; } case PWM_SERVO_SET_MIN_PWM: { + PX4_DEBUG("PWM_SERVO_SET_MIN_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - if (pwm->channel_count > _max_actuators) + if (pwm->channel_count > _max_actuators) { /* fail with error */ - { return -E2BIG; } - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] != 0) { + _mixing_output.minValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MIN); + } + } + break; } case PWM_SERVO_GET_MIN_PWM: { + PX4_DEBUG("PWM_SERVO_GET_MIN_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, _max_actuators); - - if (ret != OK) { - ret = -EIO; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _mixing_output.minValue(i); } break; } case PWM_SERVO_SET_MAX_PWM: { + PX4_DEBUG("PWM_SERVO_SET_MAX_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - if (pwm->channel_count > _max_actuators) + if (pwm->channel_count > _max_actuators) { /* fail with error */ - { return -E2BIG; } - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); - break; + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] != 0) { + _mixing_output.maxValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MAX, (uint16_t)PWM_HIGHEST_MAX); + } + } } + break; case PWM_SERVO_GET_MAX_PWM: { + PX4_DEBUG("PWM_SERVO_GET_MAX_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, _max_actuators); - - if (ret != OK) { - ret = -EIO; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _mixing_output.maxValue(i); } } - break; case PWM_SERVO_GET_TRIM_PWM: { + PX4_DEBUG("PWM_SERVO_GET_TRIM_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, pwm->values, _max_actuators); + if (_mixing_output.mixers() == nullptr) { + memset(pwm, 0, sizeof(pwm_output_values)); + PX4_WARN("warning: trim values not valid - no mixer loaded"); - if (ret != OK) { - ret = -EIO; + } else { + pwm->channel_count = _mixing_output.mixers()->get_trims((int16_t *)pwm->values); } + + break; } - break; - case PWM_SERVO_GET_COUNT: + PX4_DEBUG("PWM_SERVO_GET_COUNT"); *(unsigned *)arg = _max_actuators; break; case PWM_SERVO_SET_DISABLE_LOCKDOWN: + PX4_DEBUG("PWM_SERVO_SET_DISABLE_LOCKDOWN"); _lockdown_override = arg; break; case PWM_SERVO_GET_DISABLE_LOCKDOWN: + PX4_DEBUG("PWM_SERVO_GET_DISABLE_LOCKDOWN"); *(unsigned *)arg = _lockdown_override; break; case PWM_SERVO_SET_FORCE_SAFETY_OFF: + PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_OFF"); /* force safety swith off */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; case PWM_SERVO_SET_FORCE_SAFETY_ON: + PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_ON"); /* force safety switch on */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC); break; case PWM_SERVO_SET_FORCE_FAILSAFE: + PX4_DEBUG("PWM_SERVO_SET_FORCE_FAILSAFE"); /* force failsafe mode instantly */ if (arg == 0) { @@ -2888,6 +1752,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; case PWM_SERVO_SET_TERMINATION_FAILSAFE: + PX4_DEBUG("PWM_SERVO_SET_TERMINATION_FAILSAFE"); /* if failsafe occurs, do not allow the system to recover */ if (arg == 0) { @@ -2901,22 +1766,6 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; - case PWM_SERVO_SET_OVERRIDE_IMMEDIATE: - - /* control whether override on FMU failure is - immediate or waits for override threshold on mode - switch */ - if (arg == 0) { - /* clear override immediate flag */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0); - - } else { - /* set override immediate flag */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE); - } - - break; - case DSM_BIND_START: /* bind a DSM receiver */ ret = dsm_bind_ioctl(arg); @@ -2934,7 +1783,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = -EINVAL; } else { - if (!_test_fmu_fail && _motor_test.in_test_mode) { + if (!_test_fmu_fail && _in_test_mode) { /* send a direct PWM value */ ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); @@ -2994,22 +1843,29 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } } - _motor_test.in_test_mode = (arg == PWM_SERVO_ENTER_TEST_MODE); + _in_test_mode = (arg == PWM_SERVO_ENTER_TEST_MODE); ret = (arg == PWM_SERVO_ENTER_TEST_MODE || PWM_SERVO_EXIT_TEST_MODE) ? 0 : -EINVAL; } break; case MIXERIOCRESET: - ret = 0; /* load always resets */ + PX4_DEBUG("MIXERIOCRESET"); + _mixing_output.resetMixerThreadSafe(); break; case MIXERIOCLOADBUF: { + PX4_DEBUG("MIXERIOCLOADBUF"); + const char *buf = (const char *)arg; - ret = mixer_send(buf, strlen(buf)); + unsigned buflen = strlen(buf); + ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + break; } case PX4IO_SET_DEBUG: + PX4_DEBUG("PX4IO_SET_DEBUG"); + /* set the debug level */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); break; @@ -3042,6 +1898,8 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; case PX4IO_CHECK_CRC: { + PX4_DEBUG("PX4IO_CHECK_CRC"); + /* check IO firmware CRC against passed value */ uint32_t io_crc = 0; ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); @@ -3058,41 +1916,8 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case PX4IO_INAIR_RESTART_ENABLE: - - /* set/clear the 'in-air restart' bit */ - if (arg) { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); - - } else { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); - } - - break; - - case RC_INPUT_ENABLE_RSSI_ANALOG: - - if (arg) { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI); - - } else { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0); - } - - break; - - case RC_INPUT_ENABLE_RSSI_PWM: - - if (arg) { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI); - - } else { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0); - } - - break; - case SBUS_SET_PROTO_VERSION: + PX4_DEBUG("SBUS_SET_PROTO_VERSION"); if (arg == 1) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); @@ -3130,145 +1955,75 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) return ret; } -extern "C" __EXPORT int px4io_main(int argc, char *argv[]); - -namespace +static device::Device *get_interface() { - -device::Device * -get_interface() -{ - device::Device *interface = nullptr; - -#ifdef PX4IO_SERIAL_BASE - interface = PX4IO_serial_interface(); -#endif + device::Device *interface = PX4IO_serial_interface(); if (interface != nullptr) { - goto got; - } - - errx(1, "cannot alloc interface"); - -got: - - if (interface->init() != OK) { - delete interface; - errx(1, "interface init failed"); + if (interface->init() != OK) { + PX4_ERR("interface init failed"); + delete interface; + interface = nullptr; + } } return interface; } -void -start(int argc, char *argv[]) +static int detect(int argc, char *argv[]) { - if (g_dev != nullptr) { - errx(0, "already loaded"); - } - /* allocate the interface */ device::Device *interface = get_interface(); - /* create the driver - it will set g_dev */ - (void)new PX4IO(interface); - - if (g_dev == nullptr) { - delete interface; - errx(1, "driver allocation failed"); + if (interface == nullptr) { + PX4_ERR("interface allocation failed"); + return 1; } - bool rc_handling_disabled = false; - bool hitl_mode = false; + PX4IO *dev = new PX4IO(interface); - /* disable RC handling and/or actuator_output publication on request */ - for (int extra_args = 1; extra_args < argc; extra_args++) { - if (!strcmp(argv[extra_args], "norc")) { - rc_handling_disabled = true; - - } else if (!strcmp(argv[extra_args], "hil")) { - hitl_mode = true; - - } else if (argv[extra_args][0] != '\0') { - PX4_WARN("unknown argument: %s", argv[extra_args]); - } + if (dev == nullptr) { + PX4_ERR("driver allocation failed"); + return 1; } - if (OK != g_dev->init(rc_handling_disabled, hitl_mode)) { - delete g_dev; - g_dev = nullptr; - errx(1, "driver init failed"); - } - - exit(0); + int ret = dev->detect(); + delete dev; + return ret; } -void -detect(int argc, char *argv[]) + +int PX4IO::checkcrc(int argc, char *argv[]) { - if (g_dev != nullptr) { - errx(0, "already loaded"); - } - - /* allocate the interface */ - device::Device *interface = get_interface(); - - /* create the driver - it will set g_dev */ - (void)new PX4IO(interface); - - if (g_dev == nullptr) { - errx(1, "driver allocation failed"); - } - - int ret = g_dev->detect(); - - delete g_dev; - g_dev = nullptr; - - if (ret) { - /* nonzero, error */ - exit(1); - - } else { - exit(0); - } -} - -void -checkcrc(int argc, char *argv[]) -{ - bool keep_running = false; - - if (g_dev == nullptr) { - /* allocate the interface */ - device::Device *interface = get_interface(); - - /* create the driver - it will set g_dev */ - (void)new PX4IO(interface); - - if (g_dev == nullptr) { - PX4_ERR("driver allocation failed"); - exit(1); - } - - } else { - /* its already running, don't kill the driver */ - keep_running = true; - } - /* check IO CRC against CRC of a file */ - if (argc < 2) { + if (argc < 1) { PX4_WARN("usage: px4io checkcrc filename"); - exit(1); + return 1; } - int fd = open(argv[1], O_RDONLY); + device::Device *interface = get_interface(); + + if (interface == nullptr) { + PX4_ERR("interface allocation failed"); + return 1; + } + + PX4IO *dev = new PX4IO(interface); + + if (dev == nullptr) { + delete interface; + PX4_ERR("driver allocation failed"); + return 1; + } + + int fd = ::open(argv[0], O_RDONLY); if (fd == -1) { - PX4_ERR("open of %s failed: %d", argv[1], errno); - exit(1); + delete dev; + PX4_ERR("open of %s failed: %d", argv[0], errno); + return 1; } const uint32_t app_size_max = 0xf000; @@ -3277,7 +2032,7 @@ checkcrc(int argc, char *argv[]) while (true) { uint8_t buf[16]; - int n = read(fd, buf, sizeof(buf)); + int n = ::read(fd, buf, sizeof(buf)); if (n <= 0) { break; } @@ -3285,7 +2040,7 @@ checkcrc(int argc, char *argv[]) nbytes += n; } - close(fd); + ::close(fd); while (nbytes < app_size_max) { uint8_t b = 0xff; @@ -3293,63 +2048,52 @@ checkcrc(int argc, char *argv[]) nbytes++; } - int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + int ret = dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); - if (!keep_running) { - delete g_dev; - g_dev = nullptr; - } + delete dev; if (ret != OK) { PX4_WARN("check CRC failed: %d, CRC: %" PRIu32, ret, fw_crc); - exit(1); - - } else { - PX4_INFO("IO FW CRC match"); + return 1; } - exit(0); + PX4_INFO("IO FW CRC match"); + return 0; } -void -bind(int argc, char *argv[]) +int PX4IO::bind(int argc, char *argv[]) { int pulses; - if (g_dev == nullptr) { - errx(1, "px4io must be started first"); + if (argc < 1) { + PX4_ERR("needs argument, use dsm2, dsmx or dsmx8"); + return 1; } - if (argc < 3) { - errx(0, "needs argument, use dsm2, dsmx or dsmx8"); - } - - if (!strcmp(argv[2], "dsm2")) { + if (!strcmp(argv[0], "dsm2")) { pulses = DSM2_BIND_PULSES; - } else if (!strcmp(argv[2], "dsmx")) { + } else if (!strcmp(argv[0], "dsmx")) { pulses = DSMX_BIND_PULSES; - } else if (!strcmp(argv[2], "dsmx8")) { + } else if (!strcmp(argv[0], "dsmx8")) { pulses = DSMX8_BIND_PULSES; } else { - errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); + PX4_ERR("unknown parameter %s, use dsm2, dsmx or dsmx8", argv[0]); + return 1; } // Test for custom pulse parameter - if (argc > 3) { - pulses = atoi(argv[3]); + if (argc > 1) { + pulses = atoi(argv[1]); } - g_dev->ioctl(nullptr, DSM_BIND_START, pulses); - - exit(0); - + get_instance()->ioctl(nullptr, DSM_BIND_START, pulses); + return 0; } -void -monitor(void) +int PX4IO::monitor() { /* clear screen */ printf("\033[2J"); @@ -3362,120 +2106,143 @@ monitor(void) fds[0].fd = 0; fds[0].events = POLLIN; - if (poll(fds, 1, 2000) < 0) { - errx(1, "poll fail"); + if (::poll(fds, 1, 2000) < 0) { + PX4_ERR("poll fail"); + return 1; } if (fds[0].revents == POLLIN) { /* control logic is to cancel with any key */ char c; - (void)read(0, &c, 1); + ::read(0, &c, 1); if (cancels-- == 0) { printf("\033[2J\033[H"); /* move cursor home and clear screen */ - exit(0); + return 0; } } - if (g_dev != nullptr) { - - printf("\033[2J\033[H"); /* move cursor home and clear screen */ - (void)g_dev->print_status(false); - (void)g_dev->print_debug(); - printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); - - } else { - errx(1, "driver not loaded, exiting"); - } + printf("\033[2J\033[H"); /* move cursor home and clear screen */ + get_instance()->print_status(); + get_instance()->print_debug(); + printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); } + + return 0; } -void -lockdown(int argc, char *argv[]) +int PX4IO::lockdown(int argc, char *argv[]) { - if (g_dev != nullptr) { + if (argc > 1 && !strcmp(argv[1], "disable")) { - if (argc > 2 && !strcmp(argv[2], "disable")) { + PX4_WARN("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?"); + PX4_WARN("Press 'y' to enable, any other key to abort."); - warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?"); - warnx("Press 'y' to enable, any other key to abort."); + /* check if user wants to abort */ + char c; - /* check if user wants to abort */ - char c; + struct pollfd fds; + int ret; + hrt_abstime start = hrt_absolute_time(); + const unsigned long timeout = 5000000; - struct pollfd fds; - int ret; - hrt_abstime start = hrt_absolute_time(); - const unsigned long timeout = 5000000; + while (hrt_elapsed_time(&start) < timeout) { + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = ::poll(&fds, 1, 0); - while (hrt_elapsed_time(&start) < timeout) { - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - ret = poll(&fds, 1, 0); + if (ret > 0) { - if (ret > 0) { + if (::read(0, &c, 1) > 0) { - if (read(0, &c, 1) > 0) { + if (c != 'y') { + return 0; - if (c != 'y') { - exit(0); - - } else if (c == 'y') { - break; - } + } else if (c == 'y') { + break; } } - - px4_usleep(10000); } - if (hrt_elapsed_time(&start) > timeout) { - errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES."); - } + px4_usleep(10000); + } - (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); + if (hrt_elapsed_time(&start) > timeout) { + PX4_ERR("TIMEOUT! ABORTED WITHOUT CHANGES."); + return 1; + } - warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!"); + get_instance()->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); - } else { - (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); - warnx("ACTUATORS ARE NOW SAFE IN HIL."); + PX4_WARN("ACTUATORS ARE NOW LIVE IN HIL!"); + + } else { + get_instance()->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); + PX4_WARN("ACTUATORS ARE NOW SAFE IN HIL."); + } + + return 0; +} + + +int PX4IO::task_spawn(int argc, char *argv[]) +{ + device::Device *interface = get_interface(); + + if (interface == nullptr) { + PX4_ERR("Failed to create interface"); + return -1; + } + + PX4IO *instance = new PX4IO(interface); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() == PX4_OK) { + return PX4_OK; } } else { - errx(1, "driver not loaded, exiting"); + PX4_ERR("alloc failed"); } - exit(0); + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; } -} /* namespace */ - -int -px4io_main(int argc, char *argv[]) +int PX4IO::custom_command(int argc, char *argv[]) { - /* check for sufficient number of arguments */ - if (argc < 2) { - goto out; + const char *verb = argv[0]; + + if (!strcmp(verb, "detect")) { + if (is_running()) { + PX4_ERR("io must be stopped"); + return 1; + } + + return ::detect(argc - 1, argv + 1); } - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - errx(1, "PX4IO Not Supported"); + if (!strcmp(verb, "checkcrc")) { + if (is_running()) { + PX4_ERR("io must be stopped"); + return 1; + } + + return checkcrc(argc - 1, argv + 1); } - if (!strcmp(argv[1], "start")) { - start(argc - 1, argv + 1); - } + if (!strcmp(verb, "update")) { - if (!strcmp(argv[1], "detect")) { - detect(argc - 1, argv + 1); - } - - if (!strcmp(argv[1], "checkcrc")) { - checkcrc(argc - 1, argv + 1); - } - - if (!strcmp(argv[1], "update")) { + if (is_running()) { + PX4_ERR("io must be stopped"); + return 1; + } constexpr unsigned MAX_RETRIES = 2; unsigned retries = 0; @@ -3483,48 +2250,52 @@ px4io_main(int argc, char *argv[]) while (ret != OK && retries < MAX_RETRIES) { + device::Device *interface = get_interface(); + + if (interface == nullptr) { + PX4_ERR("interface allocation failed"); + return 1; + } + + PX4IO *dev = new PX4IO(interface); + + if (dev == nullptr) { + delete interface; + PX4_ERR("driver allocation failed"); + return 1; + } + retries++; // Sleep 200 ms before the next attempt usleep(200 * 1000); - if (g_dev == nullptr) { - /* allocate the interface */ - device::Device *interface = get_interface(); - - /* create the driver - it will set g_dev */ - (void)new PX4IO(interface); - - if (g_dev == nullptr) { - delete interface; - errx(1, "driver allocation failed"); - } - } - // Try to reboot - ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, PX4IO_REBOOT_BL_MAGIC); - // tear down the px4io instance - delete g_dev; - g_dev = nullptr; + ret = dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, PX4IO_REBOOT_BL_MAGIC); + delete dev; if (ret != OK) { PX4_WARN("reboot failed - %d, still attempting upgrade", ret); } - PX4IO_Uploader *up; - /* Assume we are using default paths */ const char *fn[4] = PX4IO_FW_SEARCH_PATHS; /* Override defaults if a path is passed on command line */ - if (argc > 2) { - fn[0] = argv[2]; + if (argc > 1) { + fn[0] = argv[1]; fn[1] = nullptr; } - up = new PX4IO_Uploader; - ret = up->upload(&fn[0]); - delete up; + PX4IO_Uploader *up = new PX4IO_Uploader(); + + if (!up) { + ret = -ENOMEM; + + } else { + ret = up->upload(&fn[0]); + delete up; + } } switch (ret) { @@ -3556,185 +2327,151 @@ px4io_main(int argc, char *argv[]) return ret; } + /* commands below here require a started driver */ - - if (g_dev == nullptr) { - errx(1, "not started"); + if (!is_running()) { + PX4_ERR("not running"); + return 1; } - if (!strcmp(argv[1], "safety_off")) { - int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); + + if (!strcmp(verb, "safety_off")) { + int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); if (ret != OK) { - warnx("failed to disable safety"); - exit(1); + PX4_ERR("failed to disable safety (%i)", ret); + return 1; } - exit(0); + return 0; } - if (!strcmp(argv[1], "safety_on")) { - int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); + if (!strcmp(verb, "safety_on")) { + int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); if (ret != OK) { - warnx("failed to enable safety"); - exit(1); + PX4_ERR("failed to enable safety (%i)", ret); + return 1; } - exit(0); + return 0; } - if (!strcmp(argv[1], "recovery")) { - - /* - * Enable in-air restart support. - * We can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - - /* stop the driver */ - delete g_dev; - g_dev = nullptr; - exit(0); - } - - - if (!strcmp(argv[1], "status")) { - - warnx("loaded"); - g_dev->print_status(true); - - exit(0); - } - - if (!strcmp(argv[1], "debug")) { - if (argc <= 2) { - warnx("usage: px4io debug LEVEL"); - exit(1); + if (!strcmp(verb, "debug")) { + if (argc <= 1) { + PX4_ERR("usage: px4io debug LEVEL"); + return 1; } - if (g_dev == nullptr) { - warnx("not started"); - exit(1); - } - - uint8_t level = atoi(argv[2]); - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level); + uint8_t level = atoi(argv[1]); + int ret = get_instance()->ioctl(nullptr, PX4IO_SET_DEBUG, level); if (ret != 0) { - warnx("SET_DEBUG failed: %d", ret); - exit(1); + PX4_ERR("SET_DEBUG failed: %d", ret); + return 1; } - warnx("SET_DEBUG %" PRIu8 " OK", level); - exit(0); + PX4_INFO("SET_DEBUG %" PRIu8 " OK", level); + return 0; } - if (!strcmp(argv[1], "rx_dsm") || - !strcmp(argv[1], "rx_dsm_10bit") || - !strcmp(argv[1], "rx_dsm_11bit") || - !strcmp(argv[1], "rx_sbus") || - !strcmp(argv[1], "rx_ppm")) { - errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); + if (!strcmp(verb, "monitor")) { + return monitor(); } - if (!strcmp(argv[1], "monitor")) { - monitor(); + if (!strcmp(verb, "bind")) { + if (!is_running()) { + PX4_ERR("io must be running"); + return 1; + } + + return bind(argc - 1, argv + 1); } - if (!strcmp(argv[1], "bind")) { - bind(argc, argv); + if (!strcmp(verb, "lockdown")) { + return lockdown(argc, argv); } - if (!strcmp(argv[1], "lockdown")) { - lockdown(argc, argv); - } - - if (!strcmp(argv[1], "sbus1_out")) { - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); + if (!strcmp(verb, "sbus1_out")) { + int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); if (ret != 0) { - errx(ret, "S.BUS v1 failed"); + PX4_ERR("S.BUS v1 failed (%i)", ret); + return 1; } - exit(0); + return 0; } - if (!strcmp(argv[1], "sbus2_out")) { - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); + if (!strcmp(verb, "sbus2_out")) { + int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); if (ret != 0) { - errx(ret, "S.BUS v2 failed"); + PX4_ERR("S.BUS v2 failed (%i)", ret); + return 1; } - exit(0); + return 0; } - if (!strcmp(argv[1], "rssi_analog")) { - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1); - - if (ret != 0) { - errx(ret, "RSSI analog failed"); - } - - exit(0); + if (!strcmp(verb, "test_fmu_fail")) { + get_instance()->test_fmu_fail(true); + return 0; } - if (!strcmp(argv[1], "rssi_pwm")) { - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1); - - if (ret != 0) { - errx(ret, "RSSI PWM failed"); - } - - exit(0); + if (!strcmp(verb, "test_fmu_ok")) { + get_instance()->test_fmu_fail(false); + return 0; } - if (!strcmp(argv[1], "test_fmu_fail")) { - if (g_dev != nullptr) { - g_dev->test_fmu_fail(true); - exit(0); - - } else { - - errx(1, "px4io must be started first"); - } - } - - if (!strcmp(argv[1], "test_fmu_ok")) { - if (g_dev != nullptr) { - g_dev->test_fmu_fail(false); - exit(0); - - } else { - - errx(1, "px4io must be started first"); - } - } - -out: - errx(1, "need a command, try 'start', 'stop', 'status', 'monitor', 'debug ',\n" - "'recovery', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" - "'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm',\n" - "'test_fmu_fail', 'test_fmu_ok'"); + return print_usage("unknown command"); +} + +int PX4IO::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Output driver communicating with the IO co-processor. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("px4io", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + + PRINT_MODULE_USAGE_COMMAND_DESCR("detect", "Try to detect the presence of an IO"); + PRINT_MODULE_USAGE_COMMAND_DESCR("checkcrc", "Check CRC for a firmware file against current code on IO"); + PRINT_MODULE_USAGE_ARG("", "Firmware file", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("update", "Update IO firmware"); + PRINT_MODULE_USAGE_ARG("", "Firmware file", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("safety_off", "Turn off safety (force)"); + PRINT_MODULE_USAGE_COMMAND_DESCR("safety_on", "Turn on safety (force)"); + PRINT_MODULE_USAGE_COMMAND_DESCR("debug", "set IO debug level"); + PRINT_MODULE_USAGE_ARG("", "0=disabled, 9=max verbosity", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("monitor", "continuously monitor status"); + PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind"); + PRINT_MODULE_USAGE_ARG("dsm2|dsmx|dsmx8", "protocol", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("lockdown", "enable (or disable) lockdown"); + PRINT_MODULE_USAGE_ARG("disable", "disable lockdown", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("sbus1_out", "enable sbus1 out"); + PRINT_MODULE_USAGE_COMMAND_DESCR("sbus2_out", "enable sbus2 out"); + PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_fail", "test: turn off IO updates"); + PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_ok", "re-enable IO updates"); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + + +extern "C" __EXPORT int px4io_main(int argc, char *argv[]) +{ + if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { + PX4_ERR("PX4IO Not Supported"); + return -1; + } + return PX4IO::main(argc, argv); } diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index 542be13b88..84385e4216 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -45,11 +45,10 @@ /** * Set usage of IO board * - * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. + * Can be used to use a configure the use of the IO board. * - * @boolean - * @min 0 - * @max 1 + * @value 0 IO PWM disabled (RC only) + * @value 1 IO enabled (RC & PWM) * @reboot_required true * @group System */ diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index a46478b929..efb7065c35 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -51,24 +51,14 @@ device::Device PX4IO_serial::PX4IO_serial() : Device("PX4IO_serial"), - _pc_txns(perf_alloc(PC_ELAPSED, "io_txns")), -#if 0 - _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), - _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), - _pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")), - _pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")), - _pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")), - _pc_idle(perf_alloc(PC_COUNT, "io_idle ")), - _pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")), -#else - _pc_retries(nullptr), - _pc_timeouts(nullptr), - _pc_crcerrs(nullptr), - _pc_protoerrs(nullptr), - _pc_uerrs(nullptr), - _pc_idle(nullptr), - _pc_badidle(nullptr), -#endif + _pc_txns(perf_alloc(PC_ELAPSED, MODULE_NAME": txns")), + _pc_retries(perf_alloc(PC_COUNT, MODULE_NAME": retries")), + _pc_timeouts(perf_alloc(PC_COUNT, MODULE_NAME": timeouts")), + _pc_crcerrs(perf_alloc(PC_COUNT, MODULE_NAME": crcerrs")), + _pc_protoerrs(perf_alloc(PC_COUNT, MODULE_NAME": protoerrs")), + _pc_uerrs(perf_alloc(PC_COUNT, MODULE_NAME": uarterrs")), + _pc_idle(perf_alloc(PC_COUNT, MODULE_NAME": idle")), + _pc_badidle(perf_alloc(PC_COUNT, MODULE_NAME": badidle")), _bus_semaphore(SEM_INITIALIZER(0)) { g_interface = this; diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 217917477b..4010e0d513 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -355,9 +355,8 @@ int PX4IO_Uploader::get_sync(unsigned timeout) { uint8_t c[2]; - int ret; - ret = recv_byte_with_timeout(c, timeout); + int ret = recv_byte_with_timeout(c, timeout); if (ret != OK) { return ret; @@ -395,13 +394,11 @@ PX4IO_Uploader::sync() int PX4IO_Uploader::get_info(int param, uint32_t &val) { - int ret; - send(PROTO_GET_DEVICE); send(param); send(PROTO_EOC); - ret = recv_bytes((uint8_t *)&val, sizeof(val)); + int ret = recv_bytes((uint8_t *)&val, sizeof(val)); if (ret != OK) { return ret; @@ -419,7 +416,6 @@ PX4IO_Uploader::erase() return get_sync(10000); /* allow 10s timeout */ } - static int read_with_retry(int fd, void *buf, size_t n) { int ret; diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 29cdb3d3e6..9de2fc979c 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -509,7 +509,7 @@ void RCInput::Run() for (unsigned i = 0; i < (unsigned)newBytes; i++) { /* set updated flag if one complete packet was parsed */ - st24_rssi = RC_INPUT_RSSI_MAX; + st24_rssi = input_rc_s::RSSI_MAX; rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); } @@ -561,7 +561,7 @@ void RCInput::Run() for (unsigned i = 0; i < (unsigned)newBytes; i++) { /* set updated flag if one complete packet was parsed */ - sumd_rssi = RC_INPUT_RSSI_MAX; + sumd_rssi = input_rc_s::RSSI_MAX; rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count, &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe)); } diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index e74ea08b9d..41c7dbbea1 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -38,7 +38,6 @@ #include #include #include -#include #include #include #include diff --git a/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp b/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp index 3a9debab3f..228b422500 100644 --- a/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp +++ b/src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp @@ -46,7 +46,6 @@ #include #include -#include #include #include diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 7ceb7ebc18..a322b8513d 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -32,5 +32,5 @@ ############################################################################ px4_add_library(mixer_module mixer_module.cpp) - +target_link_libraries(mixer_module PRIVATE output_limit) target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 9d654785e5..e2f11c0196 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -89,7 +89,11 @@ MixingOutput::~MixingOutput() void MixingOutput::printStatus() const { perf_print_counter(_control_latency_perf); - PX4_INFO("Switched to rate_ctrl work queue: %i", (int)_wq_switched); + + if (_wq_switched) { + PX4_INFO("Switched to rate_ctrl work queue"); + } + PX4_INFO("Mixer loaded: %s", _mixers ? "yes" : "no"); PX4_INFO("Driver instance: %i", _driver_instance); diff --git a/src/lib/rc/sbus.cpp b/src/lib/rc/sbus.cpp index 3d46bfee91..3a1f992ecf 100644 --- a/src/lib/rc/sbus.cpp +++ b/src/lib/rc/sbus.cpp @@ -50,6 +50,7 @@ #include "sbus.h" #include "common_rc.h" #include +#include using namespace time_literals; @@ -689,13 +690,6 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num */ void sbus1_set_output_rate_hz(uint16_t rate_hz) { - if (rate_hz > SBUS1_MAX_RATE_HZ) { - rate_hz = SBUS1_MAX_RATE_HZ; - } - - if (rate_hz < SBUS1_MIN_RATE_HZ) { - rate_hz = SBUS1_MIN_RATE_HZ; - } - - sbus1_frame_delay = (1000U * 1000U) / rate_hz; + sbus1_frame_delay = (1000U * 1000U) / math::constrain(rate_hz, (uint16_t)SBUS1_MIN_RATE_HZ, + (uint16_t)SBUS1_MAX_RATE_HZ); } diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index c5829445e5..44b86e01f9 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -83,22 +83,6 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); */ PARAM_DEFINE_INT32(SYS_HITL, 0); -/** - * Set restart type - * - * Set by px4io to indicate type of restart - * - * @min 0 - * @max 2 - * @value 0 Data survives resets - * @value 1 Data survives in-flight resets only - * @value 2 Data does not survive reset - * @category system - * @volatile - * @group System - */ -PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); - /** * Set multicopter estimator group * diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ce2a37757b..bf1d82ea73 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2467,7 +2467,7 @@ Commander::run() } // evaluate the main state machine according to mode switches - if (set_main_state(_status_changed) == TRANSITION_CHANGED) { + if (set_main_state() == TRANSITION_CHANGED) { // play tune on mode change only if armed, blink LED always tune_positive(_armed.armed); _status_changed = true; @@ -3074,29 +3074,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning) _leds_counter++; } -transition_result_t -Commander::set_main_state(bool &changed) -{ - if (_safety.override_available && _safety.override_enabled) { - return set_main_state_override_on(changed); - - } else { - return set_main_state_rc(); - } -} - -transition_result_t -Commander::set_main_state_override_on(bool &changed) -{ - const transition_result_t res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, - _internal_state); - changed = (res == TRANSITION_CHANGED); - - return res; -} - -transition_result_t -Commander::set_main_state_rc() +transition_result_t Commander::set_main_state() { if ((_manual_control_switches.timestamp == 0) || (_manual_control_switches.timestamp == _last_manual_control_switches.timestamp)) { @@ -3113,14 +3091,8 @@ Commander::set_main_state_rc() bool should_evaluate_rc_mode_switch = (_last_manual_control_switches.offboard_switch != _manual_control_switches.offboard_switch) || (_last_manual_control_switches.return_switch != _manual_control_switches.return_switch) - || (_last_manual_control_switches.mode_switch != _manual_control_switches.mode_switch) - || (_last_manual_control_switches.acro_switch != _manual_control_switches.acro_switch) - || (_last_manual_control_switches.posctl_switch != _manual_control_switches.posctl_switch) || (_last_manual_control_switches.loiter_switch != _manual_control_switches.loiter_switch) - || (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot) - || (_last_manual_control_switches.stab_switch != _manual_control_switches.stab_switch) - || (_last_manual_control_switches.man_switch != _manual_control_switches.man_switch); - + || (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot); if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // if already armed don't evaluate first time RC @@ -3216,84 +3188,6 @@ Commander::set_main_state_rc() } return res; - - } else if (_manual_control_switches.mode_switch != manual_control_switches_s::SWITCH_POS_NONE) { - /* offboard and RTL switches off or denied, check main mode switch */ - switch (_manual_control_switches.mode_switch) { - case manual_control_switches_s::SWITCH_POS_NONE: - res = TRANSITION_NOT_CHANGED; - break; - - case manual_control_switches_s::SWITCH_POS_OFF: // MANUAL - if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_NONE && - _manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) { - /* - * Legacy mode: - * Acro switch being used as stabilized switch in FW. - */ - if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) { - /* manual mode is stabilized already for multirotors, so switch to acro - * for any non-manual mode - */ - if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_status.is_vtol) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state); - - } else if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state); - - } else { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); - } - - } else { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); - } - - } else { - /* New mode: - * - Acro is Acro - * - Manual is not default anymore when the manual switch is assigned - */ - if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); - - } else if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state); - - } else if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state); - - } else if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) { - // default to MANUAL when no manual switch is set - res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); - - } else { - // default to STAB when the manual switch is assigned (but off) - res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state); - } - } - - // TRANSITION_DENIED is not possible here - break; - - case manual_control_switches_s::SWITCH_POS_MIDDLE: // ASSIST - if (_manual_control_switches.posctl_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = try_mode_change(commander_state_s::MAIN_STATE_POSCTL); - - } else { - res = try_mode_change(commander_state_s::MAIN_STATE_ALTCTL); - } - - break; - - case manual_control_switches_s::SWITCH_POS_ON: // AUTO - res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_MISSION); - break; - - default: - break; - } - } return res; @@ -3374,9 +3268,6 @@ Commander::update_control_mode() /* set vehicle_control_mode according to set_navigation_state */ _vehicle_control_mode.flag_armed = _armed.armed; - _vehicle_control_mode.flag_external_manual_override_ok = - (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol); - switch (_status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: _vehicle_control_mode.flag_control_manual_enabled = true; @@ -3409,10 +3300,6 @@ Commander::update_control_mode() break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - /* override is not ok for the RTL and recovery mode */ - _vehicle_control_mode.flag_external_manual_override_ok = false; - - /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 0c14b2f755..490d5a231a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -173,14 +173,8 @@ private: void UpdateEstimateValidity(); - // Set the main system state based on RC and override device inputs - transition_result_t set_main_state(bool &changed); - - // Enable override (manual reversion mode) on the system - transition_result_t set_main_state_override_on(bool &changed); - // Set the system main state based on the current RC inputs - transition_result_t set_main_state_rc(); + transition_result_t set_main_state(); bool shutdown_if_allowed(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d3528261d6..ec149d5ad1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -40,10 +40,9 @@ * @author Thomas Gubler */ -#include -#include -#include -#include +#include +#include +#include #include #include @@ -1889,7 +1888,7 @@ MavlinkReceiver::handle_message_rc_channels(mavlink_message_t *msg) input_rc_s rc{}; rc.timestamp_last_signal = hrt_absolute_time(); - rc.rssi = RC_INPUT_RSSI_MAX; + rc.rssi = input_rc_s::RSSI_MAX; // TODO: fake RSSI from dropped messages? // for (auto &component_state : _component_states) { @@ -1959,7 +1958,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) // metadata rc.timestamp = hrt_absolute_time(); rc.timestamp_last_signal = rc.timestamp; - rc.rssi = RC_INPUT_RSSI_MAX; + rc.rssi = input_rc_s::RSSI_MAX; rc.rc_failsafe = false; rc.rc_lost = false; rc.rc_lost_frame_count = 0; @@ -2031,7 +2030,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.rc_total_frame_count = 1; rc.rc_ppm_frame_length = 0; rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; - rc.rssi = RC_INPUT_RSSI_MAX; + rc.rssi = input_rc_s::RSSI_MAX; rc.values[0] = man.x / 2 + 1500; // roll rc.values[1] = man.y / 2 + 1500; // pitch diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp index 3ddb1b48d8..1f1c9e67cc 100644 --- a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp +++ b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -78,11 +78,8 @@ private: if (_manual_control_switches_sub.copy(&manual_control_switches)) { unsigned shift = 2; msg.buttons = 0; - msg.buttons |= (manual_control_switches.mode_switch << (shift * 0)); msg.buttons |= (manual_control_switches.return_switch << (shift * 1)); - msg.buttons |= (manual_control_switches.posctl_switch << (shift * 2)); msg.buttons |= (manual_control_switches.loiter_switch << (shift * 3)); - msg.buttons |= (manual_control_switches.acro_switch << (shift * 4)); msg.buttons |= (manual_control_switches.offboard_switch << (shift * 5)); msg.buttons |= (manual_control_switches.kill_switch << (shift * 6)); } diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 1f0ebc3f57..7f3958b067 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -51,9 +51,7 @@ target_link_libraries(px4iofirmware nuttx_apps nuttx_arch nuttx_c - mixer rc - output_limit ) if(PX4IO_PERF) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index b6d079d6f3..7d22df983c 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -43,12 +43,12 @@ #include #include -#include #include #include #include #include #include +#include #if defined(PX4IO_PERF) # include @@ -56,9 +56,6 @@ #include "px4io.h" -#define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */ -#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ - static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated); @@ -71,13 +68,11 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd = -1; int _sbus_fd = -1; -static uint16_t rc_value_override = 0; - #ifdef ADC_RSSI static unsigned _rssi_adc_counts = 0; #endif -/* receive signal strenght indicator (RSSI). 0 = no connection, 100 (RC_INPUT_RSSI_MAX): perfect connection */ +/* receive signal strenght indicator (RSSI). 0 = no connection, 100 (input_rc_s::RSSI_MAX): perfect connection */ /* Note: this is static because RC-provided telemetry does not occur every tick */ static uint16_t _rssi = 0; static unsigned _frame_drops = 0; @@ -136,7 +131,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SUMD))) { for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ - st24_rssi = RC_INPUT_RSSI_MAX; + st24_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX; *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count, &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } @@ -166,7 +161,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24))) { for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ - sumd_rssi = RC_INPUT_RSSI_MAX; + sumd_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX; *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state)); } @@ -204,19 +199,6 @@ controls_init(void) /* S.bus input (USART3) */ _sbus_fd = sbus_init("/dev/ttyS2", false); - /* default to a 1:1 input map, all enabled */ - for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) { - unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; - - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i; - r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - } - #if defined(PX4IO_PERF) c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm"); c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus"); @@ -246,11 +228,11 @@ controls_tick() _rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f); /* use 1:1 scaling on 3.3V, 12-Bit ADC input */ unsigned mV = _rssi_adc_counts * 3300 / 4095; - /* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */ - _rssi = (mV * RC_INPUT_RSSI_MAX / 3300); + /* scale to 0..100 (input_rc_s::RSSI_MAX == 100) */ + _rssi = (mV * INPUT_RC_RSSI_MAX / 3300); - if (_rssi > RC_INPUT_RSSI_MAX) { - _rssi = RC_INPUT_RSSI_MAX; + if (_rssi > INPUT_RC_RSSI_MAX) { + _rssi = INPUT_RC_RSSI_MAX; } } } @@ -276,11 +258,11 @@ controls_tick() if (sbus_updated) { atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS); - unsigned sbus_rssi = RC_INPUT_RSSI_MAX; + unsigned sbus_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX if (sbus_frame_drop) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; - sbus_rssi = RC_INPUT_RSSI_MAX / 2; + sbus_rssi = INPUT_RC_RSSI_MAX / 2; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); @@ -297,7 +279,6 @@ controls_tick() if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { _rssi = sbus_rssi; } - } } @@ -377,103 +358,6 @@ controls_tick() /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { - - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - - uint16_t raw = r_raw_rc_values[i]; - - int16_t scaled; - - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) { - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - } - - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) { - raw = conf[PX4IO_P_RC_CONFIG_MAX]; - } - - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( - conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)( - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } - - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) { - scaled = -scaled; - } - - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - - if (mapped < PX4IO_CONTROL_CHANNELS) { - - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) { - /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; - } - - if (mapped == 3 && r_setup_rc_thr_failsafe) { - /* throttle failsafe detection */ - if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || - ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; - - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - } - } - - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); - - } else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) { - /* pick out override channel, indicated by special mapping */ - rc_value_override = SIGNED_TO_REG(scaled); - } - } - } - - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) { - r_rc_values[i] = 0; - } - } - /* set RC OK flag, as we got an update */ atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK); r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK; @@ -485,11 +369,6 @@ controls_tick() } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } - - /* - * Export the valid channel bitmap - */ - r_rc_valid = assigned_channels; } /* @@ -513,85 +392,20 @@ controls_tick() * Handle losing RC input */ - /* if we are in failsafe, clear the override flag */ - if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { - atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); - } - /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ - atomic_modify_clear(&r_status_flags, ( - PX4IO_P_STATUS_FLAGS_OVERRIDE | - PX4IO_P_STATUS_FLAGS_RC_OK)); + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK); /* flag raw RC as lost */ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); - /* Mark all channels as invalid, as we just lost the RX */ - r_rc_valid = 0; - /* Set raw channel count to zero */ r_raw_rc_count = 0; /* Set the RC_LOST alarm */ atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_RC_LOST); } - - /* - * Check for manual override. - * - * Firstly, manual override must be enabled, RC input available and a mixer loaded. - */ - if (/* condition 1: Override is always allowed */ - (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - /* condition 2: We have valid RC control inputs from the user */ - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - /* condition 3: The system didn't go already into failsafe mode with fixed outputs */ - !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) && - /* condition 4: RC handling wasn't generally disabled */ - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - /* condition 5: We have a valid mixer to map RC inputs to actuator outputs */ - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - - bool override = false; - - /* - * Check mapped channel 5 (can be any remote channel, - * depends on RC_MAP_OVER parameter); - * If the value is 'high' then the pilot has - * requested override. - * - */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) { - override = true; - } - - /* - * If the FMU is dead then enable override if we have a mixer - * and we want to immediately override (instead of using the RC channel - * as in the case above. - * - * Also, do not enter manual override if we asked for termination - * failsafe and FMU is lost. - */ - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { - override = true; - } - - if (override) { - atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); - - } else { - atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); - } - - } else { - atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE); - } } static bool diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6d6de6261b..2e0e5bb14f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -51,14 +51,8 @@ #include #include -#include -#include #include -#include - -#include "mixer.h" - extern "C" { /* #define DEBUG */ #include "px4io.h" @@ -74,7 +68,6 @@ static volatile bool mixer_servos_armed = false; static volatile bool should_arm = false; static volatile bool should_arm_nothrottle = false; static volatile bool should_always_enable_pwm = false; -static volatile bool mix_failsafe = false; static bool new_fmu_data = false; static uint64_t last_fmu_update = 0; @@ -85,31 +78,12 @@ extern int _sbus_fd; enum mixer_source { MIX_NONE, MIX_DISARMED, - MIX_FMU, - MIX_OVERRIDE, MIX_FAILSAFE, - MIX_OVERRIDE_FMU_OK }; -static volatile mixer_source source; - -static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control); -static int mixer_handle_text_create_mixer(); -static void mixer_mix_failsafe(); - -static MixerGroup mixer_group; - void mixer_tick() { - /* check if the mixer got modified */ - mixer_handle_text_create_mixer(); - - if (mix_failsafe) { - mixer_mix_failsafe(); - mix_failsafe = false; - } - /* check that we are receiving fresh data from the FMU */ irqstate_t irq_flags = enter_critical_section(); const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time; @@ -123,8 +97,7 @@ mixer_tick() isr_debug(1, "AP RX timeout"); } - atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FMU_OK)); - atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_FMU_LOST); + atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK); } else { atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK); @@ -139,7 +112,7 @@ mixer_tick() } /* default to disarmed mixing */ - source = MIX_DISARMED; + mixer_source source = MIX_DISARMED; /* * Decide which set of controls we're using. @@ -149,39 +122,9 @@ mixer_tick() if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) > 0) { - /* a channel based override has been - * triggered, with FMU active */ - source = MIX_OVERRIDE_FMU_OK; - - } else { - /* don't actually mix anything - copy values from r_page_direct_pwm */ - source = MIX_NONE; - memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT); - } - - } else { - - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - - /* mix from FMU controls */ - source = MIX_FMU; - } - - else if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { - - if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - - /* if allowed, mix from RC inputs directly up to available rc channels */ - source = MIX_OVERRIDE_FMU_OK; - - } else { - /* if allowed, mix from RC inputs directly */ - source = MIX_OVERRIDE; - } - } + /* don't actually mix anything - copy values from r_page_direct_pwm */ + source = MIX_NONE; + memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT); } /* @@ -191,30 +134,22 @@ mixer_tick() * FMU or from the mixer. * */ - should_arm = ( - (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ - && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */ - || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */ - ) - ); + should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ + && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */ + ; - should_arm_nothrottle = ( - (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ - && (((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */ - || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */ - ) - ); + should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ + && ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */ + || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */ + )); - should_always_enable_pwm = ( - (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) - ); + /* we enable PWM output always on the IO side if FMU is up and running + * as zero-outputs can be controlled by FMU by sending a 0 PWM command + */ + should_always_enable_pwm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)); /* * Check if FMU is still alive, if not, terminate the flight @@ -243,100 +178,19 @@ mixer_tick() atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE)); } - /* - * Set simple mixer trim values. If the OK flag is set the mixer is fully loaded. - */ - if (update_trims && r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) { - update_trims = false; - mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT); - } - - /* - * Update air-mode parameter - */ - mixer_group.set_airmode((Mixer::Airmode)REG_TO_SIGNED(r_setup_airmode)); - - /* * Run the mixers. */ if (source == MIX_FAILSAFE) { - /* copy failsafe values to the servo outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; - - /* safe actuators for FMU feedback */ - r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } } else if (source == MIX_DISARMED) { - /* copy disarmed values to the servo outputs */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_disarmed[i]; - - /* safe actuators for FMU feedback */ - r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); - } - - } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) - && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { - - float outputs[PX4IO_SERVO_COUNT]; - unsigned mixed; - - if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) { - /* maximum value the outputs of the multirotor mixer are allowed to change in this cycle - * factor 2 is needed because actuator outputs are in the range [-1,1] - */ - float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT( - r_setup_slew_max); - mixer_group.set_max_delta_out_once(delta_out_max); - } - - /* set dt to be used in simple mixer for slew rate limiting */ - mixer_group.set_dt_once(dt); - - /* update parameter for mc thrust model if it updated */ - if (update_mc_thrust_param) { - mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac)); - update_mc_thrust_param = false; - } - - /* mix */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) { - mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); - r_mixer_limits = mixer_group.get_saturation_status(); - - } else { - mixed = 0; - } - - /* the pwm limit call takes care of out of band errors */ - output_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, - r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - - /* clamp unused outputs to zero */ - for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { - r_page_servos[i] = 0; - outputs[i] = 0.0f; - } - - /* store normalized outputs */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - } - - - if (mixed && new_fmu_data) { - new_fmu_data = false; - - /* Trigger all timer's channels in Oneshot mode to fire - * the oneshots with updated values. - */ - - up_pwm_update(); } } @@ -372,7 +226,6 @@ mixer_tick() } /* set S.BUS1 or S.BUS2 outputs */ - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); @@ -380,8 +233,7 @@ mixer_tick() sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); } - } else if (mixer_servos_armed && (should_always_enable_pwm - || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) { + } else if (mixer_servos_armed && (should_always_enable_pwm || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servo_disarmed[i]); @@ -399,283 +251,3 @@ mixer_tick() } } } - -static int -mixer_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &control) -{ - control = 0.0f; - - if (control_group >= PX4IO_CONTROL_GROUPS) { - return -1; - } - - switch (source) { - case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { - if (r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)] == INT16_MAX) { - //catch NAN values encoded as INT16 max for disarmed outputs - control = NAN; - - } else { - control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); - } - - break; - } - - return -1; - - case MIX_OVERRIDE: - if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { - control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); - break; - } - - return -1; - - case MIX_OVERRIDE_FMU_OK: - - /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ - if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { - control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); - break; - - } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { - control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); - break; - } - - return -1; - - case MIX_DISARMED: - case MIX_FAILSAFE: - case MIX_NONE: - control = 0.0f; - return -1; - } - - /* apply trim offsets for override channels */ - if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { - if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_ROLL) { - control *= REG_TO_FLOAT(r_setup_scale_roll); - control += REG_TO_FLOAT(r_setup_trim_roll); - - } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_PITCH) { - control *= REG_TO_FLOAT(r_setup_scale_pitch); - control += REG_TO_FLOAT(r_setup_trim_pitch); - - } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_YAW) { - control *= REG_TO_FLOAT(r_setup_scale_yaw); - control += REG_TO_FLOAT(r_setup_trim_yaw); - } - } - - /* limit output */ - control = math::constrain(control, -1.f, 1.f); - - /* motor spinup phase - lock throttle to zero */ - if ((pwm_limit.state == OUTPUT_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { - if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || - control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && - control_index == actuator_controls_s::INDEX_THROTTLE) { - /* limit the throttle output to zero during motor spinup, - * as the motors cannot follow any demand yet - */ - control = 0.0f; - } - } - - /* only safety off, but not armed - set throttle as invalid */ - if (should_arm_nothrottle && !should_arm) { - if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || - control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && - control_index == actuator_controls_s::INDEX_THROTTLE) { - /* mark the throttle as invalid */ - control = NAN; - } - } - - return 0; -} - -/* - * XXX error handling here should be more aggressive; currently it is - * possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has - * not loaded faithfully. - */ - -static char mixer_text[PX4IO_MAX_MIXER_LENGTH]; /* large enough for one mixer */ -static unsigned mixer_text_length = 0; -static volatile bool mixer_update_pending = false; -static volatile bool mixer_reset_pending = false; - -int -mixer_handle_text_create_mixer() -{ - /* do not allow a mixer change while safety off and FMU armed */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - return 1; - } - - if (mixer_reset_pending) { - mixer_group.reset(); - mixer_reset_pending = false; - } - - /* only run on update */ - if (!mixer_update_pending || (mixer_text_length == 0)) { - return 0; - } - - /* process the text buffer, adding new mixers as their descriptions can be parsed */ - unsigned resid = mixer_text_length; - mixer_group.load_from_buf(mixer_callback, 0, &mixer_text[0], resid); - - /* if anything was parsed */ - if (resid != mixer_text_length) { - - isr_debug(2, "used %u", mixer_text_length - resid); - - /* copy any leftover text to the base of the buffer for re-use */ - if (resid > 0) { - memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); - /* enforce null termination */ - mixer_text[resid] = '\0'; - } - - mixer_text_length = resid; - } - - mixer_update_pending = false; - - update_trims = true; - update_mc_thrust_param = true; - - return 0; -} - -int interrupt_mixer_handle_text(const void *buffer, size_t length) -{ - /* do not allow a mixer change while safety off and FMU armed */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - return 1; - } - - /* disable mixing, will be enabled once load is complete */ - atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); - - px4io_mixdata *msg = (px4io_mixdata *)buffer; - - isr_debug(2, "mix txt %u", length); - - if (length < sizeof(px4io_mixdata)) { - return 0; - } - - unsigned text_length = length - sizeof(px4io_mixdata); - - switch (msg->action) { - case F2I_MIXER_ACTION_RESET: - isr_debug(2, "reset"); - - /* THEN actually delete it */ - mixer_reset_pending = true; - mixer_text_length = 0; - - /* FALLTHROUGH */ - case F2I_MIXER_ACTION_APPEND: - isr_debug(2, "append %d", length); - - /* check for overflow - this would be really fatal */ - if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { - atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK); - return 0; - } - - /* check if the last item has been processed - bail out if not */ - if (mixer_update_pending) { - return 1; - } - - /* append mixer text and nul-terminate, guard against overflow */ - memcpy(&mixer_text[mixer_text_length], msg->text, text_length); - mixer_text_length += text_length; - mixer_text[mixer_text_length] = '\0'; - isr_debug(2, "buflen %u", mixer_text_length); - - /* flag the buffer as ready */ - mixer_update_pending = true; - - break; - } - - return 0; -} - -void interrupt_mixer_set_failsafe() -{ - mix_failsafe = true; -} - -void -mixer_mix_failsafe() -{ - /* - * Check if a custom failsafe value has been written, - * or if the mixer is not ok and bail out. - */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - return; - } - - /* set failsafe defaults to the values for all inputs = 0 */ - float outputs[PX4IO_SERVO_COUNT]; - unsigned mixed; - - if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) { - /* maximum value the outputs of the multirotor mixer are allowed to change in this cycle - * factor 2 is needed because actuator outputs are in the range [-1,1] - */ - float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT( - r_setup_slew_max); - mixer_group.set_max_delta_out_once(delta_out_max); - } - - /* set dt to be used in simple mixer for slew rate limiting */ - mixer_group.set_dt_once(dt); - - /* update parameter for mc thrust model if it updated */ - if (update_mc_thrust_param) { - mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac)); - update_mc_thrust_param = false; - } - - /* mix */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) { - mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); - r_mixer_limits = mixer_group.get_saturation_status(); - - } else { - mixed = 0; - } - - /* scale to PWM and update the servo outputs as required */ - for (unsigned i = 0; i < mixed; i++) { - /* scale to servo output */ - r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500; - } - - /* disable the rest of the outputs */ - for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { - r_page_servo_failsafe[i] = 0; - } -} diff --git a/src/modules/px4iofirmware/mixer.h b/src/modules/px4iofirmware/mixer.h deleted file mode 100644 index ea1335cfcd..0000000000 --- a/src/modules/px4iofirmware/mixer.h +++ /dev/null @@ -1,42 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017-2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mixer.h - * - * PX4IO mixer definitions - * - * @author Lorenz Meier - */ -#pragma once -#define PX4IO_MAX_MIXER_LENGTH 330 diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 3cfcee2d12..178b06c41d 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -61,7 +61,7 @@ * the PX4 system are expressed as signed integer values scaled by 10000, * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and * SIGNED_TO_REG macros to convert between register representation and - * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float. + * the signed version. * * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be @@ -75,76 +75,58 @@ #define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) #define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) -#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) -#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f)) - #define REG_TO_BOOL(_reg) ((bool)(_reg)) -#define PX4IO_PROTOCOL_VERSION 4 +#define PX4IO_PROTOCOL_VERSION 5 /* maximum allowable sizes on this protocol version */ #define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */ /* static configuration page */ -#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ #define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ -#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ +#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ #define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ #define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ -#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ -#define PX4IO_MAX_TRANSFER_LEN 64 +#define PX4IO_MAX_TRANSFER_LEN 64 /* dynamic status page */ -#define PX4IO_PAGE_STATUS 1 +#define PX4IO_PAGE_STATUS 1 #define PX4IO_P_STATUS_FREEMEM 0 #define PX4IO_P_STATUS_CPULOAD 1 #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ #define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ -#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ -#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ -#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ -#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ -#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ -#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ -#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ -#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ -#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ -#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ -#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ -#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ -#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ -#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */ -#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 15) /* SUMD input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 1) /* RC input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 2) /* PPM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */ +#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 5) /* controls from FMU are valid */ +#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 6) /* raw PWM from FMU */ +#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 7) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 8) /* initialisation of the IO completed without error */ +#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 9) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 10) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 11) /* FMU was initialized and OK once */ +#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 12) /* ST24 input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 13) /* SUMD input is valid */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ -#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ -#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ -#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ -#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ -#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ -#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ -#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ -#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ +#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 0) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 1) /* PWM configuration or output was bad */ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ -#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ - -#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */ - -/* array of post-mix actuator outputs, -10000..10000 */ -#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* array of PWM servo output values, microseconds */ -#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* array of raw RC input values, microseconds */ -#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_PAGE_RAW_RC_INPUT 4 #define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ #define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ #define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ @@ -159,47 +141,34 @@ #define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ #define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ -/* array of scaled RC input values, -10000..10000 */ -#define PX4IO_PAGE_RC_INPUT 5 -#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ -#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ - /* array of raw ADC values */ -#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ /* PWM servo information */ -#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_PAGE_PWM_INFO 7 #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 50 +#define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ #define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ -#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ -#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 2) /**< enable ADC RSSI parsing */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ -#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ -#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ -#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */ -#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 3) /* OK to switch to manual override via override RC channel */ -#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* use custom failsafe values, not 0 values of mixer */ -#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */ -#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ -#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 7) /* Disable the IO-internal evaluation of the RC */ -#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 8) /* If set, the system operates normally, but won't actuate any servos */ -#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 9) /* If set, the system will always output the failsafe values */ -#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ -#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ +#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ +#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ +#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 4) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 5) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 6) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ -#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ -#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_RELAYS_PAD 5 - -#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ -#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_VSERVO_SCALE 5 /* hardware rev [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 6 /* DSM bind state */ enum { /* DSM bind states */ dsm_bind_power_down = 0, dsm_bind_power_up, @@ -218,113 +187,33 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into 'armed' (PWM enabled) state - this is a non-data write and hence index 12 can safely be used. */ -#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ #define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */ #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ - -#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ -#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ -#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ -#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ -#define PX4IO_P_SETUP_SCALE_ROLL 19 /**< Roll scale, in actuator units */ -#define PX4IO_P_SETUP_SCALE_PITCH 20 /**< Pitch scale, in actuator units */ -#define PX4IO_P_SETUP_SCALE_YAW 21 /**< Yaw scale, in actuator units */ - -#define PX4IO_P_SETUP_SBUS_RATE 22 /**< frame rate of SBUS1 output in Hz */ - -#define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /**< max motor slew rate */ - -#define PX4IO_P_SETUP_THR_MDL_FAC 25 /**< factor for modelling motor control signal output to static thrust relationship */ - -#define PX4IO_P_SETUP_THERMAL 26 /**< thermal management */ - -#define PX4IO_P_SETUP_AIRMODE 27 /**< air-mode */ - -#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 28 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */ +#define PX4IO_P_SETUP_SBUS_RATE 16 /**< frame rate of SBUS1 output in Hz */ +#define PX4IO_P_SETUP_THERMAL 17 /**< thermal management */ +#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 18 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */ #define PX4IO_THERMAL_IGNORE UINT16_MAX #define PX4IO_THERMAL_OFF 0 #define PX4IO_THERMAL_FULL 10000 -/* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ -#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ -#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ -#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ -#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ - -#define PX4IO_P_CONTROLS_GROUP_VALID 64 -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ - -/* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 52 - -/* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */ -#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */ -#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */ -#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */ -#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */ -#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */ -#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */ -#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */ -#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0) -#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1) -#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */ - -/* PWM output - overrides mixer */ +/* PWM output */ #define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ -/* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */ -#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */ - /* Debug and test page - not used in normal operation */ #define PX4IO_PAGE_TEST 127 #define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ -/* PWM minimum values for certain ESCs */ -#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ - -/* PWM maximum values for certain ESCs */ -#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ - -/* PWM mtrim values for central position */ -#define PX4IO_PAGE_CONTROL_TRIM_PWM 108 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ - /* PWM disarmed values that are active, even when SAFETY_SAFE */ -#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ - -/** - * As-needed mixer data upload. - * - * This message adds text to the mixer text buffer; the text - * buffer is drained as the definitions are consumed. - */ -#pragma pack(push, 1) -struct px4io_mixdata { - uint16_t f2i_mixer_magic; -#define F2I_MIXER_MAGIC 0x6d74 - - uint8_t action; -#define F2I_MIXER_ACTION_RESET 0 -#define F2I_MIXER_ACTION_APPEND 1 - - char text[0]; /* actual text size may vary */ -}; -#pragma pack(pop) +#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * Serial protocol encapsulation. */ - #define PKT_MAX_REGS 32 // by agreement w/FMU #pragma pack(push, 1) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index bd23afadd3..a84ad857d7 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -59,8 +59,6 @@ # include #endif -#include - #include #define DEBUG @@ -72,10 +70,6 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -output_limit_t pwm_limit; - -float dt; - /* * a set of debug buffers to allow us to send debug information from ISRs */ @@ -313,11 +307,6 @@ user_start(int argc, char *argv[]) LED_RING(false); #endif - /* turn on servo power (if supported) */ -#ifdef POWER_SERVO - POWER_SERVO(true); -#endif - /* turn off S.Bus out (if supported) */ #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(false); @@ -350,9 +339,6 @@ user_start(int argc, char *argv[]) r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk; syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); - /* initialize PWM limit lib */ - output_limit_init(&pwm_limit); - /* Start the failsafe led init */ failsafe_led_init(); @@ -362,23 +348,12 @@ user_start(int argc, char *argv[]) uint64_t last_debug_time = 0; uint64_t last_heartbeat_time = 0; - uint64_t last_loop_time = 0; watchdog_init(); for (;;) { watchdog_pet(); - dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f; - last_loop_time = hrt_absolute_time(); - - if (dt < 0.0001f) { - dt = 0.0001f; - - } else if (dt > 0.02f) { - dt = 0.02f; - } - #if defined(PX4IO_PERF) /* track the rate at which the loop is running */ perf_count(loop_perf); @@ -416,10 +391,6 @@ user_start(int argc, char *argv[]) */ uint32_t heartbeat_period_us = 250 * 1000UL; - if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) { - heartbeat_period_us /= 4; - } - if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) { last_heartbeat_time = hrt_absolute_time(); heartbeat_blink(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 5c2f305409..c815f8b578 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -50,15 +50,13 @@ #include "protocol.h" -#include - /* * Constants and limits. */ #define PX4IO_BL_VERSION 3 #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_CONTROL_GROUPS 4 + #define PX4IO_RC_INPUT_CHANNELS 18 #define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ @@ -77,20 +75,12 @@ * Registers. */ extern volatile uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */ -extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */ extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */ extern uint16_t r_page_direct_pwm[]; /* PX4IO_PAGE_DIRECT_PWM */ extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */ -extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */ -extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ -extern uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ -extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ -extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ -extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ -extern int16_t r_page_servo_control_trim[]; /* PX4IO_PAGE_CONTROL_TRIM_PWM */ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ /* @@ -104,32 +94,16 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE]) #define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] -#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] -#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) -#define r_mixer_limits r_page_status[PX4IO_P_STATUS_MIXER] #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] #define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] #define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] #define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] -#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] -#define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] - -#define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL] -#define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH] -#define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW] -#define r_setup_scale_roll r_page_setup[PX4IO_P_SETUP_SCALE_ROLL] -#define r_setup_scale_pitch r_page_setup[PX4IO_P_SETUP_SCALE_PITCH] -#define r_setup_scale_yaw r_page_setup[PX4IO_P_SETUP_SCALE_YAW] #define r_setup_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE] -#define r_setup_thr_fac r_page_setup[PX4IO_P_SETUP_THR_MDL_FAC] -#define r_setup_slew_max r_page_setup[PX4IO_P_SETUP_MOTOR_SLEW_MAX] -#define r_setup_airmode r_page_setup[PX4IO_P_SETUP_AIRMODE] -#define r_setup_flighttermination r_page_setup[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] -#define r_control_values (&r_page_controls[0]) +#define r_setup_flighttermination r_page_setup[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] /* * System state structure. @@ -146,15 +120,9 @@ struct sys_state_s { }; extern struct sys_state_s system_state; -extern float dt; extern bool update_mc_thrust_param; extern bool update_trims; -/* - * PWM limit structure - */ -extern output_limit_t pwm_limit; - /* * GPIO handling. */ @@ -169,7 +137,6 @@ extern output_limit_t pwm_limit; #define LED_RING(_s) px4_arch_gpiowrite(GPIO_LED4, (_s)) -# define PX4IO_RELAY_CHANNELS 0 # define ENABLE_SBUS_OUT(_s) px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) # define VDD_SERVO_FAULT (!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT)) @@ -180,8 +147,6 @@ extern output_limit_t pwm_limit; #define BUTTON_SAFETY px4_arch_gpioread(GPIO_BTN_SAFETY) -#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel) - #define PX4_CRITICAL_SECTION(cmd) { irqstate_t flags = px4_enter_critical_section(); cmd; px4_leave_critical_section(flags); } void atomic_modify_or(volatile uint16_t *target, uint16_t modification); @@ -192,9 +157,6 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification); * Mixer */ extern void mixer_tick(void); -extern int interrupt_mixer_handle_text(const void *buffer, size_t length); -/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ -extern void interrupt_mixer_set_failsafe(void); /** * Safety switch/LED. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 75bc7be81e..2d4c589d77 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -74,7 +74,6 @@ static const uint16_t r_page_config[] = { [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS, [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, - [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, }; /** @@ -89,17 +88,8 @@ volatile uint16_t r_page_status[] = { [PX4IO_P_STATUS_ALARMS] = 0, [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, - [PX4IO_P_STATUS_PRSSI] = 0, - [PX4IO_P_STATUS_MIXER] = 0, }; -/** - * PAGE 2 - * - * Post-mixed actuator values. - */ -uint16_t r_page_actuators[PX4IO_SERVO_COUNT]; - /** * PAGE 3 * @@ -122,16 +112,6 @@ uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; -/** - * PAGE 5 - * - * Scaled/routed RC input - */ -uint16_t r_page_rc_input[] = { - [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ...(PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 -}; - /** * Scratch page; used for registers that are constructed as-read. * @@ -155,76 +135,29 @@ uint16_t r_page_direct_pwm[PX4IO_SERVO_COUNT]; volatile uint16_t r_page_setup[] = { /* default to RSSI ADC functionality */ [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, - [PX4IO_P_SETUP_ARMING] = (PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE), + [PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_SBUS_RATE] = 72, - /* this is unused, but we will pad it for readability (the compiler pads it automatically) */ - [PX4IO_P_SETUP_RELAYS_PAD] = 0, -#ifdef ADC_VSERVO [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, -#else - [PX4IO_P_SETUP_VBATT_SCALE] = 10000, -#endif [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, [PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0, - [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, - [PX4IO_P_SETUP_PWM_REVERSE] = 0, - [PX4IO_P_SETUP_TRIM_ROLL] = 0, - [PX4IO_P_SETUP_TRIM_PITCH] = 0, - [PX4IO_P_SETUP_TRIM_YAW] = 0, - [PX4IO_P_SETUP_SCALE_ROLL] = 10000, - [PX4IO_P_SETUP_SCALE_PITCH] = 10000, - [PX4IO_P_SETUP_SCALE_YAW] = 10000, - [PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0, - [PX4IO_P_SETUP_AIRMODE] = 0, - [PX4IO_P_SETUP_THR_MDL_FAC] = 0, [PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE, [PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0 }; -#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ - PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ - PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI) +#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT | PX4IO_P_SETUP_FEATURES_ADC_RSSI) #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_FMU_PREARMED | \ - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ - PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) -#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) - -/** - * PAGE 101 - * - * Control values from the FMU. - */ -uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS]; - -/* - * PAGE 102 does not have a buffer. - */ - -/** - * PAGE 103 - * - * R/C channel input configuration. - */ -uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; - -/* valid options */ -#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) /* * PAGE 104 uses r_page_servos. @@ -239,30 +172,6 @@ uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STR */ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; -/** - * PAGE 106 - * - * minimum PWM values when armed - * - */ -uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN }; - -/** - * PAGE 107 - * - * maximum PWM values when armed - * - */ -uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX }; - -/** - * PAGE 108 - * - * trim values for center position - * - */ -int16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM }; - /** * PAGE 109 * @@ -274,29 +183,7 @@ uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { - switch (page) { - - /* handle bulk controls input */ - case PX4IO_PAGE_CONTROLS: - - /* copy channel data */ - while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { - - /* XXX range-check value? */ - r_page_controls[offset] = *values; - - offset++; - num_values--; - values++; - } - - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; - - system_state.fmu_data_received_time = hrt_absolute_time(); - - break; - /* handle raw PWM input */ case PX4IO_PAGE_DIRECT_PWM: @@ -351,92 +238,21 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num break; - case PX4IO_PAGE_CONTROL_MIN_PWM: - - /* copy channel data */ - while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - - if (*values == 0) { - /* ignore 0 */ - } else if (*values > PWM_HIGHEST_MIN) { - r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; - - } else if (*values < PWM_LOWEST_MIN) { - r_page_servo_control_min[offset] = PWM_LOWEST_MIN; - - } else { - r_page_servo_control_min[offset] = *values; - } - - offset++; - num_values--; - values++; - } - - break; - - case PX4IO_PAGE_CONTROL_MAX_PWM: - - /* copy channel data */ - while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - - if (*values == 0) { - /* ignore 0 */ - } else if (*values > PWM_HIGHEST_MAX) { - r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; - - } else if (*values < PWM_LOWEST_MAX) { - r_page_servo_control_max[offset] = PWM_LOWEST_MAX; - - } else { - r_page_servo_control_max[offset] = *values; - } - - offset++; - num_values--; - values++; - } - - break; - - case PX4IO_PAGE_CONTROL_TRIM_PWM: - - /* copy channel data */ - while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - - r_page_servo_control_trim[offset] = *values; - - offset++; - num_values--; - values++; - } - - update_trims = true; - - break; - case PX4IO_PAGE_DISARMED_PWM: { - /* flag for all outputs */ - bool all_disarmed_off = true; - /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) { /* 0 means disabling always PWM */ r_page_servo_disarmed[offset] = 0; } else if (*values < PWM_LOWEST_MIN) { r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; - all_disarmed_off = false; } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; - all_disarmed_off = false; } else { r_page_servo_disarmed[offset] = *values; - all_disarmed_off = false; } offset++; @@ -444,25 +260,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num values++; } - if (all_disarmed_off) { - /* disable PWM output if disarmed */ - r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); - - } else { - /* enable PWM output always */ - r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; - } + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } break; - /* handle text going to the mixer parser */ - case PX4IO_PAGE_MIXERLOAD: - /* do not change the mixer if FMU is armed and IO's safety is off - * this state defines an active system. This check is done in the - * text handling function. - */ - return interrupt_mixer_handle_text(values, num_values * sizeof(*values)); - default: /* avoid offset wrap */ @@ -504,20 +305,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * Allow FMU override of arming state (to allow in-air restores), * but only if the arming state is not in sync on the IO side. */ - - if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; - - } else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { r_status_flags = value; } - if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) { - /* update failsafe values, now that the mixer is set to ok */ - interrupt_mixer_set_failsafe(); - } - break; default: @@ -541,32 +333,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* disable the conflicting options with SBUS 1 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { - value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with SBUS 2 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { - value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } #endif /* disable the conflicting options with ADC RSSI */ if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { - value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); - } - - /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ - if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { - value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + value &= ~(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* apply changes */ @@ -578,18 +357,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value &= PX4IO_P_SETUP_ARMING_VALID; - /* - * Update arming state - disarm if no longer OK. - * This builds on the requirement that the FMU driver - * asks about the FMU arming state on initialization, - * so that an in-air reset of FMU can not lead to a - * lockup of the IO arming state. - */ - - if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; - } - /* * If the failsafe termination flag is set, do not allow the autopilot to unset it */ @@ -688,32 +455,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; - case PX4IO_P_SETUP_RC_THR_FAILSAFE_US: - if (value > 650 && value < 2350) { - r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value; - } - - break; - case PX4IO_P_SETUP_SBUS_RATE: r_page_setup[offset] = value; sbus1_set_output_rate_hz(value); break; - case PX4IO_P_SETUP_THR_MDL_FAC: - update_mc_thrust_param = true; - r_page_setup[offset] = value; - break; - - case PX4IO_P_SETUP_PWM_REVERSE: - case PX4IO_P_SETUP_TRIM_ROLL: - case PX4IO_P_SETUP_TRIM_PITCH: - case PX4IO_P_SETUP_TRIM_YAW: - case PX4IO_P_SETUP_SCALE_ROLL: - case PX4IO_P_SETUP_SCALE_PITCH: - case PX4IO_P_SETUP_SCALE_YAW: - case PX4IO_P_SETUP_MOTOR_SLEW_MAX: - case PX4IO_P_SETUP_AIRMODE: case PX4IO_P_SETUP_THERMAL: case PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION: r_page_setup[offset] = value; @@ -725,101 +471,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; - case PX4IO_PAGE_RC_CONFIG: { - - /** - * do not allow a RC config change while safety is off - */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { - break; - } - - unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; - unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; - uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - - if (channel >= PX4IO_RC_INPUT_CHANNELS) { - return -1; - } - - /* disable the channel until we have a chance to sanity-check it */ - conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - - switch (index) { - - case PX4IO_P_RC_CONFIG_MIN: - case PX4IO_P_RC_CONFIG_CENTER: - case PX4IO_P_RC_CONFIG_MAX: - case PX4IO_P_RC_CONFIG_DEADZONE: - case PX4IO_P_RC_CONFIG_ASSIGNMENT: - conf[index] = value; - break; - - case PX4IO_P_RC_CONFIG_OPTIONS: - value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; - - /* clear any existing RC disabled flag */ - r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); - - /* set all options except the enabled option */ - conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - - /* should the channel be enabled? */ - /* this option is normally set last */ - if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - uint8_t count = 0; - bool disabled = false; - - /* assert min..center..max ordering */ - if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { - count++; - } - - if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { - count++; - } - - if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { - count++; - } - - if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { - count++; - } - - /* assert deadzone is sane */ - if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { - count++; - } - - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { - disabled = true; - - } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && - (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { - count++; - } - - /* sanity checks pass, enable channel */ - if (count) { - isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; - - } else if (!disabled) { - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - } - } - - break; - /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ - - } - - break; - /* case PX4IO_RC_PAGE_CONFIG */ - } - case PX4IO_PAGE_TEST: switch (offset) { case PX4IO_P_TEST_LED: @@ -862,49 +513,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_ALARMS maintained externally */ -#ifdef ADC_VBATT - /* PX4IO_P_STATUS_VBATT */ - { - /* - * Coefficients here derived by measurement of the 5-16V - * range on one unit, validated on sample points of another unit - * - * Data in Tools/tests-host/data folder. - * - * measured slope = 0.004585267878277 (int: 4585) - * nominal theoretic slope: 0.00459340659 (int: 4593) - * intercept = 0.016646394188076 (int: 16646) - * nominal theoretic intercept: 0.00 (int: 0) - * - */ - unsigned counts = adc_measure(ADC_VBATT); - - if (counts != 0xffff) { - unsigned mV = (166460 + (counts * 45934)) / 10000; - unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; - - r_page_status[PX4IO_P_STATUS_VBATT] = corrected; - } - } - -#endif -#ifdef ADC_IBATT - /* PX4IO_P_STATUS_IBATT */ - { - /* - note that we have no idea what sort of - current sensor is attached, so we just - return the raw 12 bit ADC value and let the - FMU sort it out, with user selectable - configuration for their sensor - */ - unsigned counts = adc_measure(ADC_IBATT); - - if (counts != 0xffff) { - r_page_status[PX4IO_P_STATUS_IBATT] = counts; - } - } -#endif #ifdef ADC_VSERVO /* PX4IO_P_STATUS_VSERVO */ { @@ -916,6 +524,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val r_page_status[PX4IO_P_STATUS_VSERVO] = mV; } } + #endif #ifdef ADC_RSSI /* PX4IO_P_STATUS_VRSSI */ @@ -936,12 +545,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_RAW_ADC_INPUT: memset(r_page_scratch, 0, sizeof(r_page_scratch)); -#ifdef ADC_VBATT - r_page_scratch[0] = adc_measure(ADC_VBATT); -#endif -#ifdef ADC_IBATT - r_page_scratch[1] = adc_measure(ADC_IBATT); -#endif #ifdef ADC_VSERVO r_page_scratch[0] = adc_measure(ADC_VSERVO); @@ -971,10 +574,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val SELECT_PAGE(r_page_config); break; - case PX4IO_PAGE_ACTUATORS: - SELECT_PAGE(r_page_actuators); - break; - case PX4IO_PAGE_SERVOS: SELECT_PAGE(r_page_servos); break; @@ -983,23 +582,11 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val SELECT_PAGE(r_page_raw_rc_input); break; - case PX4IO_PAGE_RC_INPUT: - SELECT_PAGE(r_page_rc_input); - break; - /* readback of input pages */ case PX4IO_PAGE_SETUP: SELECT_PAGE(r_page_setup); break; - case PX4IO_PAGE_CONTROLS: - SELECT_PAGE(r_page_controls); - break; - - case PX4IO_PAGE_RC_CONFIG: - SELECT_PAGE(r_page_rc_input_config); - break; - case PX4IO_PAGE_DIRECT_PWM: SELECT_PAGE(r_page_direct_pwm); break; @@ -1008,18 +595,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val SELECT_PAGE(r_page_servo_failsafe); break; - case PX4IO_PAGE_CONTROL_MIN_PWM: - SELECT_PAGE(r_page_servo_control_min); - break; - - case PX4IO_PAGE_CONTROL_MAX_PWM: - SELECT_PAGE(r_page_servo_control_max); - break; - - case PX4IO_PAGE_CONTROL_TRIM_PWM: - SELECT_PAGE(r_page_servo_control_trim); - break; - case PX4IO_PAGE_DISARMED_PWM: SELECT_PAGE(r_page_servo_disarmed); break; diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c index 2edf738139..2ddb20f6db 100644 --- a/src/modules/rc_update/params.c +++ b/src/modules/rc_update/params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2021 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 @@ -1329,39 +1329,6 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 0); */ PARAM_DEFINE_INT32(RC_MAP_FLTMODE, 0); -/** - * Mode switch channel mapping. - * - * This is the main flight mode selector. - * The channel index (starting from 1 for channel 1) indicates - * which channel should be used for deciding about the main mode. - * A value of zero indicates the switch is not assigned. - * - * @min 0 - * @max 18 - * @group Radio Switches - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - */ -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); - /** * Return switch channel * @@ -1390,34 +1357,6 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); -/** - * Position Control switch channel - * - * @min 0 - * @max 18 - * @group Radio Switches - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - */ -PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); - /** * Loiter switch channel * @@ -1446,34 +1385,6 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); -/** - * Acro switch channel - * - * @min 0 - * @max 18 - * @group Radio Switches - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - */ -PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); - /** * Offboard switch channel * @@ -1645,62 +1556,6 @@ PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0); -/** - * Stabilize switch channel mapping. - * - * @min 0 - * @max 18 - * @group Radio Switches - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - */ -PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0); - -/** - * Manual switch channel mapping. - * - * @min 0 - * @max 18 - * @group Radio Switches - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - */ -PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); - /** * Button flight mode selection * @@ -1732,7 +1587,6 @@ PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); * @bit 16 Mask Channel 17 as a mode button * @bit 17 Mask Channel 18 as a mode button */ - PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0); /** @@ -2019,54 +1873,6 @@ PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0); */ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); -/** - * Threshold for selecting assist mode - * - * 0-1 indicate where in the full channel range the threshold sits - * 0 : min - * 1 : max - * sign indicates polarity of comparison - * positive : true when channel>th - * negative : true when channelth - * negative : true when channelth - * negative : true when channelth - * negative : true when channelth - * negative : true when channelth - * negative : true when channel= 0) { - const bool on_inv = (on_th < 0.f); - const bool mid_inv = (mid_th < 0.f); - - const float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; - - if (on_inv ? value < on_th : value > on_th) { - return manual_control_switches_s::SWITCH_POS_ON; - - } else if (mid_inv ? value < mid_th : value > mid_th) { - return manual_control_switches_s::SWITCH_POS_MIDDLE; - - } else { - return manual_control_switches_s::SWITCH_POS_OFF; - } - } - - return manual_control_switches_s::SWITCH_POS_NONE; -} - switch_pos_t RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th) const { if (_rc.function[func] >= 0) { @@ -569,7 +572,7 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample) manual_control_switches_s switches{}; switches.timestamp_sample = timestamp_sample; - // check mode slot (RC_MAP_FLTMODE) or legacy mode switch (RC_MAP_MODE_SW), but not both + // check mode slot (RC_MAP_FLTMODE) if (_param_rc_map_fltmode.get() > 0) { // number of valid slots static constexpr int num_slots = manual_control_switches_s::MODE_SLOT_NUM; @@ -594,16 +597,6 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample) switches.mode_slot = num_slots; } - } else if (_param_rc_map_mode_sw.get() > 0) { - switches.mode_switch = get_rc_sw3pos_position(rc_channels_s::FUNCTION_MODE, - _param_rc_auto_th.get(), _param_rc_assist_th.get()); - - // only used with legacy mode switch - switches.man_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_MAN, _param_rc_man_th.get()); - switches.acro_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ACRO, _param_rc_acro_th.get()); - switches.stab_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_STAB, _param_rc_stab_th.get()); - switches.posctl_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_POSCTL, _param_rc_posctl_th.get()); - } else if (_param_rc_map_flightmode_buttons.get() > 0) { switches.mode_slot = manual_control_switches_s::MODE_SLOT_NONE; bool is_consistent_button_press = false; @@ -673,17 +666,17 @@ void RCUpdate::UpdateManualSetpoint(const hrt_abstime ×tamp_sample) manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_RC; // limit controls - manual_control_setpoint.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f); - manual_control_setpoint.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f); - manual_control_setpoint.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f); + manual_control_setpoint.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f); + manual_control_setpoint.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f); + manual_control_setpoint.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f); manual_control_setpoint.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f); - manual_control_setpoint.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f); - manual_control_setpoint.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f); - manual_control_setpoint.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f); - manual_control_setpoint.aux3 = get_rc_value(rc_channels_s::FUNCTION_AUX_3, -1.f, 1.f); - manual_control_setpoint.aux4 = get_rc_value(rc_channels_s::FUNCTION_AUX_4, -1.f, 1.f); - manual_control_setpoint.aux5 = get_rc_value(rc_channels_s::FUNCTION_AUX_5, -1.f, 1.f); - manual_control_setpoint.aux6 = get_rc_value(rc_channels_s::FUNCTION_AUX_6, -1.f, 1.f); + manual_control_setpoint.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f); + manual_control_setpoint.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f); + manual_control_setpoint.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f); + manual_control_setpoint.aux3 = get_rc_value(rc_channels_s::FUNCTION_AUX_3, -1.f, 1.f); + manual_control_setpoint.aux4 = get_rc_value(rc_channels_s::FUNCTION_AUX_4, -1.f, 1.f); + manual_control_setpoint.aux5 = get_rc_value(rc_channels_s::FUNCTION_AUX_5, -1.f, 1.f); + manual_control_setpoint.aux6 = get_rc_value(rc_channels_s::FUNCTION_AUX_6, -1.f, 1.f); // publish manual_control_setpoint topic manual_control_setpoint.timestamp = hrt_absolute_time(); diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index ef6de3a813..a21230036f 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2021 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 @@ -120,7 +120,6 @@ private: /** * Get switch position for specified function. */ - switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, float mid_th) const; switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th) const; /** @@ -208,21 +207,16 @@ private: (ParamInt) _param_rc_map_failsafe, (ParamInt) _param_rc_map_fltmode, - (ParamInt) _param_rc_map_mode_sw, (ParamInt) _param_rc_map_flaps, (ParamInt) _param_rc_map_return_sw, - (ParamInt) _param_rc_map_posctl_sw, (ParamInt) _param_rc_map_loiter_sw, - (ParamInt) _param_rc_map_acro_sw, (ParamInt) _param_rc_map_offb_sw, (ParamInt) _param_rc_map_kill_sw, (ParamInt) _param_rc_map_arm_sw, (ParamInt) _param_rc_map_trans_sw, (ParamInt) _param_rc_map_gear_sw, - (ParamInt) _param_rc_map_stab_sw, - (ParamInt) _param_rc_map_man_sw, (ParamInt) _param_rc_map_flightmode_buttons, (ParamInt) _param_rc_map_aux1, @@ -234,18 +228,12 @@ private: (ParamInt) _param_rc_fails_thr, - (ParamFloat) _param_rc_assist_th, - (ParamFloat) _param_rc_auto_th, - (ParamFloat) _param_rc_posctl_th, (ParamFloat) _param_rc_loiter_th, - (ParamFloat) _param_rc_acro_th, (ParamFloat) _param_rc_offb_th, (ParamFloat) _param_rc_killswitch_th, (ParamFloat) _param_rc_armswitch_th, (ParamFloat) _param_rc_trans_th, (ParamFloat) _param_rc_gear_th, - (ParamFloat) _param_rc_stab_th, - (ParamFloat) _param_rc_man_th, (ParamFloat) _param_rc_return_th, (ParamInt) _param_rc_chan_cnt diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 5e90ad7bab..56176d1a8f 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -43,7 +43,6 @@ #pragma once #include -#include #include #include #include @@ -60,6 +59,7 @@ #include #include #include +#include #include #include #include diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 8ed275a691..ddffe6ac5b 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -36,7 +36,6 @@ set(srcs test_bezierQuad.cpp test_bitset.cpp test_bson.cpp - test_conv.cpp test_dataman.c test_file.c test_file2.c diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp deleted file mode 100644 index 5f26cd1cea..0000000000 --- a/src/systemcmds/tests/test_conv.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file test_conv.cpp - * Tests conversions used across the system. - */ - -#include - -#include -#include -#include -#include -#include -#include - -#include "tests_main.h" - -#include -#include - -#include -#include - -int test_conv(int argc, char *argv[]) -{ - //PX4_INFO("Testing system conversions"); - - for (int i = -10000; i <= 10000; i += 1) { - float f = i / 10000.0f; - float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); - - if (fabsf(f - fres) > 0.0001f) { - PX4_ERR("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), - (double)fres); - return 1; - } - } - - //PX4_INFO("All conversions clean"); - - return 0; -} diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 8aded51a9e..dc800776d8 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -48,7 +48,7 @@ #include #include #include -#include + #include #include @@ -277,7 +277,7 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade /* reset, load in chunks */ mixer_group.reset(); - char mixer_text[PX4IO_MAX_MIXER_LENGTH]; /* large enough for one mixer */ + char mixer_text[330]; /* large enough for one mixer */ unsigned mixer_text_length = 0; unsigned transmitted = 0; @@ -290,7 +290,7 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { PX4_ERR("Mixer text length overflow for file: %s. Is PX4IO_MAX_MIXER_LENGTH too small? (curr len: %d)", filename, - PX4IO_MAX_MIXER_LENGTH); + 330); return false; } diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index 0aeff563e7..af9879b71a 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -48,7 +48,7 @@ #include #include -#include +#include #include #include diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 7b0692394a..83fe6aa703 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -48,9 +48,9 @@ #include #include -#include #include #include +#include #include "tests_main.h" diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 21fcba26a9..4d5655229c 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -81,7 +81,6 @@ const struct { {"bezier", test_bezierQuad, 0}, {"bitset", test_bitset, 0}, {"bson", test_bson, 0}, - {"conv", test_conv, 0}, {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, {"file2", test_file2, OPT_NOJIGTEST}, {"float", test_float, 0}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 5636afc349..3b28c11b53 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -47,7 +47,6 @@ extern int test_atomic_bitset(int argc, char *argv[]); extern int test_bezierQuad(int argc, char *argv[]); extern int test_bitset(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); -extern int test_conv(int argc, char *argv[]); extern int test_dataman(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_file2(int argc, char *argv[]);