dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

px4io: moving mixing to FMU side 

Using mixers on the IO side had a remote benefit of being able to
override all control surfaces with a radio remote on a fixed wing.
This ended up not being used that much and since the original design
10 years ago (2011) we have been able to convince ourselves that the
overall system stability is at a level where this marginal benefit,
which is not present on multicopters, is not worth the hazzle.

Co-authored-by: Beat Küng <beat-kueng@gmx.net>
Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Daniel Agar 2021-08-16 13:10:52 -04:00
parent f772b0f45d
commit 089c962d92
59 changed files with 1150 additions and 4170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -120,8 +120,6 @@ then
param set SENS_BOARD_X_OFF 0.000001 param set SENS_BOARD_X_OFF 0.000001
param set SENS_DPRES_OFF 0.001 param set SENS_DPRES_OFF 0.001
param set SYS_RESTART_TYPE 2
fi fi
param set-default BAT1_N_CELLS 4 param set-default BAT1_N_CELLS 4

1
ROMFS/px4fmu_common/init.d/CMakeLists.txt
View File

@ -42,7 +42,6 @@ px4_add_romfs_files(
rc.fw_apps rc.fw_apps
rc.fw_defaults rc.fw_defaults
rc.interface rc.interface
rc.io
rc.logging rc.logging
rc.mc_apps rc.mc_apps
rc.mc_defaults rc.mc_defaults

1
ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
View File

@ -56,7 +56,6 @@ param set-default RTL_DESCEND_ALT 10
param set-default RTL_RETURN_ALT 30 param set-default RTL_RETURN_ALT 30
param set-default SDLOG_DIRS_MAX 7 param set-default SDLOG_DIRS_MAX 7
param set-default SYS_RESTART_TYPE 2
param set-default VT_F_TRANS_THR 0.75 param set-default VT_F_TRANS_THR 0.75
param set-default VT_MOT_ID 1234 param set-default VT_MOT_ID 1234

1
ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo
View File

@ -48,7 +48,6 @@ param set-default CBRK_IO_SAFETY 22027
param set-default CBRK_SUPPLY_CHK 894281 param set-default CBRK_SUPPLY_CHK 894281
# RC configuration # RC configuration
param set-default RC_MAP_MODE_SW 5
param set-default RC_MAP_PITCH 2 param set-default RC_MAP_PITCH 2
param set-default RC_MAP_ROLL 1 param set-default RC_MAP_ROLL 1
param set-default RC_MAP_THROTTLE 3 param set-default RC_MAP_THROTTLE 3

30
ROMFS/px4fmu_common/init.d/rc.interface
View File

@ -17,6 +17,10 @@ set OUTPUT_DEV none
set OUTPUT_AUX_DEV /dev/pwm_output1 set OUTPUT_AUX_DEV /dev/pwm_output1
set OUTPUT_EXTRA_DEV /dev/pwm_output0 set OUTPUT_EXTRA_DEV /dev/pwm_output0
# set these before starting the modules
param set PWM_AUX_OUT ${PWM_AUX_OUT}
param set PWM_MAIN_OUT ${PWM_OUT}
# #
# If mount (gimbal) control is enabled and output mode is AUX, set the aux # If mount (gimbal) control is enabled and output mode is AUX, set the aux
# mixer to mount (override the airframe-specific MIXER_AUX setting). # mixer to mount (override the airframe-specific MIXER_AUX setting).
@ -78,9 +82,16 @@ then
fi fi
fi fi
if [ $OUTPUT_MODE = io ] #
# Start IO for PWM output or RC input if needed.
#
if [ $IO_PRESENT = yes ]
then then
. ${R}etc/init.d/rc.io if ! px4io start
then
echo "PX4IO start failed"
tune_control play -t 18 # PROG_PX4IO_ERR
fi
fi fi
if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ] if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ]
@ -91,17 +102,6 @@ then
tune_control play error tune_control play error
fi fi
fi fi
#
# Start IO for RC input if needed.
#
if [ $IO_PRESENT = yes ]
then
if [ $OUTPUT_MODE != io ]
then
. ${R}etc/init.d/rc.io
fi
fi
fi fi
if [ $MIXER != none -a $MIXER != skip ] if [ $MIXER != none -a $MIXER != skip ]
@ -212,8 +212,6 @@ then
fi fi
fi fi
param set PWM_AUX_OUT ${PWM_AUX_OUT}
if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ] if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ]
then then
if [ $PWM_OUT != none ] if [ $PWM_OUT != none ]
@ -226,8 +224,6 @@ then
fi fi
fi fi
param set PWM_MAIN_OUT ${PWM_OUT}
if [ $EXTRA_MIXER_MODE != none ] if [ $EXTRA_MIXER_MODE != none ]
then then

23
ROMFS/px4fmu_common/init.d/rc.io
View File

@ -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

17
ROMFS/px4fmu_test/init.d/rcS
View File

@ -70,14 +70,6 @@ then
set io_file /etc/extras/px4_io-v2_default.bin set io_file /etc/extras/px4_io-v2_default.bin
fi fi
if px4io start
then
echo "PX4IO OK"
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} px4io_start"
fi
if px4io checkcrc $io_file if px4io checkcrc $io_file
then then
echo "PX4IO CRC OK" echo "PX4IO CRC OK"
@ -104,6 +96,15 @@ else
fi fi
fi fi
if px4io start
then
echo "PX4IO OK"
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} px4io_start"
fi
# #
# The presence of this file suggests we're running a mount stress test # The presence of this file suggests we're running a mount stress test
# #

3
boards/bitcraze/crazyflie/syslink/syslink_main.cpp
View File

@ -53,7 +53,6 @@
#include <fcntl.h> #include <fcntl.h>
#include <mqueue.h> #include <mqueue.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_board_led.h> #include <drivers/drv_board_led.h>
#include <systemlib/err.h> #include <systemlib/err.h>
@ -96,7 +95,7 @@ Syslink::Syslink() :
_fd(0), _fd(0),
_queue(2, sizeof(syslink_message_t)), _queue(2, sizeof(syslink_message_t)),
_writebuffer(16, sizeof(crtp_message_t)), _writebuffer(16, sizeof(crtp_message_t)),
_rssi(RC_INPUT_RSSI_MAX), _rssi(input_rc_s::RSSI_MAX),
_bstate(BAT_DISCHARGING) _bstate(BAT_DISCHARGING)
{ {
px4_sem_init(&memory_sem, 0, 0); px4_sem_init(&memory_sem, 0, 0);

1
msg/input_rc.msg
View File

@ -23,6 +23,7 @@ uint64 timestamp_last_signal # last valid reception time
uint8 channel_count # number of channels actually being seen 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 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. 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.

7
msg/manual_control_switches.msg
View File

@ -26,11 +26,4 @@ uint8 kill_switch # throttle kill: _NORMAL_, KILL
uint8 gear_switch # landing gear switch: _DOWN_, UP uint8 gear_switch # landing gear switch: _DOWN_, UP
uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT
# legacy "advanced" switch configuration (will be removed soon)
uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL
uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO
uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED
uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL
uint32 switch_changes # number of switch changes uint32 switch_changes # number of switch changes

45
msg/px4io_status.msg
View File

@ -6,49 +6,38 @@ float32 voltage_v # Servo rail voltage in volts
float32 rssi_v # RSSI pin voltage in volts float32 rssi_v # RSSI pin voltage in volts
# PX4IO status flags (PX4IO_P_STATUS_FLAGS) # 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_arm_sync
bool status_init_ok
bool status_failsafe bool status_failsafe
bool status_safety_off
bool status_fmu_initialized 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_st24
bool status_rc_sumd bool status_rc_sumd
bool status_safety_off
# PX4IO alarms (PX4IO_P_STATUS_ALARMS) # 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_pwm_error
bool alarm_vservo_fault bool alarm_rc_lost
# PX4IO arming (PX4IO_P_SETUP_ARMING) # PX4IO arming (PX4IO_P_SETUP_ARMING)
bool arming_io_arm_ok bool arming_failsafe_custom
bool arming_fmu_armed bool arming_fmu_armed
bool arming_fmu_prearmed 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_force_failsafe
bool arming_io_arm_ok
bool arming_lockdown
bool arming_termination_failsafe bool arming_termination_failsafe
bool arming_override_immediate
uint16[8] pwm
uint16[8] pwm_disarmed
uint16[8] pwm_failsafe
int16[8] actuators uint16[8] pwm_rate_hz
uint16[8] servos
uint16[18] raw_inputs uint16[18] raw_inputs

53
msg/rc_channels.msg
View File

@ -4,41 +4,36 @@ uint8 FUNCTION_THROTTLE = 0
uint8 FUNCTION_ROLL = 1 uint8 FUNCTION_ROLL = 1
uint8 FUNCTION_PITCH = 2 uint8 FUNCTION_PITCH = 2
uint8 FUNCTION_YAW = 3 uint8 FUNCTION_YAW = 3
uint8 FUNCTION_MODE = 4 uint8 FUNCTION_RETURN = 4
uint8 FUNCTION_RETURN = 5 uint8 FUNCTION_LOITER = 5
uint8 FUNCTION_POSCTL = 6 uint8 FUNCTION_OFFBOARD = 6
uint8 FUNCTION_LOITER = 7 uint8 FUNCTION_FLAPS = 7
uint8 FUNCTION_OFFBOARD = 8 uint8 FUNCTION_AUX_1 = 8
uint8 FUNCTION_ACRO = 9 uint8 FUNCTION_AUX_2 = 9
uint8 FUNCTION_FLAPS = 10 uint8 FUNCTION_AUX_3 = 10
uint8 FUNCTION_AUX_1 = 11 uint8 FUNCTION_AUX_4 = 11
uint8 FUNCTION_AUX_2 = 12 uint8 FUNCTION_AUX_5 = 12
uint8 FUNCTION_AUX_3 = 13 uint8 FUNCTION_AUX_6 = 13
uint8 FUNCTION_AUX_4 = 14 uint8 FUNCTION_PARAM_1 = 14
uint8 FUNCTION_AUX_5 = 15 uint8 FUNCTION_PARAM_2 = 15
uint8 FUNCTION_PARAM_1 = 16 uint8 FUNCTION_PARAM_3_5 = 16
uint8 FUNCTION_PARAM_2 = 17 uint8 FUNCTION_KILLSWITCH = 17
uint8 FUNCTION_PARAM_3_5 = 18 uint8 FUNCTION_TRANSITION = 18
uint8 FUNCTION_KILLSWITCH = 19 uint8 FUNCTION_GEAR = 19
uint8 FUNCTION_TRANSITION = 20 uint8 FUNCTION_ARMSWITCH = 20
uint8 FUNCTION_GEAR = 21 uint8 FUNCTION_FLTBTN_SLOT_1 = 21
uint8 FUNCTION_ARMSWITCH = 22 uint8 FUNCTION_FLTBTN_SLOT_2 = 22
uint8 FUNCTION_STAB = 23 uint8 FUNCTION_FLTBTN_SLOT_3 = 23
uint8 FUNCTION_AUX_6 = 24 uint8 FUNCTION_FLTBTN_SLOT_4 = 24
uint8 FUNCTION_MAN = 25 uint8 FUNCTION_FLTBTN_SLOT_5 = 25
uint8 FUNCTION_FLTBTN_SLOT_1 = 26 uint8 FUNCTION_FLTBTN_SLOT_6 = 26
uint8 FUNCTION_FLTBTN_SLOT_2 = 27
uint8 FUNCTION_FLTBTN_SLOT_3 = 28
uint8 FUNCTION_FLTBTN_SLOT_4 = 39
uint8 FUNCTION_FLTBTN_SLOT_5 = 30
uint8 FUNCTION_FLTBTN_SLOT_6 = 31
uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6 uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6
uint64 timestamp_last_valid # Timestamp of last valid RC signal uint64 timestamp_last_valid # Timestamp of last valid RC signal
float32[18] channels # Scaled to -1..1 (throttle: 0..1) float32[18] channels # Scaled to -1..1 (throttle: 0..1)
uint8 channel_count # Number of valid channels uint8 channel_count # Number of valid channels
int8[32] function # Functions mapping int8[27] function # Functions mapping
uint8 rssi # Receive signal strength index uint8 rssi # Receive signal strength index
bool signal_lost # Control signal lost, should be checked together with topic timeout bool signal_lost # Control signal lost, should be checked together with topic timeout
uint32 frame_drop_count # Number of dropped frames uint32 frame_drop_count # Number of dropped frames

2
msg/safety.msg
View File

@ -1,5 +1,3 @@
uint64 timestamp # time since system start (microseconds) uint64 timestamp # time since system start (microseconds)
bool safety_switch_available # Set to true if a safety switch is connected bool safety_switch_available # Set to true if a safety switch is connected
bool safety_off # Set to true if safety is off 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

1
msg/vehicle_control_mode.msg
View File

@ -1,7 +1,6 @@
uint64 timestamp # time since system start (microseconds) uint64 timestamp # time since system start (microseconds)
bool flag_armed # synonym for actuator_armed.armed bool flag_armed # synonym for actuator_armed.armed
bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing
bool flag_multicopter_position_control_enabled bool flag_multicopter_position_control_enabled
bool flag_control_manual_enabled # true if manual input is mixed in bool flag_control_manual_enabled # true if manual input is mixed in

22
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -77,16 +77,16 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};
static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19};
static constexpr wq_config_t UART0{"wq:UART0", 1536, -21}; static constexpr wq_config_t UART0{"wq:UART0", 1632, -21};
static constexpr wq_config_t UART1{"wq:UART1", 1536, -22}; static constexpr wq_config_t UART1{"wq:UART1", 1632, -22};
static constexpr wq_config_t UART2{"wq:UART2", 1536, -23}; static constexpr wq_config_t UART2{"wq:UART2", 1632, -23};
static constexpr wq_config_t UART3{"wq:UART3", 1536, -24}; static constexpr wq_config_t UART3{"wq:UART3", 1632, -24};
static constexpr wq_config_t UART4{"wq:UART4", 1536, -25}; static constexpr wq_config_t UART4{"wq:UART4", 1632, -25};
static constexpr wq_config_t UART5{"wq:UART5", 1536, -26}; static constexpr wq_config_t UART5{"wq:UART5", 1632, -26};
static constexpr wq_config_t UART6{"wq:UART6", 1536, -27}; static constexpr wq_config_t UART6{"wq:UART6", 1632, -27};
static constexpr wq_config_t UART7{"wq:UART7", 1536, -28}; static constexpr wq_config_t UART7{"wq:UART7", 1632, -28};
static constexpr wq_config_t UART8{"wq:UART8", 1536, -29}; static constexpr wq_config_t UART8{"wq:UART8", 1632, -29};
static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1536, -30}; static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1632, -30};
static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50}; static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};

1
platforms/posix/cmake/sitl_tests.cmake
View File

@ -8,7 +8,6 @@ set(tests
bezier bezier
bitset bitset
bson bson
conv
dataman dataman
file2 file2
float float

1
posix-configs/SITL/init/test/cmd_template.in
View File

@ -5,7 +5,6 @@
param load param load
param set CBRK_SUPPLY_CHK 894281 param set CBRK_SUPPLY_CHK 894281
param set SYS_RESTART_TYPE 0
dataman start dataman start

1
posix-configs/SITL/init/test/test_imu_filtering
View File

@ -5,7 +5,6 @@
param load param load
param set CBRK_SUPPLY_CHK 894281 param set CBRK_SUPPLY_CHK 894281
param set SYS_RESTART_TYPE 0
dataman start dataman start

1
posix-configs/SITL/init/test/test_mavlink
View File

@ -5,7 +5,6 @@
param load param load
param set CBRK_SUPPLY_CHK 894281 param set CBRK_SUPPLY_CHK 894281
param set SYS_RESTART_TYPE 0
dataman start dataman start

1
posix-configs/SITL/init/test/test_shutdown
View File

@ -9,7 +9,6 @@ param set BAT1_N_CELLS 3
param set CBRK_SUPPLY_CHK 894281 param set CBRK_SUPPLY_CHK 894281
param set MAV_TYPE 22 param set MAV_TYPE 22
param set VT_TYPE 2 param set VT_TYPE 2
param set SYS_RESTART_TYPE 0
dataman start dataman start

1
posix-configs/SITL/init/test/tests_all
View File

@ -5,7 +5,6 @@
param load param load
param set CBRK_SUPPLY_CHK 894281 param set CBRK_SUPPLY_CHK 894281
param set SYS_RESTART_TYPE 0
dataman start dataman start

3
src/drivers/drv_pwm_output.h
View File

@ -215,9 +215,6 @@ typedef uint16_t servo_position_t;
/** force safety switch on (to enable use of safety switch) */ /** force safety switch on (to enable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 28) #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 */ /** set auxillary output mode */
#define PWM_SERVO_ENTER_TEST_MODE 18 #define PWM_SERVO_ENTER_TEST_MODE 18
#define PWM_SERVO_EXIT_TEST_MODE 19 #define PWM_SERVO_EXIT_TEST_MODE 19

80
src/drivers/drv_rc_input.h
View File

@ -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 <stdint.h>
#include <sys/ioctl.h>
#include <uORB/topics/input_rc.h>
#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 */

2
src/drivers/px4io/CMakeLists.txt
View File

@ -42,7 +42,7 @@ px4_add_module(
DEPENDS DEPENDS
arch_px4io_serial arch_px4io_serial
circuit_breaker circuit_breaker
mixer mixer_module
) )
# include the px4io binary in ROMFS # include the px4io binary in ROMFS

2467
src/drivers/px4io/px4io.cpp
View File

File diff suppressed because it is too large Load Diff

7
src/drivers/px4io/px4io_params.c
View File

@ -45,11 +45,10 @@
/** /**
* Set usage of IO board * Set usage of IO board
* *
* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. * Can be used to use a configure the use of the IO board.
* *
* @boolean * @value 0 IO PWM disabled (RC only)
* @min 0 * @value 1 IO enabled (RC & PWM)
* @max 1
* @reboot_required true * @reboot_required true
* @group System * @group System
*/ */

26
src/drivers/px4io/px4io_serial.cpp
View File

@ -51,24 +51,14 @@ device::Device
PX4IO_serial::PX4IO_serial() : PX4IO_serial::PX4IO_serial() :
Device("PX4IO_serial"), Device("PX4IO_serial"),
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns")), _pc_txns(perf_alloc(PC_ELAPSED, MODULE_NAME": txns")),
#if 0 _pc_retries(perf_alloc(PC_COUNT, MODULE_NAME": retries")),
_pc_retries(perf_alloc(PC_COUNT, "io_retries ")), _pc_timeouts(perf_alloc(PC_COUNT, MODULE_NAME": timeouts")),
_pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), _pc_crcerrs(perf_alloc(PC_COUNT, MODULE_NAME": crcerrs")),
_pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")), _pc_protoerrs(perf_alloc(PC_COUNT, MODULE_NAME": protoerrs")),
_pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")), _pc_uerrs(perf_alloc(PC_COUNT, MODULE_NAME": uarterrs")),
_pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")), _pc_idle(perf_alloc(PC_COUNT, MODULE_NAME": idle")),
_pc_idle(perf_alloc(PC_COUNT, "io_idle ")), _pc_badidle(perf_alloc(PC_COUNT, MODULE_NAME": badidle")),
_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
_bus_semaphore(SEM_INITIALIZER(0)) _bus_semaphore(SEM_INITIALIZER(0))
{ {
g_interface = this; g_interface = this;

8
src/drivers/px4io/px4io_uploader.cpp
View File

@ -355,9 +355,8 @@ int
PX4IO_Uploader::get_sync(unsigned timeout) PX4IO_Uploader::get_sync(unsigned timeout)
{ {
uint8_t c[2]; uint8_t c[2];
int ret;
ret = recv_byte_with_timeout(c, timeout); int ret = recv_byte_with_timeout(c, timeout);
if (ret != OK) { if (ret != OK) {
return ret; return ret;
@ -395,13 +394,11 @@ PX4IO_Uploader::sync()
int int
PX4IO_Uploader::get_info(int param, uint32_t &val) PX4IO_Uploader::get_info(int param, uint32_t &val)
{ {
int ret;
send(PROTO_GET_DEVICE); send(PROTO_GET_DEVICE);
send(param); send(param);
send(PROTO_EOC); send(PROTO_EOC);
ret = recv_bytes((uint8_t *)&val, sizeof(val)); int ret = recv_bytes((uint8_t *)&val, sizeof(val));
if (ret != OK) { if (ret != OK) {
return ret; return ret;
@ -419,7 +416,6 @@ PX4IO_Uploader::erase()
return get_sync(10000); /* allow 10s timeout */ return get_sync(10000); /* allow 10s timeout */
} }
static int read_with_retry(int fd, void *buf, size_t n) static int read_with_retry(int fd, void *buf, size_t n)
{ {
int ret; int ret;

4
src/drivers/rc_input/RCInput.cpp
View File

@ -509,7 +509,7 @@ void RCInput::Run()
for (unsigned i = 0; i < (unsigned)newBytes; i++) { for (unsigned i = 0; i < (unsigned)newBytes; i++) {
/* set updated flag if one complete packet was parsed */ /* 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, 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)); &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
} }
@ -561,7 +561,7 @@ void RCInput::Run()
for (unsigned i = 0; i < (unsigned)newBytes; i++) { for (unsigned i = 0; i < (unsigned)newBytes; i++) {
/* set updated flag if one complete packet was parsed */ /* 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, 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)); &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
} }

1
src/drivers/rc_input/RCInput.hpp
View File

@ -38,7 +38,6 @@
#include <board_config.h> #include <board_config.h>
#include <drivers/drv_adc.h> #include <drivers/drv_adc.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <lib/rc/crsf.h> #include <lib/rc/crsf.h>
#include <lib/rc/ghst.hpp> #include <lib/rc/ghst.hpp>

1
src/drivers/snapdragon_spektrum_rc/spektrum_rc.cpp
View File

@ -46,7 +46,6 @@
#include <px4_platform_common/getopt.h> #include <px4_platform_common/getopt.h>
#include <lib/rc/dsm.h> #include <lib/rc/dsm.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>

2
src/lib/mixer_module/CMakeLists.txt
View File

@ -32,5 +32,5 @@
############################################################################ ############################################################################
px4_add_library(mixer_module mixer_module.cpp) px4_add_library(mixer_module mixer_module.cpp)
target_link_libraries(mixer_module PRIVATE output_limit)
target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL})

6
src/lib/mixer_module/mixer_module.cpp
View File

@ -89,7 +89,11 @@ MixingOutput::~MixingOutput()
void MixingOutput::printStatus() const void MixingOutput::printStatus() const
{ {
perf_print_counter(_control_latency_perf); 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("Mixer loaded: %s", _mixers ? "yes" : "no");
PX4_INFO("Driver instance: %i", _driver_instance); PX4_INFO("Driver instance: %i", _driver_instance);

12
src/lib/rc/sbus.cpp
View File

@ -50,6 +50,7 @@
#include "sbus.h" #include "sbus.h"
#include "common_rc.h" #include "common_rc.h"
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
using namespace time_literals; 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) void sbus1_set_output_rate_hz(uint16_t rate_hz)
{ {
if (rate_hz > SBUS1_MAX_RATE_HZ) { sbus1_frame_delay = (1000U * 1000U) / math::constrain(rate_hz, (uint16_t)SBUS1_MIN_RATE_HZ,
rate_hz = SBUS1_MAX_RATE_HZ; (uint16_t)SBUS1_MAX_RATE_HZ);
}
if (rate_hz < SBUS1_MIN_RATE_HZ) {
rate_hz = SBUS1_MIN_RATE_HZ;
}
sbus1_frame_delay = (1000U * 1000U) / rate_hz;
} }

16
src/lib/systemlib/system_params.c
View File

@ -83,22 +83,6 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
*/ */
PARAM_DEFINE_INT32(SYS_HITL, 0); PARAM_DEFINE_INT32(SYS_HITL, 0);
/**
* Set restart type
*
* Set by px4io to indicate type of restart
*
* @min 0
* @max 2
* @value 0 Data survives resets
* @value 1 Data survives in-flight resets only
* @value 2 Data does not survive reset
* @category system
* @volatile
* @group System
*/
PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
/** /**
* Set multicopter estimator group * Set multicopter estimator group
* *

119
src/modules/commander/Commander.cpp
View File

@ -2467,7 +2467,7 @@ Commander::run()
} }
// evaluate the main state machine according to mode switches // 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 // play tune on mode change only if armed, blink LED always
tune_positive(_armed.armed); tune_positive(_armed.armed);
_status_changed = true; _status_changed = true;
@ -3074,29 +3074,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
_leds_counter++; _leds_counter++;
} }
transition_result_t transition_result_t Commander::set_main_state()
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()
{ {
if ((_manual_control_switches.timestamp == 0) if ((_manual_control_switches.timestamp == 0)
|| (_manual_control_switches.timestamp == _last_manual_control_switches.timestamp)) { || (_manual_control_switches.timestamp == _last_manual_control_switches.timestamp)) {
@ -3113,14 +3091,8 @@ Commander::set_main_state_rc()
bool should_evaluate_rc_mode_switch = bool should_evaluate_rc_mode_switch =
(_last_manual_control_switches.offboard_switch != _manual_control_switches.offboard_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.return_switch != _manual_control_switches.return_switch)
|| (_last_manual_control_switches.mode_switch != _manual_control_switches.mode_switch)
|| (_last_manual_control_switches.acro_switch != _manual_control_switches.acro_switch)
|| (_last_manual_control_switches.posctl_switch != _manual_control_switches.posctl_switch)
|| (_last_manual_control_switches.loiter_switch != _manual_control_switches.loiter_switch) || (_last_manual_control_switches.loiter_switch != _manual_control_switches.loiter_switch)
|| (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot) || (_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);
if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// if already armed don't evaluate first time RC // if already armed don't evaluate first time RC
@ -3216,84 +3188,6 @@ Commander::set_main_state_rc()
} }
return res; return res;
} else if (_manual_control_switches.mode_switch != manual_control_switches_s::SWITCH_POS_NONE) {
/* offboard and RTL switches off or denied, check main mode switch */
switch (_manual_control_switches.mode_switch) {
case manual_control_switches_s::SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
break;
case manual_control_switches_s::SWITCH_POS_OFF: // MANUAL
if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_NONE &&
_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
/*
* Legacy mode:
* Acro switch being used as stabilized switch in FW.
*/
if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
/* manual mode is stabilized already for multirotors, so switch to acro
* for any non-manual mode
*/
if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_status.is_vtol) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state);
} else if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
} else {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
}
} else {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
}
} else {
/* New mode:
* - Acro is Acro
* - Manual is not default anymore when the manual switch is assigned
*/
if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
} else if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state);
} else if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
} else if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
// default to MANUAL when no manual switch is set
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
} else {
// default to STAB when the manual switch is assigned (but off)
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
}
}
// TRANSITION_DENIED is not possible here
break;
case manual_control_switches_s::SWITCH_POS_MIDDLE: // ASSIST
if (_manual_control_switches.posctl_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = try_mode_change(commander_state_s::MAIN_STATE_POSCTL);
} else {
res = try_mode_change(commander_state_s::MAIN_STATE_ALTCTL);
}
break;
case manual_control_switches_s::SWITCH_POS_ON: // AUTO
res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_MISSION);
break;
default:
break;
}
} }
return res; return res;
@ -3374,9 +3268,6 @@ Commander::update_control_mode()
/* set vehicle_control_mode according to set_navigation_state */ /* set vehicle_control_mode according to set_navigation_state */
_vehicle_control_mode.flag_armed = _armed.armed; _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) { switch (_status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_MANUAL:
_vehicle_control_mode.flag_control_manual_enabled = true; _vehicle_control_mode.flag_control_manual_enabled = true;
@ -3409,10 +3300,6 @@ Commander::update_control_mode()
break; break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: 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_FOLLOW_TARGET:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:

8
src/modules/commander/Commander.hpp
View File

@ -173,14 +173,8 @@ private:
void UpdateEstimateValidity(); 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 // 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(); bool shutdown_if_allowed();

13
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -40,10 +40,9 @@
* @author Thomas Gubler <thomas@px4.io> * @author Thomas Gubler <thomas@px4.io>
*/ */
#include <airspeed/airspeed.h> #include <lib/airspeed/airspeed.h>
#include <conversion/rotation.h> #include <lib/conversion/rotation.h>
#include <drivers/drv_rc_input.h> #include <lib/systemlib/px4_macros.h>
#include <systemlib/px4_macros.h>
#include <math.h> #include <math.h>
#include <poll.h> #include <poll.h>
@ -1889,7 +1888,7 @@ MavlinkReceiver::handle_message_rc_channels(mavlink_message_t *msg)
input_rc_s rc{}; input_rc_s rc{};
rc.timestamp_last_signal = hrt_absolute_time(); rc.timestamp_last_signal = hrt_absolute_time();
rc.rssi = RC_INPUT_RSSI_MAX; rc.rssi = input_rc_s::RSSI_MAX;
// TODO: fake RSSI from dropped messages? // TODO: fake RSSI from dropped messages?
// for (auto &component_state : _component_states) { // for (auto &component_state : _component_states) {
@ -1959,7 +1958,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
// metadata // metadata
rc.timestamp = hrt_absolute_time(); rc.timestamp = hrt_absolute_time();
rc.timestamp_last_signal = rc.timestamp; 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_failsafe = false;
rc.rc_lost = false; rc.rc_lost = false;
rc.rc_lost_frame_count = 0; rc.rc_lost_frame_count = 0;
@ -2031,7 +2030,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
rc.rc_total_frame_count = 1; rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0; rc.rc_ppm_frame_length = 0;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; 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[0] = man.x / 2 + 1500; // roll
rc.values[1] = man.y / 2 + 1500; // pitch rc.values[1] = man.y / 2 + 1500; // pitch

3
src/modules/mavlink/streams/MANUAL_CONTROL.hpp
View File

@ -78,11 +78,8 @@ private:
if (_manual_control_switches_sub.copy(&manual_control_switches)) { if (_manual_control_switches_sub.copy(&manual_control_switches)) {
unsigned shift = 2; unsigned shift = 2;
msg.buttons = 0; 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.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.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.offboard_switch << (shift * 5));
msg.buttons |= (manual_control_switches.kill_switch << (shift * 6)); msg.buttons |= (manual_control_switches.kill_switch << (shift * 6));
} }

2
src/modules/px4iofirmware/CMakeLists.txt
View File

@ -51,9 +51,7 @@ target_link_libraries(px4iofirmware
nuttx_apps nuttx_apps
nuttx_arch nuttx_arch
nuttx_c nuttx_c
mixer
rc rc
output_limit
) )
if(PX4IO_PERF) if(PX4IO_PERF)

208
src/modules/px4iofirmware/controls.c
View File

@ -43,12 +43,12 @@
#include <stdbool.h> #include <stdbool.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <systemlib/ppm_decode.h> #include <systemlib/ppm_decode.h>
#include <rc/st24.h> #include <rc/st24.h>
#include <rc/sumd.h> #include <rc/sumd.h>
#include <rc/sbus.h> #include <rc/sbus.h>
#include <rc/dsm.h> #include <rc/dsm.h>
#include <uORB/topics/input_rc.h>
#if defined(PX4IO_PERF) #if defined(PX4IO_PERF)
# include <perf/perf_counter.h> # include <perf/perf_counter.h>
@ -56,9 +56,6 @@
#include "px4io.h" #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 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); 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; static int _dsm_fd = -1;
int _sbus_fd = -1; int _sbus_fd = -1;
static uint16_t rc_value_override = 0;
#ifdef ADC_RSSI #ifdef ADC_RSSI
static unsigned _rssi_adc_counts = 0; static unsigned _rssi_adc_counts = 0;
#endif #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 */ /* Note: this is static because RC-provided telemetry does not occur every tick */
static uint16_t _rssi = 0; static uint16_t _rssi = 0;
static unsigned _frame_drops = 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))) { if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SUMD))) {
for (unsigned i = 0; i < n_bytes; i++) { for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */ /* 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_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count,
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); &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))) { if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24))) {
for (unsigned i = 0; i < n_bytes; i++) { for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */ /* 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_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)); &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state));
} }
@ -204,19 +199,6 @@ controls_init(void)
/* S.bus input (USART3) */ /* S.bus input (USART3) */
_sbus_fd = sbus_init("/dev/ttyS2", false); _sbus_fd = sbus_init("/dev/ttyS2", false);
/* default to a 1:1 input map, all enabled */
for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
#if defined(PX4IO_PERF) #if defined(PX4IO_PERF)
c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm"); c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus"); c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
@ -246,11 +228,11 @@ controls_tick()
_rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f); _rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f);
/* use 1:1 scaling on 3.3V, 12-Bit ADC input */ /* use 1:1 scaling on 3.3V, 12-Bit ADC input */
unsigned mV = _rssi_adc_counts * 3300 / 4095; unsigned mV = _rssi_adc_counts * 3300 / 4095;
/* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */ /* scale to 0..100 (input_rc_s::RSSI_MAX == 100) */
_rssi = (mV * RC_INPUT_RSSI_MAX / 3300); _rssi = (mV * INPUT_RC_RSSI_MAX / 3300);
if (_rssi > RC_INPUT_RSSI_MAX) { if (_rssi > INPUT_RC_RSSI_MAX) {
_rssi = RC_INPUT_RSSI_MAX; _rssi = INPUT_RC_RSSI_MAX;
} }
} }
} }
@ -276,11 +258,11 @@ controls_tick()
if (sbus_updated) { if (sbus_updated) {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS); 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) { if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_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 { } else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
@ -297,7 +279,6 @@ controls_tick()
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
_rssi = sbus_rssi; _rssi = sbus_rssi;
} }
} }
} }
@ -377,103 +358,6 @@ controls_tick()
/* update RC-received timestamp */ /* update RC-received timestamp */
system_state.rc_channels_timestamp_received = hrt_absolute_time(); system_state.rc_channels_timestamp_received = hrt_absolute_time();
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint16_t raw = r_raw_rc_values[i];
int16_t scaled;
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) {
raw = conf[PX4IO_P_RC_CONFIG_MIN];
}
if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) {
raw = conf[PX4IO_P_RC_CONFIG_MAX];
}
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
scaled = -scaled;
}
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) {
/* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
}
if (mapped == 3 && r_setup_rc_thr_failsafe) {
/* throttle failsafe detection */
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
} else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) {
/* pick out override channel, indicated by special mapping */
rc_value_override = SIGNED_TO_REG(scaled);
}
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i))) {
r_rc_values[i] = 0;
}
}
/* set RC OK flag, as we got an update */ /* set RC OK flag, as we got an update */
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK); atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK);
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK; r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK;
@ -485,11 +369,6 @@ controls_tick()
} else { } else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
} }
/*
* Export the valid channel bitmap
*/
r_rc_valid = assigned_channels;
} }
/* /*
@ -513,85 +392,20 @@ controls_tick()
* Handle losing RC input * 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) */ /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
if (rc_input_lost) { if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */ /* Clear the RC input status flag, clear manual override flag */
atomic_modify_clear(&r_status_flags, ( atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK);
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK));
/* flag raw RC as lost */ /* flag raw RC as lost */
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK); 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 */ /* Set raw channel count to zero */
r_raw_rc_count = 0; r_raw_rc_count = 0;
/* Set the RC_LOST alarm */ /* Set the RC_LOST alarm */
atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_RC_LOST); 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 static bool

456
src/modules/px4iofirmware/mixer.cpp
View File

@ -51,14 +51,8 @@
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <lib/mixer/MixerGroup.hpp>
#include <output_limit/output_limit.h>
#include <rc/sbus.h> #include <rc/sbus.h>
#include <uORB/topics/actuator_controls.h>
#include "mixer.h"
extern "C" { extern "C" {
/* #define DEBUG */ /* #define DEBUG */
#include "px4io.h" #include "px4io.h"
@ -74,7 +68,6 @@ static volatile bool mixer_servos_armed = false;
static volatile bool should_arm = false; static volatile bool should_arm = false;
static volatile bool should_arm_nothrottle = false; static volatile bool should_arm_nothrottle = false;
static volatile bool should_always_enable_pwm = false; static volatile bool should_always_enable_pwm = false;
static volatile bool mix_failsafe = false;
static bool new_fmu_data = false; static bool new_fmu_data = false;
static uint64_t last_fmu_update = 0; static uint64_t last_fmu_update = 0;
@ -85,31 +78,12 @@ extern int _sbus_fd;
enum mixer_source { enum mixer_source {
MIX_NONE, MIX_NONE,
MIX_DISARMED, MIX_DISARMED,
MIX_FMU,
MIX_OVERRIDE,
MIX_FAILSAFE, MIX_FAILSAFE,
MIX_OVERRIDE_FMU_OK
}; };
static volatile mixer_source source;
static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
static int mixer_handle_text_create_mixer();
static void mixer_mix_failsafe();
static MixerGroup mixer_group;
void void
mixer_tick() mixer_tick()
{ {
/* check if the mixer got modified */
mixer_handle_text_create_mixer();
if (mix_failsafe) {
mixer_mix_failsafe();
mix_failsafe = false;
}
/* check that we are receiving fresh data from the FMU */ /* check that we are receiving fresh data from the FMU */
irqstate_t irq_flags = enter_critical_section(); irqstate_t irq_flags = enter_critical_section();
const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time; const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time;
@ -123,8 +97,7 @@ mixer_tick()
isr_debug(1, "AP RX timeout"); isr_debug(1, "AP RX timeout");
} }
atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FMU_OK)); atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_FMU_LOST);
} else { } else {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK); atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
@ -139,7 +112,7 @@ mixer_tick()
} }
/* default to disarmed mixing */ /* default to disarmed mixing */
source = MIX_DISARMED; mixer_source source = MIX_DISARMED;
/* /*
* Decide which set of controls we're using. * Decide which set of controls we're using.
@ -149,41 +122,11 @@ mixer_tick()
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { (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 */ /* don't actually mix anything - copy values from r_page_direct_pwm */
source = MIX_NONE; source = MIX_NONE;
memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT); 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;
}
}
}
/* /*
* Decide whether the servos should be armed right now. * Decide whether the servos should be armed right now.
* *
@ -191,30 +134,22 @@ mixer_tick()
* FMU or from the mixer. * FMU or from the mixer.
* *
*/ */
should_arm = ( should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
(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_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_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_nothrottle = ( should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
(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_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_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 */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
) ));
);
should_always_enable_pwm = ( /* we enable PWM output always on the IO side if FMU is up and running
(r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) * as zero-outputs can be controlled by FMU by sending a 0 PWM command
&& (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 * Check if FMU is still alive, if not, terminate the flight
@ -243,100 +178,19 @@ mixer_tick()
atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE)); 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. * Run the mixers.
*/ */
if (source == MIX_FAILSAFE) { if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */ /* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[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) { } else if (source == MIX_DISARMED) {
/* copy disarmed values to the servo outputs */ /* copy disarmed values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_disarmed[i]; r_page_servos[i] = r_page_servo_disarmed[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
}
} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)
&& !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
/* maximum value the outputs of the multirotor mixer are allowed to change in this cycle
* factor 2 is needed because actuator outputs are in the range [-1,1]
*/
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max);
mixer_group.set_max_delta_out_once(delta_out_max);
}
/* set dt to be used in simple mixer for slew rate limiting */
mixer_group.set_dt_once(dt);
/* update parameter for mc thrust model if it updated */
if (update_mc_thrust_param) {
mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
update_mc_thrust_param = false;
}
/* mix */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
r_mixer_limits = mixer_group.get_saturation_status();
} else {
mixed = 0;
}
/* the pwm limit call takes care of out of band errors */
output_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
/* clamp unused outputs to zero */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = 0;
outputs[i] = 0.0f;
}
/* store normalized outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
if (mixed && new_fmu_data) {
new_fmu_data = false;
/* Trigger all timer's channels in Oneshot mode to fire
* the oneshots with updated values.
*/
up_pwm_update();
} }
} }
@ -372,7 +226,6 @@ mixer_tick()
} }
/* set S.BUS1 or S.BUS2 outputs */ /* set S.BUS1 or S.BUS2 outputs */
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
@ -380,8 +233,7 @@ mixer_tick()
sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
} }
} else if (mixer_servos_armed && (should_always_enable_pwm } else if (mixer_servos_armed && (should_always_enable_pwm || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
|| (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
/* set the disarmed servo outputs. */ /* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
up_pwm_servo_set(i, r_page_servo_disarmed[i]); up_pwm_servo_set(i, r_page_servo_disarmed[i]);
@ -399,283 +251,3 @@ mixer_tick()
} }
} }
} }
static int
mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &control)
{
control = 0.0f;
if (control_group >= PX4IO_CONTROL_GROUPS) {
return -1;
}
switch (source) {
case MIX_FMU:
if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
if (r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)] == INT16_MAX) {
//catch NAN values encoded as INT16 max for disarmed outputs
control = NAN;
} else {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
}
break;
}
return -1;
case MIX_OVERRIDE:
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
break;
}
return -1;
case MIX_OVERRIDE_FMU_OK:
/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
break;
} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
break;
}
return -1;
case MIX_DISARMED:
case MIX_FAILSAFE:
case MIX_NONE:
control = 0.0f;
return -1;
}
/* apply trim offsets for override channels */
if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_ROLL) {
control *= REG_TO_FLOAT(r_setup_scale_roll);
control += REG_TO_FLOAT(r_setup_trim_roll);
} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_PITCH) {
control *= REG_TO_FLOAT(r_setup_scale_pitch);
control += REG_TO_FLOAT(r_setup_trim_pitch);
} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_YAW) {
control *= REG_TO_FLOAT(r_setup_scale_yaw);
control += REG_TO_FLOAT(r_setup_trim_yaw);
}
}
/* limit output */
control = math::constrain(control, -1.f, 1.f);
/* motor spinup phase - lock throttle to zero */
if ((pwm_limit.state == OUTPUT_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* limit the throttle output to zero during motor spinup,
* as the motors cannot follow any demand yet
*/
control = 0.0f;
}
}
/* only safety off, but not armed - set throttle as invalid */
if (should_arm_nothrottle && !should_arm) {
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* mark the throttle as invalid */
control = NAN;
}
}
return 0;
}
/*
* XXX error handling here should be more aggressive; currently it is
* possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has
* not loaded faithfully.
*/
static char mixer_text[PX4IO_MAX_MIXER_LENGTH]; /* large enough for one mixer */
static unsigned mixer_text_length = 0;
static volatile bool mixer_update_pending = false;
static volatile bool mixer_reset_pending = false;
int
mixer_handle_text_create_mixer()
{
/* do not allow a mixer change while safety off and FMU armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
return 1;
}
if (mixer_reset_pending) {
mixer_group.reset();
mixer_reset_pending = false;
}
/* only run on update */
if (!mixer_update_pending || (mixer_text_length == 0)) {
return 0;
}
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(mixer_callback, 0, &mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0) {
memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
/* enforce null termination */
mixer_text[resid] = '\0';
}
mixer_text_length = resid;
}
mixer_update_pending = false;
update_trims = true;
update_mc_thrust_param = true;
return 0;
}
int interrupt_mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while safety off and FMU armed */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
return 1;
}
/* disable mixing, will be enabled once load is complete */
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK);
px4io_mixdata *msg = (px4io_mixdata *)buffer;
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata)) {
return 0;
}
unsigned text_length = length - sizeof(px4io_mixdata);
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
isr_debug(2, "reset");
/* THEN actually delete it */
mixer_reset_pending = true;
mixer_text_length = 0;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK);
return 0;
}
/* check if the last item has been processed - bail out if not */
if (mixer_update_pending) {
return 1;
}
/* append mixer text and nul-terminate, guard against overflow */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
isr_debug(2, "buflen %u", mixer_text_length);
/* flag the buffer as ready */
mixer_update_pending = true;
break;
}
return 0;
}
void interrupt_mixer_set_failsafe()
{
mix_failsafe = true;
}
void
mixer_mix_failsafe()
{
/*
* Check if a custom failsafe value has been written,
* or if the mixer is not ok and bail out.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
return;
}
/* set failsafe defaults to the values for all inputs = 0 */
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
/* maximum value the outputs of the multirotor mixer are allowed to change in this cycle
* factor 2 is needed because actuator outputs are in the range [-1,1]
*/
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max);
mixer_group.set_max_delta_out_once(delta_out_max);
}
/* set dt to be used in simple mixer for slew rate limiting */
mixer_group.set_dt_once(dt);
/* update parameter for mc thrust model if it updated */
if (update_mc_thrust_param) {
mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
update_mc_thrust_param = false;
}
/* mix */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
r_mixer_limits = mixer_group.get_saturation_status();
} else {
mixed = 0;
}
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* scale to servo output */
r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
}
/* disable the rest of the outputs */
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
r_page_servo_failsafe[i] = 0;
}
}

42
src/modules/px4iofirmware/mixer.h
View File

@ -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 <lorenz@px4.io>
*/
#pragma once
#define PX4IO_MAX_MIXER_LENGTH 330

167
src/modules/px4iofirmware/protocol.h
View File

@ -61,7 +61,7 @@
* the PX4 system are expressed as signed integer values scaled by 10000, * 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 * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and
* SIGNED_TO_REG macros to convert between register representation 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 * Note that the implementation of readable pages prefers registers within
* readable pages to be densely packed. Page numbers do not need to be * readable pages to be densely packed. Page numbers do not need to be
@ -75,12 +75,9 @@
#define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) #define REG_TO_SIGNED(_reg) ((int16_t)(_reg))
#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) #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 REG_TO_BOOL(_reg) ((bool)(_reg))
#define PX4IO_PROTOCOL_VERSION 4 #define PX4IO_PROTOCOL_VERSION 5
/* maximum allowable sizes on this protocol version */ /* 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 */ #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 */
@ -95,7 +92,6 @@
#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ #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_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_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 */ /* dynamic status page */
@ -105,40 +101,26 @@
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ #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_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 << 1) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 2) /* PPM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */ #define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 5) /* controls from FMU are valid */
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */ #define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 6) /* raw PWM from FMU */
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* 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_MIXER_OK (1 << 8) /* mixer is OK */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 8) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 9) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 10) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 11) /* FMU was initialized and OK once */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 12) /* ST24 input is valid */
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ #define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 13) /* SUMD input is valid */
#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_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #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_RC_LOST (1 << 0) /* timed out waiting for RC input */
#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ #define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 1) /* PWM configuration or output was bad */
#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_VSERVO 6 /* [2] servo rail voltage in mV */ #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_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 */ /* 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 */
@ -159,11 +141,6 @@
#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ #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 */ #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 */ /* 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 */
@ -176,30 +153,22 @@
#define PX4IO_P_SETUP_FEATURES 0 #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_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_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 << 2) /**< enable ADC RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #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_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_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_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 << 3) /* use custom failsafe values */
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* 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_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */ #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 5) /* If set, the system will always output the failsafe values */
#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_TERMINATION_FAILSAFE (1 << 6) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
#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_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #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_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_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_RELAYS_PAD 5 #define PX4IO_P_SETUP_VSERVO_SCALE 5 /* hardware rev [2] servo voltage correction factor (float) */
#define PX4IO_P_SETUP_DSM 6 /* DSM bind state */
#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */
#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */
enum { /* DSM bind states */ enum { /* DSM bind states */
dsm_bind_power_down = 0, dsm_bind_power_down = 0,
dsm_bind_power_up, dsm_bind_power_up,
@ -218,113 +187,33 @@ enum { /* DSM bind states */
#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into #define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into
'armed' (PWM enabled) state - this is a non-data write and 'armed' (PWM enabled) state - this is a non-data write and
hence index 12 can safely be used. */ 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_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_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
#define PX4IO_P_SETUP_SBUS_RATE 16 /**< frame rate of SBUS1 output in Hz */
#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ #define PX4IO_P_SETUP_THERMAL 17 /**< thermal management */
#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ #define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 18 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
#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_THERMAL_IGNORE UINT16_MAX #define PX4IO_THERMAL_IGNORE UINT16_MAX
#define PX4IO_THERMAL_OFF 0 #define PX4IO_THERMAL_OFF 0
#define PX4IO_THERMAL_FULL 10000 #define PX4IO_THERMAL_FULL 10000
/* autopilot control values, -10000..10000 */ /* PWM output */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_VALID 64
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */
/* raw text load to the mixer parser - ignores offset */
#define PX4IO_PAGE_MIXERLOAD 52
/* R/C channel config */
#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */
#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */
#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */
#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */
/* PWM output - overrides mixer */
#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ #define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */ /* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ #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 */ /* Debug and test page - not used in normal operation */
#define PX4IO_PAGE_TEST 127 #define PX4IO_PAGE_TEST 127
#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */ #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 */ /* PWM disarmed values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ #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)
/** /**
* Serial protocol encapsulation. * Serial protocol encapsulation.
*/ */
#define PKT_MAX_REGS 32 // by agreement w/FMU #define PKT_MAX_REGS 32 // by agreement w/FMU
#pragma pack(push, 1) #pragma pack(push, 1)

29
src/modules/px4iofirmware/px4io.c
View File

@ -59,8 +59,6 @@
# include <lib/perf/perf_counter.h> # include <lib/perf/perf_counter.h>
#endif #endif
#include <output_limit/output_limit.h>
#include <stm32_uart.h> #include <stm32_uart.h>
#define DEBUG #define DEBUG
@ -72,10 +70,6 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call; 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 * a set of debug buffers to allow us to send debug information from ISRs
*/ */
@ -313,11 +307,6 @@ user_start(int argc, char *argv[])
LED_RING(false); LED_RING(false);
#endif #endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */ /* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT #ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false); ENABLE_SBUS_OUT(false);
@ -350,9 +339,6 @@ user_start(int argc, char *argv[])
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk; r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk;
syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
output_limit_init(&pwm_limit);
/* Start the failsafe led init */ /* Start the failsafe led init */
failsafe_led_init(); failsafe_led_init();
@ -362,23 +348,12 @@ user_start(int argc, char *argv[])
uint64_t last_debug_time = 0; uint64_t last_debug_time = 0;
uint64_t last_heartbeat_time = 0; uint64_t last_heartbeat_time = 0;
uint64_t last_loop_time = 0;
watchdog_init(); watchdog_init();
for (;;) { for (;;) {
watchdog_pet(); watchdog_pet();
dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f;
last_loop_time = hrt_absolute_time();
if (dt < 0.0001f) {
dt = 0.0001f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
#if defined(PX4IO_PERF) #if defined(PX4IO_PERF)
/* track the rate at which the loop is running */ /* track the rate at which the loop is running */
perf_count(loop_perf); perf_count(loop_perf);
@ -416,10 +391,6 @@ user_start(int argc, char *argv[])
*/ */
uint32_t heartbeat_period_us = 250 * 1000UL; 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) { if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
last_heartbeat_time = hrt_absolute_time(); last_heartbeat_time = hrt_absolute_time();
heartbeat_blink(); heartbeat_blink();

42
src/modules/px4iofirmware/px4io.h
View File

@ -50,15 +50,13 @@
#include "protocol.h" #include "protocol.h"
#include <output_limit/output_limit.h>
/* /*
* Constants and limits. * Constants and limits.
*/ */
#define PX4IO_BL_VERSION 3 #define PX4IO_BL_VERSION 3
#define PX4IO_SERVO_COUNT 8 #define PX4IO_SERVO_COUNT 8
#define PX4IO_CONTROL_CHANNELS 8 #define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_CONTROL_GROUPS 4
#define PX4IO_RC_INPUT_CHANNELS 18 #define PX4IO_RC_INPUT_CHANNELS 18
#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ #define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */
@ -77,20 +75,12 @@
* Registers. * Registers.
*/ */
extern volatile uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */ 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_servos[]; /* PX4IO_PAGE_SERVOS */
extern uint16_t r_page_direct_pwm[]; /* PX4IO_PAGE_DIRECT_PWM */ 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_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 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_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 */ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
/* /*
@ -104,32 +94,16 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT] #define r_raw_rc_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_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_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_features r_page_setup[PX4IO_P_SETUP_FEATURES]
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] #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_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_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_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_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. * System state structure.
@ -146,15 +120,9 @@ struct sys_state_s {
}; };
extern struct sys_state_s system_state; extern struct sys_state_s system_state;
extern float dt;
extern bool update_mc_thrust_param; extern bool update_mc_thrust_param;
extern bool update_trims; extern bool update_trims;
/*
* PWM limit structure
*/
extern output_limit_t pwm_limit;
/* /*
* GPIO handling. * GPIO handling.
*/ */
@ -169,7 +137,6 @@ extern output_limit_t pwm_limit;
#define LED_RING(_s) px4_arch_gpiowrite(GPIO_LED4, (_s)) #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 ENABLE_SBUS_OUT(_s) px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
# define VDD_SERVO_FAULT (!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT)) # define VDD_SERVO_FAULT (!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT))
@ -180,8 +147,6 @@ extern output_limit_t pwm_limit;
#define BUTTON_SAFETY px4_arch_gpioread(GPIO_BTN_SAFETY) #define 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); } #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); void atomic_modify_or(volatile uint16_t *target, uint16_t modification);
@ -192,9 +157,6 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification);
* Mixer * Mixer
*/ */
extern void mixer_tick(void); extern void mixer_tick(void);
extern int interrupt_mixer_handle_text(const void *buffer, size_t length);
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
extern void interrupt_mixer_set_failsafe(void);
/** /**
* Safety switch/LED. * Safety switch/LED.

443
src/modules/px4iofirmware/registers.c
View File

@ -74,7 +74,6 @@ static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS, [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS,
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, [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_ALARMS] = 0,
[PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 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 * 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 [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. * Scratch page; used for registers that are constructed as-read.
* *
@ -155,76 +135,29 @@ uint16_t r_page_direct_pwm[PX4IO_SERVO_COUNT];
volatile uint16_t r_page_setup[] = { volatile uint16_t r_page_setup[] = {
/* default to RSSI ADC functionality */ /* default to RSSI ADC functionality */
[PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, [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_RATES] = 0,
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
[PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_PWM_ALTRATE] = 200,
[PX4IO_P_SETUP_SBUS_RATE] = 72, [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, [PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
#else
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
#endif
[PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_SET_DEBUG] = 0,
[PX4IO_P_SETUP_REBOOT_BL] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0,
[PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 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_THERMAL] = PX4IO_THERMAL_IGNORE,
[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0 [PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0
}; };
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ #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_SBUS2_OUT | \
PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
PX4IO_P_SETUP_FEATURES_PWM_RSSI)
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_FMU_PREARMED | \ 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_IO_ARM_OK | \
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ 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_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
/**
* PAGE 101
*
* Control values from the FMU.
*/
uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
/*
* PAGE 102 does not have a buffer.
*/
/**
* PAGE 103
*
* R/C channel input configuration.
*/
uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
/* valid options */
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
/* /*
* PAGE 104 uses r_page_servos. * PAGE 104 uses r_page_servos.
@ -239,30 +172,6 @@ uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STR
*/ */
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; 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 * PAGE 109
* *
@ -274,29 +183,7 @@ uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
int int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{ {
switch (page) { 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 */ /* handle raw PWM input */
case PX4IO_PAGE_DIRECT_PWM: case PX4IO_PAGE_DIRECT_PWM:
@ -351,92 +238,21 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
break; 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: { case PX4IO_PAGE_DISARMED_PWM: {
/* flag for all outputs */
bool all_disarmed_off = true;
/* copy channel data */ /* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0) { if (*values == 0) {
/* 0 means disabling always PWM */ /* 0 means disabling always PWM */
r_page_servo_disarmed[offset] = 0; r_page_servo_disarmed[offset] = 0;
} else if (*values < PWM_LOWEST_MIN) { } else if (*values < PWM_LOWEST_MIN) {
r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
all_disarmed_off = false;
} else if (*values > PWM_HIGHEST_MAX) { } else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
all_disarmed_off = false;
} else { } else {
r_page_servo_disarmed[offset] = *values; r_page_servo_disarmed[offset] = *values;
all_disarmed_off = false;
} }
offset++; offset++;
@ -444,25 +260,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
values++; values++;
} }
if (all_disarmed_off) { r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
/* 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; break;
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
/* do not change the mixer if FMU is armed and IO's safety is off
* this state defines an active system. This check is done in the
* text handling function.
*/
return interrupt_mixer_handle_text(values, num_values * sizeof(*values));
default: default:
/* avoid offset wrap */ /* avoid offset wrap */
@ -504,20 +305,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* Allow FMU override of arming state (to allow in-air restores), * 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. * but only if the arming state is not in sync on the IO side.
*/ */
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
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)) {
r_status_flags = value; r_status_flags = value;
} }
if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) {
/* update failsafe values, now that the mixer is set to ok */
interrupt_mixer_set_failsafe();
}
break; break;
default: default:
@ -541,32 +333,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* disable the conflicting options with SBUS 1 */ /* disable the conflicting options with SBUS 1 */
if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
PX4IO_P_SETUP_FEATURES_ADC_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
} }
/* disable the conflicting options with SBUS 2 */ /* disable the conflicting options with SBUS 2 */
if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
PX4IO_P_SETUP_FEATURES_ADC_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
} }
#endif #endif
/* disable the conflicting options with ADC RSSI */ /* disable the conflicting options with ADC RSSI */
if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | value &= ~(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
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);
} }
/* apply changes */ /* apply changes */
@ -578,18 +357,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
value &= PX4IO_P_SETUP_ARMING_VALID; 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 * If the failsafe termination flag is set, do not allow the autopilot to unset it
*/ */
@ -688,32 +455,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break; 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: case PX4IO_P_SETUP_SBUS_RATE:
r_page_setup[offset] = value; r_page_setup[offset] = value;
sbus1_set_output_rate_hz(value); sbus1_set_output_rate_hz(value);
break; 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_THERMAL:
case PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION: case PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION:
r_page_setup[offset] = value; r_page_setup[offset] = value;
@ -725,101 +471,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break; break;
case PX4IO_PAGE_RC_CONFIG: {
/**
* do not allow a RC config change while safety is off
*/
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
break;
}
unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
if (channel >= PX4IO_RC_INPUT_CHANNELS) {
return -1;
}
/* disable the channel until we have a chance to sanity-check it */
conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
switch (index) {
case PX4IO_P_RC_CONFIG_MIN:
case PX4IO_P_RC_CONFIG_CENTER:
case PX4IO_P_RC_CONFIG_MAX:
case PX4IO_P_RC_CONFIG_DEADZONE:
case PX4IO_P_RC_CONFIG_ASSIGNMENT:
conf[index] = value;
break;
case PX4IO_P_RC_CONFIG_OPTIONS:
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
/* clear any existing RC disabled flag */
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
/* set all options except the enabled option */
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
/* should the channel be enabled? */
/* this option is normally set last */
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint8_t count = 0;
bool disabled = false;
/* assert min..center..max ordering */
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
count++;
}
/* assert deadzone is sane */
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
count++;
}
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
count++;
}
/* sanity checks pass, enable channel */
if (count) {
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
} else if (!disabled) {
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
}
break;
/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */
}
break;
/* case PX4IO_RC_PAGE_CONFIG */
}
case PX4IO_PAGE_TEST: case PX4IO_PAGE_TEST:
switch (offset) { switch (offset) {
case PX4IO_P_TEST_LED: case PX4IO_P_TEST_LED:
@ -862,49 +513,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
/* PX4IO_P_STATUS_ALARMS maintained externally */ /* 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 #ifdef ADC_VSERVO
/* PX4IO_P_STATUS_VSERVO */ /* PX4IO_P_STATUS_VSERVO */
{ {
@ -916,6 +524,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
r_page_status[PX4IO_P_STATUS_VSERVO] = mV; r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
} }
} }
#endif #endif
#ifdef ADC_RSSI #ifdef ADC_RSSI
/* PX4IO_P_STATUS_VRSSI */ /* PX4IO_P_STATUS_VRSSI */
@ -936,12 +545,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_RAW_ADC_INPUT: case PX4IO_PAGE_RAW_ADC_INPUT:
memset(r_page_scratch, 0, sizeof(r_page_scratch)); 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 #ifdef ADC_VSERVO
r_page_scratch[0] = adc_measure(ADC_VSERVO); r_page_scratch[0] = adc_measure(ADC_VSERVO);
@ -971,10 +574,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
SELECT_PAGE(r_page_config); SELECT_PAGE(r_page_config);
break; break;
case PX4IO_PAGE_ACTUATORS:
SELECT_PAGE(r_page_actuators);
break;
case PX4IO_PAGE_SERVOS: case PX4IO_PAGE_SERVOS:
SELECT_PAGE(r_page_servos); SELECT_PAGE(r_page_servos);
break; break;
@ -983,23 +582,11 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
SELECT_PAGE(r_page_raw_rc_input); SELECT_PAGE(r_page_raw_rc_input);
break; break;
case PX4IO_PAGE_RC_INPUT:
SELECT_PAGE(r_page_rc_input);
break;
/* readback of input pages */ /* readback of input pages */
case PX4IO_PAGE_SETUP: case PX4IO_PAGE_SETUP:
SELECT_PAGE(r_page_setup); SELECT_PAGE(r_page_setup);
break; break;
case PX4IO_PAGE_CONTROLS:
SELECT_PAGE(r_page_controls);
break;
case PX4IO_PAGE_RC_CONFIG:
SELECT_PAGE(r_page_rc_input_config);
break;
case PX4IO_PAGE_DIRECT_PWM: case PX4IO_PAGE_DIRECT_PWM:
SELECT_PAGE(r_page_direct_pwm); SELECT_PAGE(r_page_direct_pwm);
break; break;
@ -1008,18 +595,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
SELECT_PAGE(r_page_servo_failsafe); SELECT_PAGE(r_page_servo_failsafe);
break; 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: case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_disarmed); SELECT_PAGE(r_page_servo_disarmed);
break; break;

244
src/modules/rc_update/params.c
View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -1329,39 +1329,6 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 0);
*/ */
PARAM_DEFINE_INT32(RC_MAP_FLTMODE, 0); 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 * Return switch channel
* *
@ -1390,34 +1357,6 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
*/ */
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
/**
* Position Control switch channel
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
/** /**
* Loiter switch channel * Loiter switch channel
* *
@ -1446,34 +1385,6 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
*/ */
PARAM_DEFINE_INT32(RC_MAP_LOITER_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 * Offboard switch channel
* *
@ -1645,62 +1556,6 @@ PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0);
*/ */
PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0); PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0);
/**
* Stabilize switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0);
/**
* Manual switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
/** /**
* Button flight mode selection * Button flight mode selection
* *
@ -1732,7 +1587,6 @@ PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
* @bit 16 Mask Channel 17 as a mode button * @bit 16 Mask Channel 17 as a mode button
* @bit 17 Mask Channel 18 as a mode button * @bit 17 Mask Channel 18 as a mode button
*/ */
PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0); PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0);
/** /**
@ -2019,54 +1873,6 @@ PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0);
*/ */
PARAM_DEFINE_INT32(RC_FAILS_THR, 0); 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 channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*/
PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f);
/**
* Threshold for selecting auto 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 channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*/
PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
/**
* Threshold for selecting posctl 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 channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*/
PARAM_DEFINE_FLOAT(RC_POSCTL_TH, 0.75f);
/** /**
* Threshold for selecting return to launch mode * Threshold for selecting return to launch mode
* *
@ -2099,22 +1905,6 @@ PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.75f);
*/ */
PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.75f); PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.75f);
/**
* Threshold for selecting acro 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 channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*/
PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.75f);
/** /**
* Threshold for selecting offboard mode * Threshold for selecting offboard mode
* *
@ -2195,38 +1985,6 @@ PARAM_DEFINE_FLOAT(RC_TRANS_TH, 0.75f);
*/ */
PARAM_DEFINE_FLOAT(RC_GEAR_TH, 0.75f); PARAM_DEFINE_FLOAT(RC_GEAR_TH, 0.75f);
/**
* Threshold for the stabilize switch.
*
* 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 channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*/
PARAM_DEFINE_FLOAT(RC_STAB_TH, 0.5f);
/**
* Threshold for the manual switch.
*
* 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 channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*/
PARAM_DEFINE_FLOAT(RC_MAN_TH, 0.75f);
/** /**
* PWM input channel that provides RSSI. * PWM input channel that provides RSSI.
* *

146
src/modules/rc_update/params_deprecated.c
View File

@ -31,6 +31,39 @@
* *
****************************************************************************/ ****************************************************************************/
/**
* Mode switch channel mapping (deprecated)
*
* 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);
/** /**
* Rattitude switch channel (deprecated) * Rattitude switch channel (deprecated)
* *
@ -58,3 +91,116 @@
* @value 18 Channel 18 * @value 18 Channel 18
*/ */
PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0);
/**
* Position Control switch channel (deprecated)
*
* @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);
/**
* Acro switch channel (deprecated)
*
* @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);
/**
* Stabilize switch channel mapping (deprecated)
*
* @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 (deprecated)
*
* @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);

81
src/modules/rc_update/rc_update.cpp
View File

@ -48,18 +48,13 @@ namespace RCUpdate
static bool operator ==(const manual_control_switches_s &a, const manual_control_switches_s &b) static bool operator ==(const manual_control_switches_s &a, const manual_control_switches_s &b)
{ {
return (a.mode_slot == b.mode_slot && return (a.mode_slot == b.mode_slot &&
a.mode_switch == b.mode_switch &&
a.return_switch == b.return_switch && a.return_switch == b.return_switch &&
a.posctl_switch == b.posctl_switch &&
a.loiter_switch == b.loiter_switch && a.loiter_switch == b.loiter_switch &&
a.acro_switch == b.acro_switch &&
a.offboard_switch == b.offboard_switch && a.offboard_switch == b.offboard_switch &&
a.kill_switch == b.kill_switch && a.kill_switch == b.kill_switch &&
a.arm_switch == b.arm_switch && a.arm_switch == b.arm_switch &&
a.transition_switch == b.transition_switch && a.transition_switch == b.transition_switch &&
a.gear_switch == b.gear_switch && a.gear_switch == b.gear_switch);
a.stab_switch == b.stab_switch &&
a.man_switch == b.man_switch);
} }
static bool operator !=(const manual_control_switches_s &a, const manual_control_switches_s &b) { return !(a == b); } static bool operator !=(const manual_control_switches_s &a, const manual_control_switches_s &b) { return !(a == b); }
@ -161,12 +156,47 @@ void RCUpdate::parameters_updated()
{ {
int32_t rc_map_value = 0; int32_t rc_map_value = 0;
if (param_get(param_find("RC_MAP_MODE_SW"), &rc_map_value) == PX4_OK) {
if (rc_map_value != 0) {
PX4_WARN("RC_MAP_MODE_SW deprecated");
param_reset(param_find("RC_MAP_MODE_SW"));
}
}
if (param_get(param_find("RC_MAP_RATT_SW"), &rc_map_value) == PX4_OK) { if (param_get(param_find("RC_MAP_RATT_SW"), &rc_map_value) == PX4_OK) {
if (rc_map_value != 0) { if (rc_map_value != 0) {
PX4_WARN("RC_MAP_RATT_SW deprecated"); PX4_WARN("RC_MAP_RATT_SW deprecated");
param_reset(param_find("RC_MAP_RATT_SW")); param_reset(param_find("RC_MAP_RATT_SW"));
} }
} }
if (param_get(param_find("RC_MAP_POSCTL_SW"), &rc_map_value) == PX4_OK) {
if (rc_map_value != 0) {
PX4_WARN("RC_MAP_POSCTL_SW deprecated");
param_reset(param_find("RC_MAP_POSCTL_SW"));
}
}
if (param_get(param_find("RC_MAP_ACRO_SW"), &rc_map_value) == PX4_OK) {
if (rc_map_value != 0) {
PX4_WARN("RC_MAP_ACRO_SW deprecated");
param_reset(param_find("RC_MAP_ACRO_SW"));
}
}
if (param_get(param_find("RC_MAP_STAB_SW"), &rc_map_value) == PX4_OK) {
if (rc_map_value != 0) {
PX4_WARN("RC_MAP_STAB_SW deprecated");
param_reset(param_find("RC_MAP_STAB_SW"));
}
}
if (param_get(param_find("RC_MAP_MAN_SW"), &rc_map_value) == PX4_OK) {
if (rc_map_value != 0) {
PX4_WARN("RC_MAP_MAN_SW deprecated");
param_reset(param_find("RC_MAP_MAN_SW"));
}
}
} }
} }
@ -178,18 +208,13 @@ void RCUpdate::update_rc_functions()
_rc.function[rc_channels_s::FUNCTION_PITCH] = _param_rc_map_pitch.get() - 1; _rc.function[rc_channels_s::FUNCTION_PITCH] = _param_rc_map_pitch.get() - 1;
_rc.function[rc_channels_s::FUNCTION_YAW] = _param_rc_map_yaw.get() - 1; _rc.function[rc_channels_s::FUNCTION_YAW] = _param_rc_map_yaw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_MODE] = _param_rc_map_mode_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_RETURN] = _param_rc_map_return_sw.get() - 1; _rc.function[rc_channels_s::FUNCTION_RETURN] = _param_rc_map_return_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_POSCTL] = _param_rc_map_posctl_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_LOITER] = _param_rc_map_loiter_sw.get() - 1; _rc.function[rc_channels_s::FUNCTION_LOITER] = _param_rc_map_loiter_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_ACRO] = _param_rc_map_acro_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_OFFBOARD] = _param_rc_map_offb_sw.get() - 1; _rc.function[rc_channels_s::FUNCTION_OFFBOARD] = _param_rc_map_offb_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_KILLSWITCH] = _param_rc_map_kill_sw.get() - 1; _rc.function[rc_channels_s::FUNCTION_KILLSWITCH] = _param_rc_map_kill_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_ARMSWITCH] = _param_rc_map_arm_sw.get() - 1; _rc.function[rc_channels_s::FUNCTION_ARMSWITCH] = _param_rc_map_arm_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_TRANSITION] = _param_rc_map_trans_sw.get() - 1; _rc.function[rc_channels_s::FUNCTION_TRANSITION] = _param_rc_map_trans_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_GEAR] = _param_rc_map_gear_sw.get() - 1; _rc.function[rc_channels_s::FUNCTION_GEAR] = _param_rc_map_gear_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_STAB] = _param_rc_map_stab_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_MAN] = _param_rc_map_man_sw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_FLAPS] = _param_rc_map_flaps.get() - 1; _rc.function[rc_channels_s::FUNCTION_FLAPS] = _param_rc_map_flaps.get() - 1;
@ -524,28 +549,6 @@ void RCUpdate::Run()
perf_end(_loop_perf); perf_end(_loop_perf);
} }
switch_pos_t RCUpdate::get_rc_sw3pos_position(uint8_t func, float on_th, float mid_th) const
{
if (_rc.function[func] >= 0) {
const bool on_inv = (on_th < 0.f);
const bool mid_inv = (mid_th < 0.f);
const float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return manual_control_switches_s::SWITCH_POS_ON;
} else if (mid_inv ? value < mid_th : value > mid_th) {
return manual_control_switches_s::SWITCH_POS_MIDDLE;
} else {
return manual_control_switches_s::SWITCH_POS_OFF;
}
}
return manual_control_switches_s::SWITCH_POS_NONE;
}
switch_pos_t RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th) const switch_pos_t RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th) const
{ {
if (_rc.function[func] >= 0) { if (_rc.function[func] >= 0) {
@ -569,7 +572,7 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
manual_control_switches_s switches{}; manual_control_switches_s switches{};
switches.timestamp_sample = timestamp_sample; switches.timestamp_sample = timestamp_sample;
// check mode slot (RC_MAP_FLTMODE) or legacy mode switch (RC_MAP_MODE_SW), but not both // check mode slot (RC_MAP_FLTMODE)
if (_param_rc_map_fltmode.get() > 0) { if (_param_rc_map_fltmode.get() > 0) {
// number of valid slots // number of valid slots
static constexpr int num_slots = manual_control_switches_s::MODE_SLOT_NUM; static constexpr int num_slots = manual_control_switches_s::MODE_SLOT_NUM;
@ -594,16 +597,6 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
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.stab_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_STAB, _param_rc_stab_th.get());
switches.posctl_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_POSCTL, _param_rc_posctl_th.get());
} else if (_param_rc_map_flightmode_buttons.get() > 0) { } else if (_param_rc_map_flightmode_buttons.get() > 0) {
switches.mode_slot = manual_control_switches_s::MODE_SLOT_NONE; switches.mode_slot = manual_control_switches_s::MODE_SLOT_NONE;
bool is_consistent_button_press = false; bool is_consistent_button_press = false;

14
src/modules/rc_update/rc_update.h
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * Copyright (c) 2016-2021 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -120,7 +120,6 @@ private:
/** /**
* Get switch position for specified function. * Get switch position for specified function.
*/ */
switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, float mid_th) const;
switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th) const; switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th) const;
/** /**
@ -208,21 +207,16 @@ private:
(ParamInt<px4::params::RC_MAP_FAILSAFE>) _param_rc_map_failsafe, (ParamInt<px4::params::RC_MAP_FAILSAFE>) _param_rc_map_failsafe,
(ParamInt<px4::params::RC_MAP_FLTMODE>) _param_rc_map_fltmode, (ParamInt<px4::params::RC_MAP_FLTMODE>) _param_rc_map_fltmode,
(ParamInt<px4::params::RC_MAP_MODE_SW>) _param_rc_map_mode_sw,
(ParamInt<px4::params::RC_MAP_FLAPS>) _param_rc_map_flaps, (ParamInt<px4::params::RC_MAP_FLAPS>) _param_rc_map_flaps,
(ParamInt<px4::params::RC_MAP_RETURN_SW>) _param_rc_map_return_sw, (ParamInt<px4::params::RC_MAP_RETURN_SW>) _param_rc_map_return_sw,
(ParamInt<px4::params::RC_MAP_POSCTL_SW>) _param_rc_map_posctl_sw,
(ParamInt<px4::params::RC_MAP_LOITER_SW>) _param_rc_map_loiter_sw, (ParamInt<px4::params::RC_MAP_LOITER_SW>) _param_rc_map_loiter_sw,
(ParamInt<px4::params::RC_MAP_ACRO_SW>) _param_rc_map_acro_sw,
(ParamInt<px4::params::RC_MAP_OFFB_SW>) _param_rc_map_offb_sw, (ParamInt<px4::params::RC_MAP_OFFB_SW>) _param_rc_map_offb_sw,
(ParamInt<px4::params::RC_MAP_KILL_SW>) _param_rc_map_kill_sw, (ParamInt<px4::params::RC_MAP_KILL_SW>) _param_rc_map_kill_sw,
(ParamInt<px4::params::RC_MAP_ARM_SW>) _param_rc_map_arm_sw, (ParamInt<px4::params::RC_MAP_ARM_SW>) _param_rc_map_arm_sw,
(ParamInt<px4::params::RC_MAP_TRANS_SW>) _param_rc_map_trans_sw, (ParamInt<px4::params::RC_MAP_TRANS_SW>) _param_rc_map_trans_sw,
(ParamInt<px4::params::RC_MAP_GEAR_SW>) _param_rc_map_gear_sw, (ParamInt<px4::params::RC_MAP_GEAR_SW>) _param_rc_map_gear_sw,
(ParamInt<px4::params::RC_MAP_STAB_SW>) _param_rc_map_stab_sw,
(ParamInt<px4::params::RC_MAP_MAN_SW>) _param_rc_map_man_sw,
(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_flightmode_buttons, (ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_flightmode_buttons,
(ParamInt<px4::params::RC_MAP_AUX1>) _param_rc_map_aux1, (ParamInt<px4::params::RC_MAP_AUX1>) _param_rc_map_aux1,
@ -234,18 +228,12 @@ private:
(ParamInt<px4::params::RC_FAILS_THR>) _param_rc_fails_thr, (ParamInt<px4::params::RC_FAILS_THR>) _param_rc_fails_thr,
(ParamFloat<px4::params::RC_ASSIST_TH>) _param_rc_assist_th,
(ParamFloat<px4::params::RC_AUTO_TH>) _param_rc_auto_th,
(ParamFloat<px4::params::RC_POSCTL_TH>) _param_rc_posctl_th,
(ParamFloat<px4::params::RC_LOITER_TH>) _param_rc_loiter_th, (ParamFloat<px4::params::RC_LOITER_TH>) _param_rc_loiter_th,
(ParamFloat<px4::params::RC_ACRO_TH>) _param_rc_acro_th,
(ParamFloat<px4::params::RC_OFFB_TH>) _param_rc_offb_th, (ParamFloat<px4::params::RC_OFFB_TH>) _param_rc_offb_th,
(ParamFloat<px4::params::RC_KILLSWITCH_TH>) _param_rc_killswitch_th, (ParamFloat<px4::params::RC_KILLSWITCH_TH>) _param_rc_killswitch_th,
(ParamFloat<px4::params::RC_ARMSWITCH_TH>) _param_rc_armswitch_th, (ParamFloat<px4::params::RC_ARMSWITCH_TH>) _param_rc_armswitch_th,
(ParamFloat<px4::params::RC_TRANS_TH>) _param_rc_trans_th, (ParamFloat<px4::params::RC_TRANS_TH>) _param_rc_trans_th,
(ParamFloat<px4::params::RC_GEAR_TH>) _param_rc_gear_th, (ParamFloat<px4::params::RC_GEAR_TH>) _param_rc_gear_th,
(ParamFloat<px4::params::RC_STAB_TH>) _param_rc_stab_th,
(ParamFloat<px4::params::RC_MAN_TH>) _param_rc_man_th,
(ParamFloat<px4::params::RC_RETURN_TH>) _param_rc_return_th, (ParamFloat<px4::params::RC_RETURN_TH>) _param_rc_return_th,
(ParamInt<px4::params::RC_CHAN_CNT>) _param_rc_chan_cnt (ParamInt<px4::params::RC_CHAN_CNT>) _param_rc_chan_cnt

2
src/modules/simulator/simulator.h
View File

@ -43,7 +43,6 @@
#pragma once #pragma once
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp> #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp> #include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp> #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
@ -60,6 +59,7 @@
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/differential_pressure.h> #include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h> #include <uORB/topics/distance_sensor.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/irlock_report.h> #include <uORB/topics/irlock_report.h>
#include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/optical_flow.h> #include <uORB/topics/optical_flow.h>

1
src/systemcmds/tests/CMakeLists.txt
View File

@ -36,7 +36,6 @@ set(srcs
test_bezierQuad.cpp test_bezierQuad.cpp
test_bitset.cpp test_bitset.cpp
test_bson.cpp test_bson.cpp
test_conv.cpp
test_dataman.c test_dataman.c
test_file.c test_file.c
test_file2.c test_file2.c

74
src/systemcmds/tests/test_conv.cpp
View File

@ -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 <sys/types.h>
#include <stdio.h>
#include <poll.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include "tests_main.h"
#include <math.h>
#include <float.h>
#include <unit_test.h>
#include <px4iofirmware/protocol.h>
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;
}

6
src/systemcmds/tests/test_mixer.cpp
View File

@ -48,7 +48,7 @@
#include <output_limit/output_limit.h> #include <output_limit/output_limit.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <px4iofirmware/mixer.h>
#include <px4iofirmware/protocol.h> #include <px4iofirmware/protocol.h>
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
@ -277,7 +277,7 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade
/* reset, load in chunks */ /* reset, load in chunks */
mixer_group.reset(); 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 mixer_text_length = 0;
unsigned transmitted = 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 */ /* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { 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, 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; return false;
} }

2
src/systemcmds/tests/test_ppm_loopback.c
View File

@ -48,7 +48,7 @@
#include <errno.h> #include <errno.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <drivers/drv_rc_input.h> #include <uORB/topics/input_rc.h>
#include <uORB/topics/rc_channels.h> #include <uORB/topics/rc_channels.h>
#include <systemlib/err.h> #include <systemlib/err.h>

2
src/systemcmds/tests/test_rc.c
View File

@ -48,9 +48,9 @@
#include <errno.h> #include <errno.h>
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <uORB/topics/input_rc.h>
#include "tests_main.h" #include "tests_main.h"

1
src/systemcmds/tests/tests_main.c
View File

@ -81,7 +81,6 @@ const struct {
{"bezier", test_bezierQuad, 0}, {"bezier", test_bezierQuad, 0},
{"bitset", test_bitset, 0}, {"bitset", test_bitset, 0},
{"bson", test_bson, 0}, {"bson", test_bson, 0},
{"conv", test_conv, 0},
{"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST},
{"file2", test_file2, OPT_NOJIGTEST}, {"file2", test_file2, OPT_NOJIGTEST},
{"float", test_float, 0}, {"float", test_float, 0},

1
src/systemcmds/tests/tests_main.h
View File

@ -47,7 +47,6 @@ extern int test_atomic_bitset(int argc, char *argv[]);
extern int test_bezierQuad(int argc, char *argv[]); extern int test_bezierQuad(int argc, char *argv[]);
extern int test_bitset(int argc, char *argv[]); extern int test_bitset(int argc, char *argv[]);
extern int test_bson(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_dataman(int argc, char *argv[]);
extern int test_file(int argc, char *argv[]); extern int test_file(int argc, char *argv[]);
extern int test_file2(int argc, char *argv[]); extern int test_file2(int argc, char *argv[]);