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/4030_3dr_solo b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo index 55af8d5485..1826aaaa2f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo +++ b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo @@ -49,7 +49,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 be788effa1..b7f6d52b23 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -93,19 +93,15 @@ then fi fi - if [ $OUTPUT_MODE = io ] - then - . ${R}etc/init.d/rc.io - fi - # # Start IO for RC input if needed. # - if [ $IO_PRESENT = yes ] + if [ $IO_PRESENT = yes -a $OUTPUT_MODE = io ] then - if [ $OUTPUT_MODE != io ] + if ! px4io start then - . ${R}etc/init.d/rc.io + echo "PX4IO start failed" + tune_control play -t 18 # PROG_PX4IO_ERR fi fi fi 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/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index b6e7b06aaf..bf5b843b53 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 fb68c1d44d..d140743761 100644 --- a/msg/manual_control_switches.msg +++ b/msg/manual_control_switches.msg @@ -26,12 +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 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE -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 18d8d97f58..110401167d 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,37 +1,31 @@ uint64 timestamp # time since system start (microseconds) -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_RATTITUDE=19 -uint8 FUNCTION_KILLSWITCH=20 -uint8 FUNCTION_TRANSITION=21 -uint8 FUNCTION_GEAR=22 -uint8 FUNCTION_ARMSWITCH=23 -uint8 FUNCTION_STAB=24 -uint8 FUNCTION_AUX_6=25 -uint8 FUNCTION_MAN=26 +uint8 FUNCTION_THROTTLE = 0 +uint8 FUNCTION_ROLL = 1 +uint8 FUNCTION_PITCH = 2 +uint8 FUNCTION_YAW = 3 +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 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[27] function # Functions mapping +int8[21] 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 08b8c43da8..34d273c2dd 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -1,8 +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_control_manual_enabled # true if manual input is mixed in bool flag_control_auto_enabled # true if onboard autopilot should act bool flag_control_offboard_enabled # true if offboard control should be used 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 b79a5a1e2e..cc9b55dd25 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 @@ -66,7 +66,7 @@ static constexpr wq_config_t I2C3{"wq:I2C3", 2336, -11}; static constexpr wq_config_t I2C4{"wq:I2C4", 2336, -12}; // PX4 att/pos controllers, highest priority after sensors. -static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1760, -13}; +static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1792, -13}; static constexpr wq_config_t INS0{"wq:INS0", 6000, -14}; static constexpr wq_config_t INS1{"wq:INS1", 6000, -15}; @@ -77,16 +77,16 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; static constexpr wq_config_t uavcan{"wq:uavcan", 2576, -19}; -static constexpr wq_config_t UART0{"wq:UART0", 1400, -21}; -static constexpr wq_config_t UART1{"wq:UART1", 1400, -22}; -static constexpr wq_config_t UART2{"wq:UART2", 1400, -23}; -static constexpr wq_config_t UART3{"wq:UART3", 1400, -24}; -static constexpr wq_config_t UART4{"wq:UART4", 1400, -25}; -static constexpr wq_config_t UART5{"wq:UART5", 1400, -26}; -static constexpr wq_config_t UART6{"wq:UART6", 1400, -27}; -static constexpr wq_config_t UART7{"wq:UART7", 1400, -28}; -static constexpr wq_config_t UART8{"wq:UART8", 1400, -29}; -static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -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", 1700, -50}; diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index 4a99595634..a4befe7043 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -10,7 +10,6 @@ set(tests bson commander controllib - conv dataman file2 float diff --git a/platforms/qurt/src/px4/common/commands_hil.c b/platforms/qurt/src/px4/common/commands_hil.c index 6519fcce32..7eb3948a34 100644 --- a/platforms/qurt/src/px4/common/commands_hil.c +++ b/platforms/qurt/src/px4/common/commands_hil.c @@ -77,8 +77,6 @@ const char *get_commands() "param set RC6_MIN 992\n" "param set RC6_TRIM 1504\n" "param set RC_CHAN_CNT 8\n" - "param set RC_MAP_MODE_SW 5\n" - "param set RC_MAP_POSCTL_SW 7\n" "param set RC_MAP_RETURN_SW 8\n" "param set MC_YAW_P 1.5\n" "param set MC_PITCH_P 3.0\n" diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 821390e435..55f7a7118e 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. These correspond to enum Mode in px4fmu/fmu.cpp */ #define PWM_SERVO_MODE_NONE 0 #define PWM_SERVO_MODE_1PWM 1 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 df9dbdde43..7cd7518e3a 100644 --- a/src/drivers/px4io/CMakeLists.txt +++ b/src/drivers/px4io/CMakeLists.txt @@ -35,6 +35,10 @@ px4_add_module( MAIN px4io COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable + -Wno-error + #-DDEBUG_BUILD + STACK_MAIN + 10000 SRCS px4io.cpp px4io_serial.cpp @@ -42,7 +46,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 1a89fa43fc..d241385d2d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2020 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 @@ -37,64 +37,44 @@ * * 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 #include @@ -107,12 +87,11 @@ #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 UPDATE_INTERVAL_MIN{2}; // 2 ms -> 500 Hz +static constexpr unsigned UPDATE_INTERVAL_MAX{100}; // 100 ms -> 10 Hz using namespace time_literals; @@ -121,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 OutputModuleInterface { public: /** @@ -129,31 +108,22 @@ 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. @@ -173,11 +143,6 @@ public: */ virtual int ioctl(file *filp, int cmd, unsigned long arg); - /** - * Disable RC input handling - */ - int disable_rc_handling(); - /** * Print IO status. * @@ -185,7 +150,7 @@ public: * * @param extended_status Shows more verbose information (in particular RC config) */ - void print_status(bool extended_status); + void print_status(); /** * Fetch and print debug console output. @@ -197,35 +162,44 @@ 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;} + inline 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: + void Run() override; + + void updateDisarmed(); + void updateFailsafe(); + + static constexpr int PX4IO_MAX_ACTUATORS = 8; + device::Device *_interface; - 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 + 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 - 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 + uint64_t _rc_last_valid{0}; ///< last valid timestamp - volatile int _task; ///< worker task id - volatile bool _task_should_exit; ///< worker terminate flag + int _class_instance{-1}; - orb_advert_t _mavlink_log_pub; ///< mavlink log pub + volatile int _task{-1}; ///< worker task id + volatile bool _task_should_exit{false}; ///< worker terminate flag - 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) + 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 @@ -234,75 +208,35 @@ 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}; ///< allow to 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) + bool _cb_flighttermination{true}; ///< true if the flight termination circuit breaker is enabled - 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 + 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 - bool _test_fmu_fail; ///< To test what happens if IO loses FMU + bool _test_fmu_fail{false}; ///< 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 - - /** - * 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(); + MixingOutput _mixing_output{PX4IO_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true}; + uint16_t _prev_outputs[MAX_ACTUATORS] {}; + hrt_abstime _last_full_output_update{0}; /** * Update IO's arming-related state @@ -321,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) * @@ -427,60 +346,38 @@ 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_map_kill_sw, + (ParamFloat) _param_rc_killswitch_th, + (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 + ) }; namespace { -PX4IO *g_dev = nullptr; +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); + + /* Fetch initial flight termination circuit breaker state */ + _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); } PX4IO::~PX4IO() @@ -494,19 +391,18 @@ PX4IO::~PX4IO() px4_usleep(100000); } - /* well, kill it anyway, though this will probably crash */ - if (_task != -1) { - task_delete(_task); - } + delete _interface; - if (_interface != nullptr) { - delete _interface; + /* clean up the alternate device node */ + if (_param_sys_hitl.get() == 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); + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_interface_read_perf); + perf_free(_interface_write_perf); g_dev = nullptr; } @@ -514,12 +410,10 @@ PX4IO::~PX4IO() int PX4IO::detect() { - int ret; - if (_task == -1) { /* do regular cdev init */ - ret = CDev::init(); + int ret = CDev::init(); if (ret != OK) { return ret; @@ -546,37 +440,36 @@ 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; + const bool full_update = (hrt_elapsed_time(&_last_full_output_update) >= 200_ms); - 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); + /* output to the servos */ + for (size_t i = 0; i < num_outputs; i++) { + // TODO: only update if changed + if (_prev_outputs[i] != outputs[i] || full_update) { + io_reg_set(PX4IO_PAGE_DIRECT_PWM, i, outputs[i]); + _prev_outputs[i] = outputs[i]; } } + if (full_update) { + _last_full_output_update = hrt_absolute_time(); + } + + 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; } @@ -603,12 +496,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) || (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { @@ -633,18 +524,13 @@ 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)); @@ -653,318 +539,113 @@ 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)) { + /* 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); - /* get a status update from IO */ - io_get_status(); - - mavlink_log_emergency(&_mavlink_log_pub, "RECOVERING FROM FMU 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 5s */ - if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) { - mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (1), abort"); - 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"); - /* 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 5s */ - if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) { - mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (3), abort"); - 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 5s */ - if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) { - mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (2), abort"); - 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"); - 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); - } + /* publish RC config to IO */ + ret = io_set_rc_config(); + if (ret != OK) { + mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail"); + 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) { + _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(2500); } - /* start the IO interface task */ - _task = px4_task_spawn_cmd("px4io", - SCHED_DEFAULT, - SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 1500, - (px4_main_t)&PX4IO::task_main_trampoline, - nullptr); + updateDisarmed(); + updateFailsafe(); - if (_task < 0) { - PX4_ERR("task start failed: %d", errno); - return -errno; - } + update_params(); + + _task = 2; // TODO + + 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{}; -void -PX4IO::task_main() -{ - hrt_abstime poll_last = 0; - hrt_abstime orb_check_last = 0; - - /* - * 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.disarmedValue(i); } - /* Fetch initial flight termination circuit breaker state */ - _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm.values, _max_actuators); +} - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _t_actuator_controls_0; - fds[0].events = POLLIN; +void PX4IO::updateFailsafe() +{ + pwm_output_values pwm{}; - _param_update_force = true; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm.values[i] = _mixing_output.failsafeValue(i); + } - /* lock against the ioctl handler */ - lock(); + io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, _max_actuators); +} - /* loop talking to IO */ - while (!_task_should_exit) { +void PX4IO::Run() +{ + if (_task_should_exit) { + ScheduleClear(); + _mixing_output.unregister(); - /* sleep waiting for topic updates, but no more than 20ms */ - unlock(); - int ret = ::poll(fds, 1, 20); - lock(); + //exit_and_cleanup(); + _task = -1; + return; + } - /* this would be bad... */ - if (ret < 0) { - warnx("poll error %d", errno); - continue; - } + perf_begin(_cycle_perf); + perf_count(_interval_perf); - perf_begin(_perf_update); - hrt_abstime now = hrt_absolute_time(); + if (!_task_should_exit) { + // schedule minimal update rate if there are no actuator controls + ScheduleDelayed(20_ms); /* 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 (_param_sys_hitl.get() == 0) { + _mixing_output.update(); } - if (!_armed && !_lockdown_override) { - handle_motor_test(); + SmartLock lock_guard(_lock); - } else { - _motor_test.in_test_mode = false; - } - - if (now >= poll_last + IO_POLL_INTERVAL) { - /* run at 50-250Hz */ - poll_last = now; + 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(); + } - /* fetch PWM outputs from IO */ - io_publish_pwm_outputs(); - + if (_param_sys_hitl.get() == 0) { /* 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) { + if (_t_actuator_armed.updated()) { io_set_arming_state(); + + // TODO: throttle + updateDisarmed(); + updateFailsafe(); } } - if (!_armed && (now >= orb_check_last + ORB_CHECK_INTERVAL)) { - /* run at 5Hz */ - orb_check_last = now; - + if (!_mixing_output.armed().armed) { /* vehicle command */ if (_t_vehicle_command.updated()) { vehicle_command_s cmd{}; @@ -1003,8 +684,6 @@ PX4IO::task_main() /* * If parameters have changed, re-send RC mappings to IO - * - * XXX this may be a bit spammy */ // check for parameter updates @@ -1015,187 +694,67 @@ PX4IO::task_main() _param_update_force = false; - if (!_rc_handling_disabled) { - /* re-upload RC input config as it may have changed */ - io_set_rc_config(); - } + ModuleParams::updateParams(); - /* send RC throttle failsafe value to IO */ - int32_t failsafe_param_val; - param_t failsafe_param = param_find("RC_FAILS_THR"); + update_params(); - 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", (int)failsafe_thr); - } - } - } + /* re-upload RC input config as it may have changed */ + io_set_rc_config(); /* 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); + 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); + 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); + if (_param_sens_en_themal.get() != _thermal_control || _param_update_force) { - param_t thermal_param = param_find("SENS_EN_THERMAL"); + _thermal_control = _param_sens_en_themal.get(); + /* set power management state for thermal */ + uint16_t tctrl; - if (thermal_param != PARAM_INVALID) { + if (_thermal_control < 0) { + tctrl = PX4IO_THERMAL_IGNORE; - 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); + } else { + tctrl = PX4IO_THERMAL_OFF; } - } - 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)); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, tctrl); } /* S.BUS output */ - int sbus_mode; - parm_handle = param_find("PWM_SBUS_MODE"); + 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); - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, &sbus_mode); + } 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); - 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); - } + } 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); } - - /* 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); + // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) + _mixing_output.updateSubscriptions(false, true); } - unlock(); - -out: - PX4_DEBUG("exiting"); - - /* clean up the alternate device node */ - if (_primary_pwm_device) { - unregister_driver(PWM_OUTPUT0_DEVICE_PATH); - } - - /* tell the dtor that we are exiting */ - _task = -1; - _exit(0); + perf_end(_cycle_perf); } void PX4IO::update_params() { // skip update when armed - if (_armed) { + if (_mixing_output.armed().armed) { return; } @@ -1213,59 +772,104 @@ void PX4IO::update_params() char str[17]; - - // PWM_MAIN_MINx - { - pwm_output_values pwm{}; - pwm.channel_count = _max_actuators; - - for (unsigned i = 0; i < _max_actuators; i++) { + for (unsigned i = 0; i < _max_actuators; i++) { + // PWM_MAIN_MINx + { 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, PWM_LOWEST_MIN, PWM_HIGHEST_MIN); + _mixing_output.minValue(i) = math::constrain(pwm_min, PWM_LOWEST_MIN, 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 { - pwm.values[i] = pwm_min_default; + _mixing_output.minValue(i) = pwm_min_default; } } } - io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm.values, pwm.channel_count); - } - - // PWM_MAIN_MAXx - { - pwm_output_values pwm{}; - pwm.channel_count = _max_actuators; - - for (unsigned i = 0; i < _max_actuators; i++) { + // PWM_MAIN_MAXx + { 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, PWM_LOWEST_MAX, PWM_HIGHEST_MAX); + _mixing_output.maxValue(i) = math::constrain(pwm_max, PWM_LOWEST_MAX, 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 { - pwm.values[i] = pwm_max_default; + _mixing_output.maxValue(i) = pwm_max_default; } } } - io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm.values, pwm.channel_count); + // PWM_MAIN_FAILx + { + sprintf(str, "%s_FAIL%u", prefix, i + 1); + int32_t pwm_failsafe = -1; + + if (param_get(param_find(str), &pwm_failsafe) == PX4_OK) { + if (pwm_failsafe >= 0) { + _mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, 0, PWM_HIGHEST_MAX); + + if (pwm_failsafe != _mixing_output.failsafeValue(i)) { + int32_t pwm_fail_new = _mixing_output.failsafeValue(i); + param_set(param_find(str), &pwm_fail_new); + } + } + } + } + + // PWM_MAIN_DISx + { + 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) { + _mixing_output.disarmedValue(i) = math::constrain(pwm_dis, 0, PWM_HIGHEST_MAX); + + 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 { + _mixing_output.disarmedValue(i) = pwm_disarmed_default; + } + } + + // if (_mixing_output.disarmedValue(i) > 0) { + // num_disarmed_set++; + // } + } + + // PWM_MAIN_REVx + { + sprintf(str, "%s_REV%u", prefix, i + 1); + int32_t pwm_rev = 0; + + if (param_get(param_find(str), &pwm_rev) == PX4_OK) { + uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); + + if (pwm_rev >= 1) { + reverse_pwm_mask = reverse_pwm_mask | (1 << i); + + } else { + reverse_pwm_mask = reverse_pwm_mask & ~(1 << i); + } + } + } } // PWM_MAIN_FAILx @@ -1318,128 +922,9 @@ void PX4IO::update_params() io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm.values, pwm.channel_count); } - - // PWM_MAIN_REVx - { - int16_t reverse_pwm_mask = 0; - - for (unsigned i = 0; i < _max_actuators; i++) { - sprintf(str, "%s_REV%u", prefix, i + 1); - int32_t pwm_rev = -1; - - if (param_get(param_find(str), &pwm_rev) == PX4_OK) { - if (pwm_rev >= 1) { - reverse_pwm_mask |= (1 << i); - } - - } - } - - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, reverse_pwm_mask); - } - - // PWM_MAIN_TRIMx - { - uint16_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); - } - } - - // copy the trim values to the mixer offsets - io_reg_set(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, 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)}; @@ -1452,74 +937,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] + - (uint16_t)((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() { @@ -1529,17 +946,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; @@ -1579,19 +992,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; @@ -1601,144 +1001,41 @@ 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() +int PX4IO::io_set_rc_config() { unsigned offset = 0; int input_map[_max_rc_input]; - int32_t ichan; + int32_t ichan = 0; 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'. - */ + // KILL SWITCH + // find RCx_MIN, RCx_TRIM, RCx_MAX, RCx_REV, RCx_DZ - /* ROLL */ - param_get(param_find("RC_MAP_ROLL"), &ichan); + if (_param_rc_map_kill_sw.get() != 0) { + // kill switch enabled - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 0; + // find RCx_REV + } else { + // kill switch disabled } - /* 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 channel set, then find if it's reversed 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; + input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_KILLSWITCH; } - /* - * Iterate all possible RC inputs. - */ + // 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; @@ -1774,8 +1071,7 @@ PX4IO::io_set_rc_config() return ret; } -int -PX4IO::io_handle_status(uint16_t status) +int PX4IO::io_handle_status(uint16_t status) { int ret = 1; /** @@ -1811,18 +1107,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); @@ -1831,8 +1122,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 && @@ -1889,8 +1179,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, @@ -1939,50 +1228,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); @@ -2003,11 +1294,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; @@ -2020,7 +1312,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; @@ -2030,15 +1322,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]; @@ -2067,7 +1357,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; } @@ -2095,48 +1385,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) { @@ -2145,59 +1418,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))) { @@ -2205,7 +1431,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(%hhu,%hhu,%u): error %d", page, offset, num_values, ret); @@ -2215,14 +1443,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))) { @@ -2230,7 +1456,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(%hhu,%hhu,%u): data error %d", page, offset, num_values, ret); @@ -2240,8 +1468,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; @@ -2252,13 +1479,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; @@ -2314,124 +1538,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"); - /* 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) +void PX4IO::print_status() { /* basic configuration */ printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", @@ -2441,12 +1548,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("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", + + printf("%u controls %u actuators %u R/C inputs %u 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)}; @@ -2457,23 +1564,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("%hu raw R/C inputs", raw_inputs); @@ -2482,36 +1573,6 @@ PX4IO::print_status(bool extended_status) printf(" %u", 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%04hx%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%04hx", mapped_inputs); - - for (unsigned i = 0; i < _max_rc_input; i++) { - if (mapped_inputs & (1 << i)) { - printf(" %u:%hd", 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"); @@ -2524,57 +1585,26 @@ 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%04hx%s%s%s%s\n", features, + printf("features 0x%04hx%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%04x default %u alt %u sbus %u\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 %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE)); + printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - for (unsigned group = 0; group < 4; group++) { - printf("controls %u:", group); - - for (unsigned i = 0; i < _max_controls; i++) { - printf(" %hd", (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 %u center %u max %u deadzone %u assigned %u options 0x%04hx%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(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - } - - printf("\ndisarmed values"); - - for (unsigned i = 0; i < _max_actuators; i++) { - printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); + // 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 assigned %u options 0x%04hx%s%s\n", + i, + 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" : "")); } /* IMU heater (Pixhawk 2.1) */ @@ -2589,15 +1619,12 @@ PX4IO::print_status(bool extended_status) } } - if (_hitl_mode) { - printf("\nHITL Mode"); - } - printf("\n"); + + _mixing_output.printStatus(); } -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; @@ -2605,41 +1632,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); @@ -2660,11 +1695,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) @@ -2673,12 +1709,17 @@ 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); + } + } + 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; @@ -2692,122 +1733,138 @@ 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); + } + } + 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) { @@ -2822,6 +1879,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) { @@ -2835,22 +1893,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); @@ -2868,7 +1910,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 /*&& _motor_test.in_test_mode*/) { /* send a direct PWM value */ ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); @@ -2928,22 +1970,29 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } } - _motor_test.in_test_mode = (arg == PWM_SERVO_ENTER_TEST_MODE); + //_motor_test.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; @@ -2976,6 +2025,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); @@ -2992,41 +2043,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); @@ -3069,14 +2087,9 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace { -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; @@ -3094,8 +2107,7 @@ got: return interface; } -void -start(int argc, char *argv[]) +void start(int argc, char *argv[]) { if (g_dev != nullptr) { errx(0, "already loaded"); @@ -3105,30 +2117,14 @@ start(int argc, char *argv[]) device::Device *interface = get_interface(); /* create the driver - it will set g_dev */ - (void)new PX4IO(interface); + PX4IO *dev = new PX4IO(interface); if (g_dev == nullptr) { delete interface; errx(1, "driver allocation failed"); } - bool rc_handling_disabled = false; - bool hitl_mode = false; - - /* 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 (OK != g_dev->init(rc_handling_disabled, hitl_mode)) { + if (OK != g_dev->init()) { delete g_dev; g_dev = nullptr; errx(1, "driver init failed"); @@ -3137,8 +2133,7 @@ start(int argc, char *argv[]) exit(0); } -void -detect(int argc, char *argv[]) +void detect(int argc, char *argv[]) { if (g_dev != nullptr) { errx(0, "already loaded"); @@ -3168,8 +2163,7 @@ detect(int argc, char *argv[]) } } -void -checkcrc(int argc, char *argv[]) +void checkcrc(int argc, char *argv[]) { bool keep_running = false; @@ -3201,7 +2195,7 @@ checkcrc(int argc, char *argv[]) int fd = open(argv[1], O_RDONLY); if (fd == -1) { - PX4_ERR("open of %s failed: %d", argv[1], errno); + PX4_WARN("open of %s failed: %d", argv[1], errno); exit(1); } @@ -3245,8 +2239,7 @@ checkcrc(int argc, char *argv[]) exit(0); } -void -bind(int argc, char *argv[]) +void bind(int argc, char *argv[]) { int pulses; @@ -3276,14 +2269,16 @@ bind(int argc, char *argv[]) pulses = atoi(argv[3]); } + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { + errx(1, "system must not be armed"); + } + g_dev->ioctl(nullptr, DSM_BIND_START, pulses); exit(0); - } -void -monitor(void) +void monitor() { /* clear screen */ printf("\033[2J"); @@ -3314,7 +2309,7 @@ monitor(void) 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_status(); (void)g_dev->print_debug(); printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); @@ -3324,8 +2319,7 @@ monitor(void) } } -void -lockdown(int argc, char *argv[]) +void lockdown(int argc, char *argv[]) { if (g_dev != nullptr) { @@ -3385,8 +2379,7 @@ lockdown(int argc, char *argv[]) } /* namespace */ -int -px4io_main(int argc, char *argv[]) +int px4io_main(int argc, char *argv[]) { /* check for sufficient number of arguments */ if (argc < 2) { @@ -3518,17 +2511,6 @@ px4io_main(int argc, char *argv[]) exit(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 */ @@ -3537,11 +2519,10 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "status")) { warnx("loaded"); - g_dev->print_status(true); + g_dev->print_status(); exit(0); } @@ -3618,32 +2599,6 @@ px4io_main(int argc, char *argv[]) exit(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(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(argv[1], "test_fmu_fail")) { if (g_dev != nullptr) { g_dev->test_fmu_fail(true); @@ -3668,7 +2623,7 @@ px4io_main(int argc, char *argv[]) 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" + "bind', 'checkcrc', 'safety_on', 'safety_off',\n" + "'update', 'update', 'sbus1_out', 'sbus2_out',\n" "'test_fmu_fail', 'test_fmu_ok'"); } diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index 542be13b88..70f599b84e 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -47,9 +47,10 @@ * * 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. * - * @boolean - * @min 0 - * @max 1 + * @value 0 IO disabled + * @value 1 IO default (RC & PWM output) + * @value 2 RC only + * @value 3 PWM output only * @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 8fcabd9f8a..f83e16caed 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -480,7 +480,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)); } @@ -529,7 +529,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 f965aa764b..6e3e9b155f 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/spektrum_rc/spektrum_rc.cpp b/src/drivers/spektrum_rc/spektrum_rc.cpp index 3a9debab3f..228b422500 100644 --- a/src/drivers/spektrum_rc/spektrum_rc.cpp +++ b/src/drivers/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 e451b22b31..f4487b6baf 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -32,3 +32,4 @@ ############################################################################ px4_add_library(mixer_module mixer_module.cpp) +target_link_libraries(mixer_module PRIVATE output_limit) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index e1eff51dc3..cd7846a54f 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/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 38797b8545..80a9e9285d 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2300,7 +2300,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; @@ -2873,29 +2873,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)) { @@ -2912,15 +2890,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.rattitude_switch != _manual_control_switches.rattitude_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 @@ -3137,147 +3108,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 if (_manual_control_switches.rattitude_switch == manual_control_switches_s::SWITCH_POS_ON) { - /* Similar to acro transitions for multirotors. FW aircraft don't need a - * rattitude mode.*/ - if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_RATTITUDE, _status_flags, &_internal_state); - - } else { - 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 { - /* 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.rattitude_switch == manual_control_switches_s::SWITCH_POS_ON) { - res = main_state_transition(_status, commander_state_s::MAIN_STATE_RATTITUDE, _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 = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - print_reject_mode("POSITION CONTROL"); - } - - // fallback to ALTCTL - res = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this mode - } - - if (_manual_control_switches.posctl_switch != manual_control_switches_s::SWITCH_POS_ON) { - print_reject_mode("ALTITUDE CONTROL"); - } - - // fallback to MANUAL - res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state); - // TRANSITION_DENIED is not possible here - break; - - case manual_control_switches_s::SWITCH_POS_ON: // AUTO - res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - print_reject_mode("AUTO MISSION"); - - // fallback to LOITER if home position not set - res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - // fallback to POSCTL - res = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - // fallback to ALTCTL - res = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, &_internal_state); - - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } - - // fallback to MANUAL - res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state); - // TRANSITION_DENIED is not possible here - break; - - default: - break; - } - } return res; @@ -3359,9 +3189,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; @@ -3401,10 +3228,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 4574f1c741..90a9caeece 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 6c8b00ed1f..aba486c18b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include @@ -2015,7 +2014,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; @@ -2087,7 +2086,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 3b76556342..3afa8ddb45 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -50,9 +50,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 9e668de4c2..a6850dc1ee 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)); } @@ -210,10 +205,6 @@ controls_init(void) 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; } @@ -247,11 +238,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; } } } @@ -277,11 +268,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); @@ -298,7 +289,6 @@ controls_tick() if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { _rssi = sbus_rssi; } - } } @@ -390,91 +380,27 @@ controls_tick() 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; - } + int16_t scaled = 10000.f * r_raw_rc_values[i]; /* invert channel if requested */ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) { scaled = -scaled; } + // TODO: find kill switch + /* 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) { + if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_KILLSWITCH) { /* pick out override channel, indicated by special mapping */ - rc_value_override = SIGNED_TO_REG(scaled); + //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; + // kill engaged + // r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + // kill disengaged + // r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } } @@ -489,11 +415,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; } /* @@ -517,85 +438,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 43e35efded..03a1fb3938 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" @@ -85,40 +79,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_mix_threadsafe(float *outputs, volatile uint16_t *limits); - -static MixerGroup mixer_group; - -int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits) -{ - /* poor mans mutex */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) == 0) { - return 0; - } - - in_mixer = true; - int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); - *limits = mixer_group.get_saturation_status(); - in_mixer = false; - - return mixcount; -} - void mixer_tick() { - /* check if the mixer got modified */ - mixer_handle_text_create_mixer(); - /* 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; @@ -132,8 +98,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); @@ -148,7 +113,7 @@ mixer_tick() } /* default to disarmed mixing */ - source = MIX_DISARMED; + mixer_source source = MIX_DISARMED; /* * Decide which set of controls we're using. @@ -158,39 +123,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); } /* @@ -200,30 +135,21 @@ 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 */ + || (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_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) - ); + 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 @@ -252,94 +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 */ - mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits); - - /* 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(); } } @@ -375,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); @@ -383,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]); @@ -402,287 +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 */ - if (control > 1.0f) { - control = 1.0f; - - } else if (control < -1.0f) { - control = -1.0f; - } - - /* 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 bool mixer_update_pending = false; - -int -mixer_handle_text_create_mixer() -{ - /* only run on update */ - if (!mixer_update_pending) { - return 0; - } - - /* 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; - } - - /* abort if we're in the mixer - it will be tried again in the next iteration */ - if (in_mixer) { - return 1; - } - - /* 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; - - return 0; -} - -int -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); - - /* set the update flags to dirty so we reload those values after a mixer change */ - update_trims = true; - update_mc_thrust_param = true; - - /* abort if we're in the mixer - the caller is expected to retry */ - if (in_mixer) { - return 1; - } - - 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_group.reset(); - 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 -mixer_set_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 */ - mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits); - - /* 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..f839cfc332 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 is bypassing the mixer */ +#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, not 0 values of mixer */ +#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,63 +187,25 @@ 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_ASSIGNMENT 0 /**< mapped input value */ +#define PX4IO_P_RC_CONFIG_ASSIGNMENT_KILLSWITCH 100 /**< magic value for kill switch */ +#define PX4IO_P_RC_CONFIG_OPTIONS 1 /**< 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 */ +#define PX4IO_P_RC_CONFIG_STRIDE 2 /**< spacing between channel config data */ /* PWM output - overrides mixer */ #define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -282,49 +213,16 @@ enum { /* DSM bind states */ /* 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 520a4621fe..a709dbed61 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -58,8 +58,6 @@ # include #endif -#include - #include #define DEBUG @@ -71,10 +69,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 */ @@ -312,11 +306,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); @@ -349,44 +338,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); - - /* - * P O L I C E L I G H T S - * - * Not enough memory, lock down. - * - * We might need to allocate mixers later, and this will - * ensure that a developer doing a change will notice - * that he just burned the remaining RAM with static - * allocations. We don't want him to be able to - * get past that point. This needs to be clearly - * documented in the dev guide. - * - */ - if (minfo.mxordblk < 550) { - - syslog(LOG_ERR, "ERR: not enough MEM"); - bool phase = false; - - while (true) { - - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - - } else { - LED_AMBER(false); - LED_BLUE(true); - } - - up_udelay(250000); - - phase = !phase; - } - } - /* Start the failsafe led init */ failsafe_led_init(); @@ -396,19 +347,8 @@ user_start(int argc, char *argv[]) uint64_t last_debug_time = 0; uint64_t last_heartbeat_time = 0; - uint64_t last_loop_time = 0; for (;;) { - 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); @@ -446,10 +386,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 40d6c2a7f7..1e8a9f30bf 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,13 @@ * 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 +95,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. @@ -147,15 +122,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. */ @@ -170,7 +139,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)) @@ -181,8 +149,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); @@ -193,10 +159,6 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification); * Mixer */ extern void mixer_tick(void); -extern int mixer_handle_text_create_mixer(void); -extern int 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 mixer_set_failsafe(void); /** * Safety switch/LED. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index f02ddb1fa8..229d7b18e4 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,66 +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 @@ -239,30 +182,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 +193,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,118 +248,30 @@ 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++; num_values--; 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; - } } 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 mixer_handle_text(values, num_values * sizeof(*values)); - default: /* avoid offset wrap */ @@ -504,21 +313,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 */ - mixer_set_failsafe(); - } - break; default: @@ -542,32 +341,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 */ @@ -579,18 +365,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 */ @@ -689,32 +463,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; @@ -747,11 +500,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) 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; @@ -760,9 +508,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) 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; @@ -772,33 +517,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) 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)) { + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_KILLSWITCH)) { count++; } @@ -863,49 +586,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 */ { @@ -917,6 +597,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 */ @@ -937,12 +618,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); @@ -972,10 +647,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; @@ -984,19 +655,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; @@ -1009,18 +672,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 ee550a65a8..30197d2960 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 @@ -1330,39 +1330,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 * @@ -1391,62 +1358,6 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); -/** - * Rattitude 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_RATT_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 * @@ -1475,34 +1386,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 * @@ -1674,62 +1557,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); - /** * AUX1 Passthrough RC channel * @@ -2012,70 +1839,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 channelth - * negative : true when channel 0) { // number of valid slots static constexpr int num_slots = manual_control_switches_s::MODE_SLOT_NUM; @@ -533,17 +568,6 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample) if (switches.mode_slot > num_slots) { 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.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RATTITUDE, _param_rc_ratt_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()); } switches.return_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RETURN, _param_rc_return_th.get()); diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 3ee7f290ed..c95b2f04ea 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -198,22 +198,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_ratt_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_aux1, (ParamInt) _param_rc_map_aux2, @@ -224,19 +218,12 @@ private: (ParamInt) _param_rc_fails_thr, - (ParamFloat) _param_rc_assist_th, - (ParamFloat) _param_rc_auto_th, - (ParamFloat) _param_rc_ratt_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 f128fd1e27..f2012b4acd 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 42e6422e27..16c98dd70b 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -37,7 +37,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 de056a8e19..f228515f20 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -84,7 +84,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 6c682f4f3c..b43ac35e02 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -48,7 +48,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[]);