diff --git a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin index 3eab1b7bde..5de3a1b96a 100755 Binary files a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin differ diff --git a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin index 3eab1b7bde..5de3a1b96a 100755 Binary files a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin differ diff --git a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin and b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin differ diff --git a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin and b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin differ diff --git a/boards/mro/x21-777/extras/px4_io-v2_default.bin b/boards/mro/x21-777/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/mro/x21-777/extras/px4_io-v2_default.bin and b/boards/mro/x21-777/extras/px4_io-v2_default.bin differ diff --git a/boards/mro/x21/extras/px4_io-v2_default.bin b/boards/mro/x21/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/mro/x21/extras/px4_io-v2_default.bin and b/boards/mro/x21/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v3/src/board_config.h b/boards/px4/fmu-v3/src/board_config.h index 3c12d70e13..643e175c7a 100644 --- a/boards/px4/fmu-v3/src/board_config.h +++ b/boards/px4/fmu-v3/src/board_config.h @@ -159,12 +159,6 @@ #define BOARD_HAS_ON_RESET 1 -/* Internal IMU Heater - * - * Connected to the IO MCU; tell compiler to enable support - */ -#define PX4IO_HEATER_ENABLED - __BEGIN_DECLS /**************************************************************************************************** diff --git a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin index 5140391401..3e3ccc0c96 100755 Binary files a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin differ diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg index 6867adf2c8..65e1ad9310 100644 --- a/msg/actuator_armed.msg +++ b/msg/actuator_armed.msg @@ -2,7 +2,6 @@ uint64 timestamp # time since system start (microseconds) bool armed # Set to true if system is armed bool prearmed # Set to true if the actuator safety is disabled but motors are not armed -bool ready_to_arm # Set to true if system is ready to be armed bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) bool manual_lockdown # Set to true if manual throttle kill switch is engaged bool force_failsafe # Set to true if the actuators are forced to the failsafe position diff --git a/msg/heater_status.msg b/msg/heater_status.msg index 44207a3989..c27c37e4e3 100644 --- a/msg/heater_status.msg +++ b/msg/heater_status.msg @@ -14,7 +14,3 @@ uint32 controller_time_on_usec float32 proportional_value float32 integrator_value float32 feed_forward_value - -uint8 MODE_GPIO = 1 -uint8 MODE_PX4IO = 2 -uint8 mode diff --git a/msg/px4io_status.msg b/msg/px4io_status.msg index 295c8fba66..dc116a3f22 100644 --- a/msg/px4io_status.msg +++ b/msg/px4io_status.msg @@ -6,19 +6,16 @@ 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_arm_sync -bool status_failsafe -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_button_event # px4io safety button was pressed for longer than 1 second # PX4IO alarms (PX4IO_P_STATUS_ALARMS) @@ -26,18 +23,11 @@ bool alarm_pwm_error bool alarm_rc_lost # PX4IO arming (PX4IO_P_SETUP_ARMING) -bool arming_failsafe_custom bool arming_fmu_armed -bool arming_fmu_prearmed -bool arming_force_failsafe bool arming_io_arm_ok -bool arming_lockdown -bool arming_termination_failsafe uint16[8] pwm -uint16[8] pwm_disarmed -uint16[8] pwm_failsafe -uint16[8] pwm_rate_hz +uint16[4] pwm_group_rate_hz uint16[18] raw_inputs diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h index 964627c636..714110d19d 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h @@ -117,7 +117,6 @@ public: virtual ~ArchPX4IOSerial(); virtual int init(); - virtual int ioctl(unsigned operation, unsigned &arg); protected: /** diff --git a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp index 0ab41dd384..a22934b8ae 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp @@ -158,70 +158,6 @@ ArchPX4IOSerial::init() return 0; } -int -ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) -{ - switch (operation) { - - case 1: /* XXX magic number - test operation */ - switch (arg) { - case 0: - syslog(LOG_INFO, "test 0\n"); - - /* kill DMA, this is a PIO test */ - stm32_dmastop(_tx_dma); - stm32_dmastop(_rx_dma); - rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); - - for (;;) { - while (!(rSR & USART_SR_TXE)) - ; - - rDR = 0x55; - } - - return 0; - - case 1: { - unsigned fails = 0; - - for (unsigned count = 0;; count++) { - uint16_t value = count & 0xffff; - - if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) { - fails++; - } - - if (count >= 5000) { - syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); - perf_print_counter(_pc_txns); - perf_print_counter(_pc_retries); - perf_print_counter(_pc_timeouts); - perf_print_counter(_pc_crcerrs); - perf_print_counter(_pc_dmaerrs); - perf_print_counter(_pc_protoerrs); - perf_print_counter(_pc_uerrs); - perf_print_counter(_pc_idle); - perf_print_counter(_pc_badidle); - count = 0; - } - } - - return 0; - } - - case 2: - syslog(LOG_INFO, "test 2\n"); - return 0; - } - - default: - break; - } - - return -1; -} - int ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) { diff --git a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp index 2899c8db3f..efeb8977be 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp @@ -170,70 +170,6 @@ ArchPX4IOSerial::init() return 0; } -int -ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) -{ - switch (operation) { - - case 1: /* XXX magic number - test operation */ - switch (arg) { - case 0: - syslog(LOG_INFO, "test 0\n"); - - /* kill DMA, this is a PIO test */ - stm32_dmastop(_tx_dma); - stm32_dmastop(_rx_dma); - rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); - - for (;;) { - while (!(rISR & USART_ISR_TXE)) - ; - - rTDR = 0x55; - } - - return 0; - - case 1: { - unsigned fails = 0; - - for (unsigned count = 0;; count++) { - uint16_t value = count & 0xffff; - - if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) { - fails++; - } - - if (count >= 5000) { - syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); - perf_print_counter(_pc_txns); - perf_print_counter(_pc_retries); - perf_print_counter(_pc_timeouts); - perf_print_counter(_pc_crcerrs); - perf_print_counter(_pc_dmaerrs); - perf_print_counter(_pc_protoerrs); - perf_print_counter(_pc_uerrs); - perf_print_counter(_pc_idle); - perf_print_counter(_pc_badidle); - count = 0; - } - } - - return 0; - } - - case 2: - syslog(LOG_INFO, "test 2\n"); - return 0; - } - - default: - break; - } - - return -1; -} - int ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) { diff --git a/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp index 3b97f834e0..3479209aa7 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp @@ -208,70 +208,6 @@ ArchPX4IOSerial::init() return 0; } -int -ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) -{ - switch (operation) { - - case 1: /* XXX magic number - test operation */ - switch (arg) { - case 0: - syslog(LOG_INFO, "test 0\n"); - - /* kill DMA, this is a PIO test */ - stm32_dmastop(_tx_dma); - stm32_dmastop(_rx_dma); - rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); - - for (;;) { - while (!(rISR & USART_ISR_TXE)) - ; - - rTDR = 0x55; - } - - return 0; - - case 1: { - unsigned fails = 0; - - for (unsigned count = 0;; count++) { - uint16_t value = count & 0xffff; - - if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) { - fails++; - } - - if (count >= 5000) { - syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); - perf_print_counter(_pc_txns); - perf_print_counter(_pc_retries); - perf_print_counter(_pc_timeouts); - perf_print_counter(_pc_crcerrs); - perf_print_counter(_pc_dmaerrs); - perf_print_counter(_pc_protoerrs); - perf_print_counter(_pc_uerrs); - perf_print_counter(_pc_idle); - perf_print_counter(_pc_badidle); - count = 0; - } - } - - return 0; - } - - case 2: - syslog(LOG_INFO, "test 2\n"); - return 0; - } - - default: - break; - } - - return -1; -} - int ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) { diff --git a/src/drivers/drv_io_heater.h b/src/drivers/drv_io_heater.h deleted file mode 100644 index 65146e391b..0000000000 --- a/src/drivers/drv_io_heater.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file drv_io_heater.h - * - * IO IMU heater control interface (PX4IO) - */ - -#ifndef _DRV_IO_HEATER_H -#define _DRV_IO_HEATER_H - -#include -#include - -/* - * ioctl() definitions - */ - -#define IO_HEATER_DEVICE_PATH "/dev/px4io" - -#define _IO_HEATER_BASE (0x2e00) - -#define PX4IO_HEATER_CONTROL _PX4_IOC(_IO_HEATER_BASE, 0) - -/* ... to IOX_SET_VALUE + 8 */ - -/* enum passed to IO_HEATER_CONTROL ioctl()*/ -enum IO_HEATER_MODE { - HEATER_MODE_OFF = 0, - HEATER_MODE_ON = 1, - HEATER_MODE_DISABLED = -1 -}; - -#endif diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 13251bfebd..7550112152 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -111,9 +111,6 @@ __BEGIN_DECLS */ #define _PWM_SERVO_BASE 0x2a00 -/** start DSM bind */ -#define DSM_BIND_START _PX4_IOC(_PWM_SERVO_BASE, 10) - /** specific rates for configuring the timer for OneShot or PWM */ #define PWM_RATE_ONESHOT 0u #define PWM_RATE_LOWER_LIMIT 1u diff --git a/src/drivers/drv_sbus.h b/src/drivers/drv_sbus.h deleted file mode 100644 index 4b07c64490..0000000000 --- a/src/drivers/drv_sbus.h +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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_sbus.h - * - * Futaba S.BUS / S.BUS 2 compatible interface. - */ - -#ifndef _DRV_SBUS_H -#define _DRV_SBUS_H - -#include -#include - -#include "drv_orb_dev.h" - -/** - * Path for the default S.BUS device - */ -#define SBUS0_DEVICE_PATH "/dev/sbus0" - -#define _SBUS_BASE 0x2c00 - -/** Enable S.BUS version 1 / 2 output (0 to disable) */ -#define SBUS_SET_PROTO_VERSION _IOC(_SBUS_BASE, 0) - -#endif /* _DRV_SBUS_H */ diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index 170bd859a4..c977d1f0cc 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -46,34 +46,11 @@ #include #include #include -#include - -#if defined(BOARD_USES_PX4IO_VERSION) and defined(PX4IO_HEATER_ENABLED) -// Heater on some boards is on IO MCU -// Use ioctl calls to IO driver to turn heater on/off -# define HEATER_PX4IO -#else -// Use direct calls to turn GPIO pin on/off -# ifndef GPIO_HEATER_OUTPUT -# error "To use the heater driver, the board_config.h must define and initialize GPIO_HEATER_OUTPUT" -# endif -# define HEATER_GPIO -#endif Heater::Heater() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { -#ifdef HEATER_PX4IO - _io_fd = px4_open(IO_HEATER_DEVICE_PATH, O_RDWR); - - if (_io_fd < 0) { - PX4_ERR("Unable to open heater device path"); - return; - } - -#endif - _heater_status_pub.advertise(); } @@ -96,65 +73,23 @@ int Heater::custom_command(int argc, char *argv[]) void Heater::disable_heater() { // Reset heater to off state. -#ifdef HEATER_PX4IO - if (_io_fd >= 0) { - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED); - } - -#endif - -#ifdef HEATER_GPIO px4_arch_configgpio(GPIO_HEATER_OUTPUT); -#endif } void Heater::initialize_heater_io() { // Initialize heater to off state. -#ifdef HEATER_PX4IO - if (_io_fd < 0) { - _io_fd = px4_open(IO_HEATER_DEVICE_PATH, O_RDWR); - } - - if (_io_fd >= 0) { - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); - } - -#endif - -#ifdef HEATER_GPIO px4_arch_configgpio(GPIO_HEATER_OUTPUT); -#endif } void Heater::heater_off() { -#ifdef HEATER_PX4IO - - if (_io_fd >= 0) { - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); - } - -#endif - -#ifdef HEATER_GPIO HEATER_OUTPUT_EN(false); -#endif } void Heater::heater_on() { -#ifdef HEATER_PX4IO - - if (_io_fd >= 0) { - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON); - } - -#endif - -#ifdef HEATER_GPIO HEATER_OUTPUT_EN(true); -#endif } bool Heater::initialize_topics() @@ -182,14 +117,6 @@ bool Heater::initialize_topics() void Heater::Run() { if (should_exit()) { -#if defined(HEATER_PX4IO) - - // must be closed from wq thread - if (_io_fd >= 0) { - px4_close(_io_fd); - } - -#endif exit_and_cleanup(); return; } @@ -260,14 +187,6 @@ void Heater::publish_status() status.proportional_value = _proportional_value; status.integrator_value = _integrator_value; status.feed_forward_value = _param_sens_imu_temp_ff.get(); - -#ifdef HEATER_PX4IO - status.mode = heater_status_s::MODE_PX4IO; -#endif -#ifdef HEATER_GPIO - status.mode = heater_status_s::MODE_GPIO; -#endif - status.timestamp = hrt_absolute_time(); _heater_status_pub.publish(status); } diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h index 966b04019c..44fcc71ea0 100644 --- a/src/drivers/heater/heater.h +++ b/src/drivers/heater/heater.h @@ -130,14 +130,6 @@ private: */ void update_params(const bool force = false); - /** Work queue struct for the scheduler. */ - static struct work_s _work; - - /** File descriptor for PX4IO for heater ioctl's */ -#if defined(PX4IO_HEATER_ENABLED) - int _io_fd {-1}; -#endif - bool _heater_initialized = false; bool _heater_on = false; bool _temperature_target_met = false; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index fba6a2bd67..cf794281e3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -49,11 +49,8 @@ #include #include -#include #include -#include -#include #include #include #include @@ -68,8 +65,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -83,8 +79,6 @@ #include "uploader.h" -#include "modules/dataman/dataman.h" - #include "px4io_driver.h" #define PX4IO_SET_DEBUG _IOC(0xff00, 0) @@ -152,16 +146,12 @@ public: */ void test_fmu_fail(bool is_fail) { _test_fmu_fail = is_fail; }; - 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(); void updateTimerRateGroups(); static int checkcrc(int argc, char *argv[]); @@ -173,7 +163,6 @@ private: 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 @@ -199,6 +188,7 @@ private: uORB::Subscription _t_actuator_armed{ORB_ID(actuator_armed)}; ///< system armed control topic uORB::Subscription _t_vehicle_command{ORB_ID(vehicle_command)}; ///< vehicle command topic uORB::Subscription _t_vehicle_status{ORB_ID(vehicle_status)}; ///< vehicle status topic + uORB::Subscription _heater_status_sub{ORB_ID(heater_status)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -216,23 +206,14 @@ private: ButtonPublisher _button_publisher; bool _previous_safety_off{false}; - bool _lockdown_override{false}; ///< override the safety lockdown - 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{false}; ///< To test what happens if IO loses FMU - bool _in_test_mode{false}; ///< true if PWM_SERVO_ENTER_TEST_MODE is active MixingOutput _mixing_output{"PWM_MAIN", PX4IO_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true}; - bool _pwm_min_configured{false}; - bool _pwm_max_configured{false}; - bool _pwm_fail_configured{false}; - bool _pwm_dis_configured{false}; - bool _pwm_rev_configured{false}; - /** * Update IO's arming-related state */ @@ -305,15 +286,6 @@ private: */ int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits); - /** - * Handle a status update from IO. - * - * Publish IO status information if necessary. - * - * @param status The status register - */ - int io_handle_status(uint16_t status); - /** * Handle issuing dsm bind ioctl to px4io. * @@ -332,7 +304,6 @@ private: void update_params(); DEFINE_PARAMETERS( - (ParamInt) _param_pwm_sbus_mode, (ParamInt) _param_rc_rssi_pwm_chan, (ParamInt) _param_rc_rssi_pwm_max, (ParamInt) _param_rc_rssi_pwm_min, @@ -412,7 +383,6 @@ int 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_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); @@ -452,8 +422,7 @@ int PX4IO::init() } /* 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); + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); if (ret != OK) { mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail\t"); @@ -482,28 +451,6 @@ int PX4IO::init() return OK; } -void PX4IO::updateDisarmed() -{ - uint16_t values[PX4IO_MAX_ACTUATORS] {}; - - for (unsigned i = 0; i < _max_actuators; i++) { - values[i] = _mixing_output.disarmedValue(i); - } - - io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, values, _max_actuators); -} - -void PX4IO::updateFailsafe() -{ - uint16_t values[PX4IO_MAX_ACTUATORS] {}; - - for (unsigned i = 0; i < _max_actuators; i++) { - values[i] = _mixing_output.actualFailsafeValue(i); - } - - io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, values, _max_actuators); -} - void PX4IO::Run() { if (should_exit()) { @@ -519,6 +466,23 @@ void PX4IO::Run() perf_begin(_cycle_perf); perf_count(_interval_perf); + if (_thermal_control) { + if (_heater_status_sub.updated()) { + heater_status_s heater_status; + + if (_heater_status_sub.copy(&heater_status)) { + + if (heater_status.heater_on) { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_FULL); + + } else { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_OFF); + } + } + } + } + + /* if we have new control data from the ORB, handle it */ if (_param_sys_hitl.get() <= 0) { _mixing_output.update(); @@ -595,11 +559,6 @@ void PX4IO::Run() update_params(); - /* Check if the flight termination circuit breaker has been updated */ - bool disable_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); - /* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */ - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION, !disable_flighttermination); - if (_param_sens_en_themal.get() != _thermal_control || _param_update_force) { _thermal_control = _param_sens_en_themal.get(); @@ -615,21 +574,6 @@ void PX4IO::Run() io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, tctrl); } - - /* S.BUS output */ - if (_param_pwm_sbus_mode.get() == 1) { - /* enable S.BUS 1 */ - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); - - } else if (_param_pwm_sbus_mode.get() == 2) { - /* enable S.BUS 2 */ - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); - - } else { - /* disable S.BUS */ - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, - (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); - } } } @@ -738,8 +682,6 @@ void PX4IO::update_params() // sync params to IO updateTimerRateGroups(); - updateFailsafe(); - updateDisarmed(); _first_param_update = false; return; } @@ -776,38 +718,7 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - if (armed.prearmed) { - set |= PX4IO_P_SETUP_ARMING_FMU_PREARMED; - - } else { - clear |= PX4IO_P_SETUP_ARMING_FMU_PREARMED; - } - - if ((armed.lockdown || armed.manual_lockdown) && !_lockdown_override) { - set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; - _lockdown_override = true; - - } else if (!(armed.lockdown || armed.manual_lockdown) && _lockdown_override) { - clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; - _lockdown_override = false; - } - - if (armed.force_failsafe) { - set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; - - } else { - clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; - } - - // XXX this is for future support in the commander - // but can be removed if unneeded - // if (armed.termination_failsafe) { - // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } else { - // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } - - if (armed.ready_to_arm) { + if (true) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } else { @@ -824,60 +735,6 @@ PX4IO::io_set_arming_state() return 0; } -int PX4IO::io_handle_status(uint16_t status) -{ - int ret = 1; - /** - * WARNING: This section handles in-air resets. - */ - - /* check for IO reset - force it back to armed if necessary */ - if (!(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { - /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC); - - /* set new status */ - _status = status; - - } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { - - /* set the sync flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC); - /* set new status */ - _status = status; - - } else { - ret = 0; - - /* set new status */ - _status = status; - } - - /** - * Get and handle the safety button status - */ - const bool safety_button_pressed = status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT; - - if (safety_button_pressed) { - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0); - _button_publisher.safetyButtonTriggerEvent(); - } - - /** - * Inform PX4IO board about safety_off state for LED control - */ - vehicle_status_s vehicle_status; - - if (_t_vehicle_status.update(&vehicle_status)) { - if (_previous_safety_off != vehicle_status.safety_off) { - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off); - _previous_safety_off = vehicle_status.safety_off; - } - } - - return ret; -} - int PX4IO::dsm_bind_ioctl(int dsmMode) { // Do not bind if invalid pulses are provided @@ -933,7 +790,30 @@ int PX4IO::io_get_status() const uint16_t STATUS_VSERVO = regs[4]; const uint16_t STATUS_VRSSI = regs[5]; - io_handle_status(STATUS_FLAGS); + /* set new status */ + _status = STATUS_FLAGS; + + /** + * Get and handle the safety button status + */ + const bool safety_button_pressed = _status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT; + + if (safety_button_pressed) { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0); + _button_publisher.safetyButtonTriggerEvent(); + } + + /** + * Inform PX4IO board about safety_off state for LED control + */ + vehicle_status_s vehicle_status; + + if (_t_vehicle_status.update(&vehicle_status)) { + if (_previous_safety_off != vehicle_status.safety_off) { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off); + _previous_safety_off = vehicle_status.safety_off; + } + } const float rssi_v = STATUS_VRSSI * 0.001f; // voltage is scaled to mV @@ -963,48 +843,32 @@ int PX4IO::io_get_status() status.free_memory_bytes = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM); // PX4IO_P_STATUS_FLAGS - status.status_outputs_armed = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; 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_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_button_event = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT; // PX4IO_P_STATUS_ALARMS - 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_rc_lost = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST; + status.alarm_pwm_error = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_PWM_ERROR; // 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_failsafe_custom = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; - 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_io_arm_ok = SETUP_ARMING & PX4IO_P_SETUP_ARMING_IO_ARM_OK; + status.arming_fmu_armed = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_ARMED; for (unsigned i = 0; i < _max_actuators; 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); + status.pwm[i] = io_reg_get(PX4IO_PAGE_DIRECT_PWM, i); } - { - int i = 0; - - for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) { - // This is a bit different than below, setting the groups, not the channels - status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset); - } - } + status.pwm_group_rate_hz[0] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP0); + status.pwm_group_rate_hz[1] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP1); + status.pwm_group_rate_hz[2] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP2); + status.pwm_group_rate_hz[3] = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP3); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); @@ -1233,8 +1097,7 @@ int PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC + 1)); - printf("%" PRIu32 " controls %" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs\n", - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), + printf("%" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs\n", 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)); @@ -1269,14 +1132,10 @@ int PX4IO::print_status() /* setup and state */ uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); - printf("features 0x%04" PRIx16 "%s%s%s\n", features, - ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), - ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), + printf("features 0x%04" PRIx16 "%s\n", features, ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") ); - printf("sbus rate %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE)); - printf("debuglevel %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); /* IMU heater (Pixhawk 2.1) */ @@ -1304,11 +1163,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* regular ioctl? */ switch (cmd) { - case DSM_BIND_START: - /* bind a DSM receiver */ - ret = dsm_bind_ioctl(arg); - break; - case PX4IO_SET_DEBUG: PX4_DEBUG("PX4IO_SET_DEBUG"); @@ -1317,7 +1171,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; case PX4IO_REBOOT_BOOTLOADER: - if (system_status() & PX4IO_P_SETUP_ARMING_FMU_ARMED) { + if (_status & PX4IO_P_SETUP_ARMING_FMU_ARMED) { PX4_ERR("not upgrading IO firmware, system is armed"); return -EINVAL; @@ -1357,36 +1211,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) 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); - - } else if (arg == 2) { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); - - } else { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, - (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); - } - - break; - - case PX4IO_HEATER_CONTROL: - if (arg == (unsigned long)HEATER_MODE_DISABLED) { - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_IGNORE); - - } else if (arg == 1) { - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_FULL); - - } else { - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, PX4IO_THERMAL_OFF); - - } - - break; - default: /* see if the parent class can make any use of it */ ret = CDev::ioctl(filep, cmd, arg); @@ -1507,7 +1331,7 @@ int PX4IO::bind(int argc, char *argv[]) pulses = atoi(argv[1]); } - get_instance()->ioctl(nullptr, DSM_BIND_START, pulses); + get_instance()->dsm_bind_ioctl(pulses); return 0; } @@ -1678,28 +1502,6 @@ int PX4IO::custom_command(int argc, char *argv[]) return bind(argc - 1, argv + 1); } - if (!strcmp(verb, "sbus1_out")) { - int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); - - if (ret != 0) { - PX4_ERR("S.BUS v1 failed (%i)", ret); - return 1; - } - - return 0; - } - - if (!strcmp(verb, "sbus2_out")) { - int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); - - if (ret != 0) { - PX4_ERR("S.BUS v2 failed (%i)", ret); - return 1; - } - - return 0; - } - if (!strcmp(verb, "test_fmu_fail")) { get_instance()->test_fmu_fail(true); return 0; @@ -1736,8 +1538,6 @@ Output driver communicating with the IO co-processor. PRINT_MODULE_USAGE_ARG("", "0=disabled, 9=max verbosity", false); PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind"); PRINT_MODULE_USAGE_ARG("dsm2|dsmx|dsmx8", "protocol", false); - PRINT_MODULE_USAGE_COMMAND_DESCR("sbus1_out", "enable sbus1 out"); - PRINT_MODULE_USAGE_COMMAND_DESCR("sbus2_out", "enable sbus2 out"); PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_fail", "test: turn off IO updates"); PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_ok", "re-enable IO updates"); diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index ef1c1ee6ab..d0e21aabef 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -53,13 +53,3 @@ * @group System */ PARAM_DEFINE_INT32(SYS_USE_IO, 0); - -/** - * S.BUS out - * - * Set to 1 to enable S.BUS version 1 output instead of RSSI. - * - * @boolean - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(PWM_SBUS_MODE, 0); diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 5972a2b64d..fa2d6a6431 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -66,7 +66,6 @@ int ex_hwtest_main(int argc, char *argv[]) memset(&arm, 0, sizeof(arm)); arm.timestamp = hrt_absolute_time(); - arm.ready_to_arm = true; arm.armed = true; orb_advert_t arm_pub_ptr = orb_advertise(ORB_ID(actuator_armed), &arm); orb_publish(ORB_ID(actuator_armed), arm_pub_ptr, &arm); @@ -75,13 +74,6 @@ int ex_hwtest_main(int argc, char *argv[]) int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); orb_copy(ORB_ID(actuator_armed), arm_sub_fd, &arm); - if (arm.ready_to_arm && arm.armed) { - warnx("Actuator armed"); - - } else { - errx(1, "Arming actuators failed"); - } - hrt_abstime stime; int count = 0; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 49d37d50f4..8bb00b1181 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -79,7 +79,6 @@ MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, Ou /* Safely initialize armed flags */ _armed.armed = false; _armed.prearmed = false; - _armed.ready_to_arm = false; _armed.lockdown = false; _armed.force_failsafe = false; _armed.in_esc_calibration_mode = false; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f6a4d1ed68..e3f40e4cd2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -93,7 +93,6 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme { return (a.armed == b.armed && a.prearmed == b.prearmed && - a.ready_to_arm == b.ready_to_arm && a.lockdown == b.lockdown && a.manual_lockdown == b.manual_lockdown && a.force_failsafe == b.force_failsafe && @@ -2232,7 +2231,6 @@ Commander::run() // publish actuator_armed first (used by output modules) _actuator_armed.armed = _arm_state_machine.isArmed(); - _actuator_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby(); _actuator_armed.timestamp = hrt_absolute_time(); _actuator_armed_pub.publish(_actuator_armed); diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 3df90ffdac..65960018db 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -31,12 +31,9 @@ # ############################################################################ -option(PX4IO_PERF "Enable px4io perf counters" OFF) - add_library(px4iofirmware adc.cpp controls.cpp - mixer.cpp px4io.cpp registers.c safety_button.cpp @@ -54,8 +51,3 @@ target_link_libraries(px4iofirmware nuttx_mm rc ) - -if(PX4IO_PERF) - target_compile_definitions(px4iofirmware PRIVATE PX4IO_PERF) - target_link_libraries(px4iofirmware PRIVATE perf) -endif() diff --git a/src/modules/px4iofirmware/adc.cpp b/src/modules/px4iofirmware/adc.cpp index 1e748e3951..80b5754a01 100644 --- a/src/modules/px4iofirmware/adc.cpp +++ b/src/modules/px4iofirmware/adc.cpp @@ -45,10 +45,6 @@ #include -#if defined(PX4IO_PERF) -# include -#endif - #define DEBUG #include "px4io.h" @@ -79,17 +75,9 @@ #define rJDR4 REG(STM32_ADC_JDR4_OFFSET) #define rDR REG(STM32_ADC_DR_OFFSET) -#if defined(PX4IO_PERF) -perf_counter_t adc_perf; -#endif - int adc_init(void) { -#if defined(PX4IO_PERF) - adc_perf = perf_alloc(PC_ELAPSED, "adc"); -#endif - /* put the ADC into power-down mode */ rCR2 &= ~ADC_CR2_ADON; up_udelay(10); @@ -143,10 +131,6 @@ adc_init(void) uint16_t adc_measure(unsigned channel) { -#if defined(PX4IO_PERF) - perf_begin(adc_perf); -#endif - /* clear any previous EOC */ rSR = 0; (void)rDR; @@ -162,9 +146,6 @@ adc_measure(unsigned channel) /* never spin forever - this will give a bogus result though */ if (hrt_elapsed_time(&now) > 100) { -#if defined(PX4IO_PERF) - perf_end(adc_perf); -#endif return 0xffff; } } @@ -173,8 +154,5 @@ adc_measure(unsigned channel) uint16_t result = rDR; rSR = 0; -#if defined(PX4IO_PERF) - perf_end(adc_perf); -#endif return result; } diff --git a/src/modules/px4iofirmware/controls.cpp b/src/modules/px4iofirmware/controls.cpp index f5259f62f7..3629d59ae4 100644 --- a/src/modules/px4iofirmware/controls.cpp +++ b/src/modules/px4iofirmware/controls.cpp @@ -50,21 +50,11 @@ #include #include -#if defined(PX4IO_PERF) -# include -#endif - #include "px4io.h" 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); -#if defined(PX4IO_PERF) -static perf_counter_t c_gather_dsm; -static perf_counter_t c_gather_sbus; -static perf_counter_t c_gather_ppm; -#endif - static int _dsm_fd = -1; int _sbus_fd = -1; @@ -79,9 +69,6 @@ static unsigned _frame_drops = 0; bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { -#if defined(PX4IO_PERF) - perf_begin(c_gather_dsm); -#endif uint8_t n_bytes = 0; uint8_t *bytes; bool dsm_11_bit; @@ -118,10 +105,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool } } -#if defined(PX4IO_PERF) - perf_end(c_gather_dsm); -#endif - /* get data from FD and attempt to parse with DSM and ST24 libs */ uint8_t st24_rssi, lost_count; uint16_t st24_channel_count = 0; @@ -198,12 +181,6 @@ controls_init(void) /* S.bus input (USART3) */ _sbus_fd = sbus_init("/dev/ttyS2", false); - -#if defined(PX4IO_PERF) - c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm"); - c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus"); - c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm"); -#endif } void @@ -244,10 +221,6 @@ controls_tick() _rssi = 0; } -#if defined(PX4IO_PERF) - perf_begin(c_gather_sbus); -#endif - bool sbus_updated = false; if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24 | PX4IO_P_STATUS_FLAGS_RC_SUMD))) { @@ -282,18 +255,11 @@ controls_tick() } } -#if defined(PX4IO_PERF) - perf_end(c_gather_sbus); -#endif - /* * XXX each S.bus frame will cause a PPM decoder interrupt * storm (lots of edges). It might be sensible to actually * disable the PPM decoder completely if we have S.bus signal. */ -#if defined(PX4IO_PERF) - perf_begin(c_gather_ppm); -#endif bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); if (ppm_updated) { @@ -303,16 +269,9 @@ controls_tick() r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } -#if defined(PX4IO_PERF) - perf_end(c_gather_ppm); -#endif - bool dsm_updated = false, st24_updated = false, sumd_updated = false; if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_SBUS | PX4IO_P_STATUS_FLAGS_RC_PPM))) { -#if defined(PX4IO_PERF) - perf_begin(c_gather_dsm); -#endif (void)dsm_port_input(&_rssi, &dsm_updated, &st24_updated, &sumd_updated); @@ -327,10 +286,6 @@ controls_tick() if (sumd_updated) { atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SUMD); } - -#if defined(PX4IO_PERF) - perf_end(c_gather_dsm); -#endif } /* limit number of channels to allowable data size */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp deleted file mode 100644 index 9fc40dcbde..0000000000 --- a/src/modules/px4iofirmware/mixer.cpp +++ /dev/null @@ -1,251 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2017 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.cpp - * - * Control channel input/output mixer and failsafe. - * - * @author Lorenz Meier - */ - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include - -extern "C" { - /* #define DEBUG */ -#include "px4io.h" -} - -/* - * Maximum interval in us before FMU signal is considered lost - */ -#define FMU_INPUT_DROP_LIMIT_US 500000 - -/* current servo arm/disarm state */ -static volatile bool mixer_servos_armed = false; -static volatile bool should_arm = false; -static volatile bool should_arm_nothrottle = false; -static volatile bool should_always_enable_pwm = false; - -static bool new_fmu_data = false; -static uint64_t last_fmu_update = 0; - -extern int _sbus_fd; - -/* selected control values and count for mixing */ -enum mixer_source { - MIX_NONE, - MIX_DISARMED, - MIX_FAILSAFE, -}; - -void -mixer_tick() -{ - /* 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; - leave_critical_section(irq_flags); - - if ((fmu_data_received_time == 0) || - hrt_elapsed_time(&fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { - - /* too long without FMU input, time to go to failsafe */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - isr_debug(1, "AP RX timeout"); - } - - 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); - - /* this flag is never cleared once OK */ - atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED); - - if (fmu_data_received_time > last_fmu_update) { - new_fmu_data = true; - last_fmu_update = fmu_data_received_time; - } - } - - /* default to disarmed mixing */ - mixer_source source = MIX_DISARMED; - - /* - * Decide which set of controls we're using. - */ - - /* Do not mix if we have raw PWM and FMU is ok. */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - - /* 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); - } - - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - */ - should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ - && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */ - ; - - should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ - && ((r_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 */ - )); - - /* we enable PWM output always on the IO side if FMU is up and running - * as zero-outputs can be controlled by FMU by sending a 0 PWM command - */ - should_always_enable_pwm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)); - - /* - * Check if FMU is still alive, if not, terminate the flight - */ - if (REG_TO_BOOL(r_setup_flighttermination) && /* Flight termination is allowed */ - (source == MIX_DISARMED) && /* and if we ended up not changing the default mixer */ - should_arm && /* and we should be armed, so we intended to provide outputs */ - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { /* and FMU is initialized */ - atomic_modify_or(&r_setup_arming, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); /* then FMU is dead -> terminate flight */ - } - - /* - * Check if we should force failsafe - and do it if we have to - */ - if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) { - source = MIX_FAILSAFE; - } - - /* - * Set failsafe status flag depending on mixing source - */ - if (source == MIX_FAILSAFE) { - atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FAILSAFE); - - } else { - atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE)); - } - - /* - * 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]; - } - - } 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]; - } - } - - /* set arming */ - bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm); - - /* lockdown means to send a valid pulse which disables the outputs */ - if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { - needs_to_arm = true; - } - - if (needs_to_arm && !mixer_servos_armed) { - /* need to arm, but not armed */ - up_pwm_servo_arm(true, 0); - mixer_servos_armed = true; - atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); - isr_debug(5, "> PWM enabled"); - - } else if (!needs_to_arm && mixer_servos_armed) { - /* armed but need to disarm */ - up_pwm_servo_arm(false, 0); - mixer_servos_armed = false; - atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)); - isr_debug(5, "> PWM disabled"); - } - - if (mixer_servos_armed - && (should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE)) - && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { - /* update the servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - up_pwm_servo_set(i, r_page_servos[i]); - } - - /* 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); - - } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { - 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))) { - /* set the disarmed servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - up_pwm_servo_set(i, r_page_servo_disarmed[i]); - /* copy values into reporting register */ - r_page_servos[i] = r_page_servo_disarmed[i]; - } - - /* set S.BUS1 or S.BUS2 outputs */ - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { - sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); - } - - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { - sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); - } - } -} diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index a0389d78b4..840e583cc6 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -79,16 +79,13 @@ #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_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_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 */ @@ -97,21 +94,20 @@ /* dynamic status page */ #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 */ +/* (1 << 0) */ #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 1) /* RC input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 2) /* PPM input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */ #define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 5) /* controls from FMU are valid */ #define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 6) /* raw PWM from FMU */ -#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 7) /* the arming state between IO and FMU is in sync */ -#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 8) /* initialisation of the IO completed without error */ -#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 9) /* failsafe is active */ +/* UNUSED (1 << 7) */ +/* UNUSED (1 << 8) */ +/* UNUSED (1 << 9) */ #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 */ +/* UNUSED (1 << 11) */ #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_FLAGS_SAFETY_BUTTON_EVENT (1 << 14) /* px4io safety button was pressed for longer than 1 second */ @@ -123,9 +119,6 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ -/* array of PWM servo output values, microseconds */ -#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_P_RAW_RC_COUNT 0 /* number of valid channels */ @@ -152,22 +145,14 @@ /* setup page */ #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 */ +/* UNUSED (1 << 0) */ +/* UNUSED (1 << 1) */ #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_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values */ -#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 4) /* If set, the system operates normally, but won't actuate any servos */ -#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 5) /* If set, the system will always output the failsafe values */ -#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 6) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ -#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ -#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_VSERVO_SCALE 5 /* hardware rev [2] servo voltage correction factor (float) */ #define PX4IO_P_SETUP_DSM 6 /* DSM bind state */ enum { /* DSM bind states */ @@ -187,9 +172,9 @@ enum { /* DSM bind states */ /* storage space of 12 occupied by CRC */ #define PX4IO_P_SETUP_SAFETY_BUTTON_ACK 14 /**< ACK from FMU when it gets safety button pressed status */ #define PX4IO_P_SETUP_SAFETY_OFF 15 /**< FMU inform PX4IO about safety_off for LED indication*/ -#define PX4IO_P_SETUP_SBUS_RATE 16 /**< frame rate of SBUS1 output in Hz */ +/* UNUSED 16 */ #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 */ +/* UNUSED 18 */ #define PX4IO_P_SETUP_PWM_RATE_GROUP0 19 /* Configure timer group 0 update rate in Hz */ #define PX4IO_P_SETUP_PWM_RATE_GROUP1 20 /* Configure timer group 1 update rate in Hz */ #define PX4IO_P_SETUP_PWM_RATE_GROUP2 21 /* Configure timer group 2 update rate in Hz */ @@ -202,16 +187,6 @@ enum { /* DSM bind states */ /* PWM output */ #define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ -/* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ - -/* 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 disarmed values that are active */ -#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ - /** * Serial protocol encapsulation. */ diff --git a/src/modules/px4iofirmware/px4io.cpp b/src/modules/px4iofirmware/px4io.cpp index 69527d7bf2..a40e0d34ac 100644 --- a/src/modules/px4iofirmware/px4io.cpp +++ b/src/modules/px4iofirmware/px4io.cpp @@ -55,10 +55,6 @@ #include #include -#if defined(PX4IO_PERF) -# include -#endif - #include #define DEBUG @@ -77,6 +73,20 @@ static volatile uint32_t last_msg_counter; static volatile uint8_t msg_next_out; static volatile uint8_t msg_next_in; +#include +#include + +/* + * Maximum interval in us before FMU signal is considered lost + */ +#define FMU_INPUT_DROP_LIMIT_US 500000 + +/* current servo arm/disarm state */ +static volatile bool mixer_servos_armed = false; + +static bool new_fmu_data = false; +static uint64_t last_fmu_update = 0; + /* * WARNING: too large buffers here consume the memory required * for mixer handling. Do not allocate more than 80 bytes for @@ -272,6 +282,64 @@ calculate_fw_crc(void) r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16; } +static void mixer_tick() +{ + /* 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; + leave_critical_section(irq_flags); + + if ((fmu_data_received_time == 0) || + hrt_elapsed_time(&fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + + /* too long without FMU input, time to go to failsafe */ + if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { + isr_debug(1, "AP RX timeout"); + } + + 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); + + if (fmu_data_received_time > last_fmu_update) { + new_fmu_data = true; + last_fmu_update = fmu_data_received_time; + } + } + + /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + */ + const bool should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) + && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM); + + if (should_arm && !mixer_servos_armed) { + /* need to arm, but not armed */ + up_pwm_servo_arm(true, 0); + mixer_servos_armed = true; + isr_debug(5, "> PWM enabled"); + + } else if (!should_arm && mixer_servos_armed) { + /* armed but need to disarm */ + up_pwm_servo_arm(false, 0); + mixer_servos_armed = false; + isr_debug(5, "> PWM disabled"); + } + + if (mixer_servos_armed && should_arm) { + /* update the servo outputs. */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + up_pwm_servo_set(i, r_page_direct_pwm[i]); + } + } +} + extern "C" __EXPORT int user_start(int argc, char *argv[]) { /* configure the first 8 PWM outputs (i.e. all of them) */ @@ -307,11 +375,6 @@ extern "C" __EXPORT int user_start(int argc, char *argv[]) LED_GREEN(false); #endif /* LED_GREEN */ - /* turn off S.Bus out (if supported) */ -#ifdef ENABLE_SBUS_OUT - ENABLE_SBUS_OUT(false); -#endif - /* start the safety button handler */ safety_button_init(); @@ -324,17 +387,6 @@ extern "C" __EXPORT int user_start(int argc, char *argv[]) /* start the FMU interface */ interface_init(); -#if defined(PX4IO_PERF) - /* add a performance counter for mixing */ - perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); - - /* add a performance counter for controls */ - perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); - - /* and one for measuring the loop rate */ - perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); -#endif - struct mallinfo minfo = mallinfo(); r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk; syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); @@ -354,30 +406,9 @@ extern "C" __EXPORT int user_start(int argc, char *argv[]) for (;;) { watchdog_pet(); -#if defined(PX4IO_PERF) - /* track the rate at which the loop is running */ - perf_count(loop_perf); - - /* kick the mixer */ - perf_begin(mixer_perf); -#endif - mixer_tick(); - -#if defined(PX4IO_PERF) - perf_end(mixer_perf); - - /* kick the control inputs */ - perf_begin(controls_perf); -#endif - controls_tick(); -#if defined(PX4IO_PERF) - perf_end(controls_perf); -#endif - - /* blink blue LED at 4Hz in normal operation. When in override blink 4x faster so the user can clearly see diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 99cba83e67..855424f3a2 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -77,13 +77,10 @@ __BEGIN_DECLS * Registers. */ extern volatile uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */ -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 volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ -extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ -extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ /* * Register aliases. @@ -99,13 +96,6 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #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_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE] - -#define r_setup_flighttermination r_page_setup[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] /* * System state structure. @@ -123,8 +113,6 @@ struct sys_state_s { extern struct sys_state_s system_state; -# define ENABLE_SBUS_OUT(_s) px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) - # define VDD_SERVO_FAULT (!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT)) # define PX4IO_ADC_CHANNEL_COUNT 2 @@ -137,11 +125,6 @@ void atomic_modify_or(volatile uint16_t *target, uint16_t modification); void atomic_modify_clear(volatile uint16_t *target, uint16_t modification); void atomic_modify_and(volatile uint16_t *target, uint16_t modification); -/* - * Mixer - */ -extern void mixer_tick(void); - /** * Safety button/LED. */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a191577ef3..539f28951c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -55,7 +55,6 @@ #include "protocol.h" static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); -static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate); /** * PAGE 0 @@ -67,7 +66,6 @@ static const uint16_t r_page_config[] = { [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2, [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = PX4IO_BL_VERSION, [PX4IO_P_CONFIG_MAX_TRANSFER] = PX4IO_MAX_TRANSFER_LEN, - [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, [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, @@ -80,20 +78,12 @@ static const uint16_t r_page_config[] = { */ volatile uint16_t r_page_status[] = { [PX4IO_P_STATUS_FREEMEM] = 0, - [PX4IO_P_STATUS_CPULOAD] = 0, [PX4IO_P_STATUS_FLAGS] = 0, [PX4IO_P_STATUS_ALARMS] = 0, [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, }; -/** - * PAGE 3 - * - * Servo PWM values - */ -uint16_t r_page_servos[PX4IO_SERVO_COUNT]; - /** * PAGE 4 * @@ -133,51 +123,19 @@ 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] = 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, [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, [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_THERMAL] = PX4IO_THERMAL_IGNORE, - [PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0, [PX4IO_P_SETUP_PWM_RATE_GROUP0 ... PX4IO_P_SETUP_PWM_RATE_GROUP3] = 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) +#define PX4IO_P_SETUP_FEATURES_VALID (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_IO_ARM_OK | \ - PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ - PX4IO_P_SETUP_ARMING_LOCKDOWN | \ - PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) +#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_IO_ARM_OK) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) -/* - * PAGE 104 uses r_page_servos. - */ - -/** - * PAGE 105 - * - * Failsafe servo PWM values - * - * Disable pulses as default. - */ -uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; - -/** - * PAGE 109 - * - * disarmed PWM values for difficult ESCs - * - */ -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) { @@ -208,60 +166,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num break; - /* handle setup for servo failsafe values */ - case PX4IO_PAGE_FAILSAFE_PWM: - - /* copy channel data */ - while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - - if (*values == 0) { - /* ignore 0 */ - } else if (*values < PWM_LOWEST_MIN) { - r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; - - } else if (*values > PWM_HIGHEST_MAX) { - r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; - - } else { - r_page_servo_failsafe[offset] = *values; - } - - /* flag the failsafe values as custom */ - r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; - - offset++; - num_values--; - values++; - } - - break; - - case PX4IO_PAGE_DISARMED_PWM: { - /* 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; - - } else if (*values > PWM_HIGHEST_MAX) { - r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; - - } else { - r_page_servo_disarmed[offset] = *values; - } - - offset++; - num_values--; - values++; - } - - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; - } - break; - default: /* avoid offset wrap */ @@ -297,19 +201,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_status_alarms &= ~value; break; - case PX4IO_P_STATUS_FLAGS: - - /* - * 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 (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { - r_status_flags = value; - - } - - break; - default: /* just ignore writes to other registers in this page */ break; @@ -323,29 +214,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value &= PX4IO_P_SETUP_FEATURES_VALID; - /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ - - /* switch S.Bus output pin as needed */ -#ifdef ENABLE_SBUS_OUT - ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); - - /* disable the conflicting options with SBUS 1 */ - if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_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_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_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT); - } - /* apply changes */ r_setup_features = value; @@ -355,58 +223,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value &= PX4IO_P_SETUP_ARMING_VALID; - /* - * If the failsafe termination flag is set, do not allow the autopilot to unset it - */ - value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); - - /* - * If failsafe termination is enabled and force failsafe bit is set, do not allow - * the autopilot to clear it. - */ - if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) { - value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); - } - r_setup_arming = value; break; - case PX4IO_P_SETUP_PWM_RATES: - value &= PX4IO_P_SETUP_RATES_VALID; - pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); - break; - - case PX4IO_P_SETUP_PWM_DEFAULTRATE: - if (value < 25) { - value = 25; - } - - if (value > 400) { - value = 400; - } - - pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); - break; - - case PX4IO_P_SETUP_PWM_ALTRATE: - - /* For PWM constrain to [25,400]Hz - * For Oneshot there is no rate, 0 is therefore used to select Oneshot mode - */ - if (value != 0) { - if (value < 25) { - value = 25; - } - - if (value > 400) { - value = 400; - } - } - - pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); - break; - case PX4IO_P_SETUP_SET_DEBUG: r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); @@ -446,13 +266,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; - case PX4IO_P_SETUP_SBUS_RATE: - r_page_setup[offset] = value; - sbus1_set_output_rate_hz(value); - break; - case PX4IO_P_SETUP_THERMAL: - case PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION: r_page_setup[offset] = value; break; @@ -489,15 +303,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; - case PX4IO_PAGE_TEST: - switch (offset) { - case PX4IO_P_TEST_LED: - LED_AMBER(value & 1); - break; - } - - break; - default: return -1; } @@ -525,8 +330,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_STATUS: /* PX4IO_P_STATUS_FREEMEM */ - /* XXX PX4IO_P_STATUS_CPULOAD */ - /* PX4IO_P_STATUS_FLAGS maintained externally */ /* PX4IO_P_STATUS_ALARMS maintained externally */ @@ -592,10 +395,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val SELECT_PAGE(r_page_config); break; - case PX4IO_PAGE_SERVOS: - SELECT_PAGE(r_page_servos); - break; - case PX4IO_PAGE_RAW_RC_INPUT: SELECT_PAGE(r_page_raw_rc_input); break; @@ -609,14 +408,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val SELECT_PAGE(r_page_direct_pwm); break; - case PX4IO_PAGE_FAILSAFE_PWM: - SELECT_PAGE(r_page_servo_failsafe); - break; - - case PX4IO_PAGE_DISARMED_PWM: - SELECT_PAGE(r_page_servo_disarmed); - break; - default: return -1; } @@ -638,51 +429,3 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val return 0; } - -/* - * Helper function to handle changes to the PWM rate control registers. - */ -static void -pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) -{ - for (unsigned pass = 0; pass < 2; pass++) { - for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) { - - /* get the channel mask for this rate group */ - uint32_t mask = up_pwm_servo_get_rate_group(group); - - if (mask == 0) { - continue; - } - - /* all channels in the group must be either default or alt-rate */ - uint32_t alt = map & mask; - - if (pass == 0) { - /* preflight */ - if ((alt != 0) && (alt != mask)) { - /* not a legal map, bail with an alarm */ - r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; - return; - } - - } else { - /* set it - errors here are unexpected */ - if (alt != 0) { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) { - r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; - } - - } else { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) { - r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; - } - } - } - } - } - - r_setup_pwm_rates = map; - r_setup_pwm_defaultrate = defaultrate; - r_setup_pwm_altrate = altrate; -} diff --git a/src/modules/px4iofirmware/safety_button.cpp b/src/modules/px4iofirmware/safety_button.cpp index 6d4672d5f2..66d871eea9 100644 --- a/src/modules/px4iofirmware/safety_button.cpp +++ b/src/modules/px4iofirmware/safety_button.cpp @@ -136,12 +136,6 @@ safety_button_check(void *arg) static void failsafe_blink(void *arg) { - /* indicate that a serious initialisation error occured */ - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { - LED_AMBER(true); - return; - } - static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ diff --git a/src/modules/px4iofirmware/serial.cpp b/src/modules/px4iofirmware/serial.cpp index 9829fc2907..bd400a6e66 100644 --- a/src/modules/px4iofirmware/serial.cpp +++ b/src/modules/px4iofirmware/serial.cpp @@ -55,20 +55,6 @@ //#define DEBUG #include "px4io.h" -#if defined(PX4IO_PERF) -# include - -static perf_counter_t pc_txns; -static perf_counter_t pc_errors; -static perf_counter_t pc_ore; -static perf_counter_t pc_fe; -static perf_counter_t pc_ne; -static perf_counter_t pc_idle; -static perf_counter_t pc_badidle; -static perf_counter_t pc_regerr; -static perf_counter_t pc_crcerr; -#endif - static void rx_handle_packet(void); static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); static DMA_HANDLE tx_dma; @@ -92,18 +78,6 @@ static struct IOPacket dma_packet; void interface_init(void) { -#if defined(PX4IO_PERF) - pc_txns = perf_alloc(PC_ELAPSED, "txns"); - pc_errors = perf_alloc(PC_COUNT, "errors"); - pc_ore = perf_alloc(PC_COUNT, "overrun"); - pc_fe = perf_alloc(PC_COUNT, "framing"); - pc_ne = perf_alloc(PC_COUNT, "noise"); - pc_idle = perf_alloc(PC_COUNT, "idle"); - pc_badidle = perf_alloc(PC_COUNT, "badidle"); - pc_regerr = perf_alloc(PC_COUNT, "regerr"); - pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); -#endif - /* allocate DMA */ tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); @@ -165,10 +139,6 @@ rx_handle_packet(void) dma_packet.crc = 0; if (crc != crc_packet(&dma_packet)) { -#if defined(PX4IO_PERF) - perf_count(pc_crcerr); -#endif - /* send a CRC error reply */ dma_packet.count_code = PKT_CODE_CORRUPT; dma_packet.page = 0xff; @@ -181,10 +151,6 @@ rx_handle_packet(void) /* it's a blind write - pass it on */ if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { -#if defined(PX4IO_PERF) - perf_count(pc_regerr); -#endif - dma_packet.count_code = PKT_CODE_ERROR; } else { @@ -201,10 +167,6 @@ rx_handle_packet(void) uint16_t *registers; if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { -#if defined(PX4IO_PERF) - perf_count(pc_regerr); -#endif - dma_packet.count_code = PKT_CODE_ERROR; } else { @@ -238,9 +200,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) * We are here because DMA completed, or UART reception stopped and * we think we have a packet in the buffer. */ -#if defined(PX4IO_PERF) - perf_begin(pc_txns); -#endif /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); @@ -265,10 +224,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) DMA_CCR_MSIZE_8BITS); stm32_dmastart(tx_dma, NULL, NULL, false); rCR3 |= USART_CR3_DMAT; - -#if defined(PX4IO_PERF) - perf_end(pc_txns); -#endif } static int @@ -283,23 +238,6 @@ serial_interrupt(int irq, void *context, FAR void *arg) USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ -#if defined(PX4IO_PERF) - perf_count(pc_errors); - - if (sr & USART_SR_ORE) { - perf_count(pc_ore); - } - - if (sr & USART_SR_NE) { - perf_count(pc_ne); - } - - if (sr & USART_SR_FE) { - perf_count(pc_fe); - } - -#endif - /* send a line break - this will abort transmission/reception on the other end */ rCR1 |= USART_CR1_SBK; @@ -331,9 +269,6 @@ serial_interrupt(int irq, void *context, FAR void *arg) if ((length < 1) || (length < PKT_SIZE(dma_packet))) { /* it was too short - possibly truncated */ -#if defined(PX4IO_PERF) - perf_count(pc_badidle); -#endif dma_reset(); return 0; } @@ -342,9 +277,6 @@ serial_interrupt(int irq, void *context, FAR void *arg) * Looks like we received a packet. Stop the DMA and go process the * packet. */ -#if defined(PX4IO_PERF) - perf_count(pc_idle); -#endif stm32_dmastop(rx_dma); rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); }