forked from Archive/PX4-Autopilot
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
parent
f772b0f45d
commit
089c962d92
|
|
@ -120,8 +120,6 @@ then
|
|||
|
||||
param set SENS_BOARD_X_OFF 0.000001
|
||||
param set SENS_DPRES_OFF 0.001
|
||||
|
||||
param set SYS_RESTART_TYPE 2
|
||||
fi
|
||||
|
||||
param set-default BAT1_N_CELLS 4
|
||||
|
|
|
|||
|
|
@ -42,7 +42,6 @@ px4_add_romfs_files(
|
|||
rc.fw_apps
|
||||
rc.fw_defaults
|
||||
rc.interface
|
||||
rc.io
|
||||
rc.logging
|
||||
rc.mc_apps
|
||||
rc.mc_defaults
|
||||
|
|
|
|||
|
|
@ -56,7 +56,6 @@ param set-default RTL_DESCEND_ALT 10
|
|||
param set-default RTL_RETURN_ALT 30
|
||||
|
||||
param set-default SDLOG_DIRS_MAX 7
|
||||
param set-default SYS_RESTART_TYPE 2
|
||||
|
||||
param set-default VT_F_TRANS_THR 0.75
|
||||
param set-default VT_MOT_ID 1234
|
||||
|
|
|
|||
|
|
@ -48,7 +48,6 @@ param set-default CBRK_IO_SAFETY 22027
|
|||
param set-default CBRK_SUPPLY_CHK 894281
|
||||
|
||||
# RC configuration
|
||||
param set-default RC_MAP_MODE_SW 5
|
||||
param set-default RC_MAP_PITCH 2
|
||||
param set-default RC_MAP_ROLL 1
|
||||
param set-default RC_MAP_THROTTLE 3
|
||||
|
|
|
|||
|
|
@ -17,6 +17,10 @@ set OUTPUT_DEV none
|
|||
set OUTPUT_AUX_DEV /dev/pwm_output1
|
||||
set OUTPUT_EXTRA_DEV /dev/pwm_output0
|
||||
|
||||
# set these before starting the modules
|
||||
param set PWM_AUX_OUT ${PWM_AUX_OUT}
|
||||
param set PWM_MAIN_OUT ${PWM_OUT}
|
||||
|
||||
#
|
||||
# If mount (gimbal) control is enabled and output mode is AUX, set the aux
|
||||
# mixer to mount (override the airframe-specific MIXER_AUX setting).
|
||||
|
|
@ -78,9 +82,16 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
if [ $OUTPUT_MODE = io ]
|
||||
#
|
||||
# Start IO for PWM output or RC input if needed.
|
||||
#
|
||||
if [ $IO_PRESENT = yes ]
|
||||
then
|
||||
. ${R}etc/init.d/rc.io
|
||||
if ! px4io start
|
||||
then
|
||||
echo "PX4IO start failed"
|
||||
tune_control play -t 18 # PROG_PX4IO_ERR
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ]
|
||||
|
|
@ -91,17 +102,6 @@ then
|
|||
tune_control play error
|
||||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# Start IO for RC input if needed.
|
||||
#
|
||||
if [ $IO_PRESENT = yes ]
|
||||
then
|
||||
if [ $OUTPUT_MODE != io ]
|
||||
then
|
||||
. ${R}etc/init.d/rc.io
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ $MIXER != none -a $MIXER != skip ]
|
||||
|
|
@ -212,8 +212,6 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
param set PWM_AUX_OUT ${PWM_AUX_OUT}
|
||||
|
||||
if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ]
|
||||
then
|
||||
if [ $PWM_OUT != none ]
|
||||
|
|
@ -226,8 +224,6 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
param set PWM_MAIN_OUT ${PWM_OUT}
|
||||
|
||||
if [ $EXTRA_MIXER_MODE != none ]
|
||||
then
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -70,14 +70,6 @@ then
|
|||
set io_file /etc/extras/px4_io-v2_default.bin
|
||||
fi
|
||||
|
||||
if px4io start
|
||||
then
|
||||
echo "PX4IO OK"
|
||||
else
|
||||
set unit_test_failure 1
|
||||
set unit_test_failure_list "${unit_test_failure_list} px4io_start"
|
||||
fi
|
||||
|
||||
if px4io checkcrc $io_file
|
||||
then
|
||||
echo "PX4IO CRC OK"
|
||||
|
|
@ -104,6 +96,15 @@ else
|
|||
fi
|
||||
fi
|
||||
|
||||
if px4io start
|
||||
then
|
||||
echo "PX4IO OK"
|
||||
else
|
||||
set unit_test_failure 1
|
||||
set unit_test_failure_list "${unit_test_failure_list} px4io_start"
|
||||
fi
|
||||
|
||||
|
||||
#
|
||||
# The presence of this file suggests we're running a mount stress test
|
||||
#
|
||||
|
|
|
|||
|
|
@ -53,7 +53,6 @@
|
|||
#include <fcntl.h>
|
||||
#include <mqueue.h>
|
||||
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <drivers/drv_board_led.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
|
|
@ -96,7 +95,7 @@ Syslink::Syslink() :
|
|||
_fd(0),
|
||||
_queue(2, sizeof(syslink_message_t)),
|
||||
_writebuffer(16, sizeof(crtp_message_t)),
|
||||
_rssi(RC_INPUT_RSSI_MAX),
|
||||
_rssi(input_rc_s::RSSI_MAX),
|
||||
_bstate(BAT_DISCHARGING)
|
||||
{
|
||||
px4_sem_init(&memory_sem, 0, 0);
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@ uint64 timestamp_last_signal # last valid reception time
|
|||
|
||||
uint8 channel_count # number of channels actually being seen
|
||||
|
||||
int8 RSSI_MAX = 100
|
||||
int32 rssi # receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception
|
||||
|
||||
bool rc_failsafe # explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly.
|
||||
|
|
|
|||
|
|
@ -26,11 +26,4 @@ uint8 kill_switch # throttle kill: _NORMAL_, KILL
|
|||
uint8 gear_switch # landing gear switch: _DOWN_, UP
|
||||
uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT
|
||||
|
||||
# legacy "advanced" switch configuration (will be removed soon)
|
||||
uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
|
||||
uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL
|
||||
uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO
|
||||
uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED
|
||||
uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL
|
||||
|
||||
uint32 switch_changes # number of switch changes
|
||||
|
|
|
|||
|
|
@ -6,49 +6,38 @@ float32 voltage_v # Servo rail voltage in volts
|
|||
float32 rssi_v # RSSI pin voltage in volts
|
||||
|
||||
# PX4IO status flags (PX4IO_P_STATUS_FLAGS)
|
||||
bool status_outputs_armed
|
||||
bool status_override
|
||||
bool status_rc_ok
|
||||
bool status_rc_ppm
|
||||
bool status_rc_dsm
|
||||
bool status_rc_sbus
|
||||
bool status_fmu_ok
|
||||
bool status_raw_pwm
|
||||
bool status_mixer_ok
|
||||
bool status_arm_sync
|
||||
bool status_init_ok
|
||||
bool status_failsafe
|
||||
bool status_safety_off
|
||||
bool status_fmu_initialized
|
||||
bool status_fmu_ok
|
||||
bool status_init_ok
|
||||
bool status_outputs_armed
|
||||
bool status_raw_pwm
|
||||
bool status_rc_ok
|
||||
bool status_rc_dsm
|
||||
bool status_rc_ppm
|
||||
bool status_rc_sbus
|
||||
bool status_rc_st24
|
||||
bool status_rc_sumd
|
||||
bool status_safety_off
|
||||
|
||||
# PX4IO alarms (PX4IO_P_STATUS_ALARMS)
|
||||
bool alarm_vbatt_low
|
||||
bool alarm_temperature
|
||||
bool alarm_servo_current
|
||||
bool alarm_acc_current
|
||||
bool alarm_fmu_lost
|
||||
bool alarm_rc_lost
|
||||
bool alarm_pwm_error
|
||||
bool alarm_vservo_fault
|
||||
bool alarm_rc_lost
|
||||
|
||||
# PX4IO arming (PX4IO_P_SETUP_ARMING)
|
||||
bool arming_io_arm_ok
|
||||
bool arming_failsafe_custom
|
||||
bool arming_fmu_armed
|
||||
bool arming_fmu_prearmed
|
||||
bool arming_manual_override_ok
|
||||
bool arming_failsafe_custom
|
||||
bool arming_inair_restart_ok
|
||||
bool arming_always_pwm_enable
|
||||
bool arming_rc_handling_disabled
|
||||
bool arming_lockdown
|
||||
bool arming_force_failsafe
|
||||
bool arming_io_arm_ok
|
||||
bool arming_lockdown
|
||||
bool arming_termination_failsafe
|
||||
bool arming_override_immediate
|
||||
|
||||
uint16[8] pwm
|
||||
uint16[8] pwm_disarmed
|
||||
uint16[8] pwm_failsafe
|
||||
|
||||
int16[8] actuators
|
||||
uint16[8] servos
|
||||
uint16[8] pwm_rate_hz
|
||||
|
||||
uint16[18] raw_inputs
|
||||
|
|
|
|||
|
|
@ -4,41 +4,36 @@ uint8 FUNCTION_THROTTLE = 0
|
|||
uint8 FUNCTION_ROLL = 1
|
||||
uint8 FUNCTION_PITCH = 2
|
||||
uint8 FUNCTION_YAW = 3
|
||||
uint8 FUNCTION_MODE = 4
|
||||
uint8 FUNCTION_RETURN = 5
|
||||
uint8 FUNCTION_POSCTL = 6
|
||||
uint8 FUNCTION_LOITER = 7
|
||||
uint8 FUNCTION_OFFBOARD = 8
|
||||
uint8 FUNCTION_ACRO = 9
|
||||
uint8 FUNCTION_FLAPS = 10
|
||||
uint8 FUNCTION_AUX_1 = 11
|
||||
uint8 FUNCTION_AUX_2 = 12
|
||||
uint8 FUNCTION_AUX_3 = 13
|
||||
uint8 FUNCTION_AUX_4 = 14
|
||||
uint8 FUNCTION_AUX_5 = 15
|
||||
uint8 FUNCTION_PARAM_1 = 16
|
||||
uint8 FUNCTION_PARAM_2 = 17
|
||||
uint8 FUNCTION_PARAM_3_5 = 18
|
||||
uint8 FUNCTION_KILLSWITCH = 19
|
||||
uint8 FUNCTION_TRANSITION = 20
|
||||
uint8 FUNCTION_GEAR = 21
|
||||
uint8 FUNCTION_ARMSWITCH = 22
|
||||
uint8 FUNCTION_STAB = 23
|
||||
uint8 FUNCTION_AUX_6 = 24
|
||||
uint8 FUNCTION_MAN = 25
|
||||
uint8 FUNCTION_FLTBTN_SLOT_1 = 26
|
||||
uint8 FUNCTION_FLTBTN_SLOT_2 = 27
|
||||
uint8 FUNCTION_FLTBTN_SLOT_3 = 28
|
||||
uint8 FUNCTION_FLTBTN_SLOT_4 = 39
|
||||
uint8 FUNCTION_FLTBTN_SLOT_5 = 30
|
||||
uint8 FUNCTION_FLTBTN_SLOT_6 = 31
|
||||
uint8 FUNCTION_RETURN = 4
|
||||
uint8 FUNCTION_LOITER = 5
|
||||
uint8 FUNCTION_OFFBOARD = 6
|
||||
uint8 FUNCTION_FLAPS = 7
|
||||
uint8 FUNCTION_AUX_1 = 8
|
||||
uint8 FUNCTION_AUX_2 = 9
|
||||
uint8 FUNCTION_AUX_3 = 10
|
||||
uint8 FUNCTION_AUX_4 = 11
|
||||
uint8 FUNCTION_AUX_5 = 12
|
||||
uint8 FUNCTION_AUX_6 = 13
|
||||
uint8 FUNCTION_PARAM_1 = 14
|
||||
uint8 FUNCTION_PARAM_2 = 15
|
||||
uint8 FUNCTION_PARAM_3_5 = 16
|
||||
uint8 FUNCTION_KILLSWITCH = 17
|
||||
uint8 FUNCTION_TRANSITION = 18
|
||||
uint8 FUNCTION_GEAR = 19
|
||||
uint8 FUNCTION_ARMSWITCH = 20
|
||||
uint8 FUNCTION_FLTBTN_SLOT_1 = 21
|
||||
uint8 FUNCTION_FLTBTN_SLOT_2 = 22
|
||||
uint8 FUNCTION_FLTBTN_SLOT_3 = 23
|
||||
uint8 FUNCTION_FLTBTN_SLOT_4 = 24
|
||||
uint8 FUNCTION_FLTBTN_SLOT_5 = 25
|
||||
uint8 FUNCTION_FLTBTN_SLOT_6 = 26
|
||||
|
||||
uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6
|
||||
|
||||
uint64 timestamp_last_valid # Timestamp of last valid RC signal
|
||||
float32[18] channels # Scaled to -1..1 (throttle: 0..1)
|
||||
uint8 channel_count # Number of valid channels
|
||||
int8[32] function # Functions mapping
|
||||
int8[27] function # Functions mapping
|
||||
uint8 rssi # Receive signal strength index
|
||||
bool signal_lost # Control signal lost, should be checked together with topic timeout
|
||||
uint32 frame_drop_count # Number of dropped frames
|
||||
|
|
|
|||
|
|
@ -1,5 +1,3 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
bool safety_switch_available # Set to true if a safety switch is connected
|
||||
bool safety_off # Set to true if safety is off
|
||||
bool override_available # Set to true if external override system is connected
|
||||
bool override_enabled # Set to true if override is engaged
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
bool flag_armed # synonym for actuator_armed.armed
|
||||
|
||||
bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing
|
||||
bool flag_multicopter_position_control_enabled
|
||||
|
||||
bool flag_control_manual_enabled # true if manual input is mixed in
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -77,16 +77,16 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};
|
|||
|
||||
static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19};
|
||||
|
||||
static constexpr wq_config_t UART0{"wq:UART0", 1536, -21};
|
||||
static constexpr wq_config_t UART1{"wq:UART1", 1536, -22};
|
||||
static constexpr wq_config_t UART2{"wq:UART2", 1536, -23};
|
||||
static constexpr wq_config_t UART3{"wq:UART3", 1536, -24};
|
||||
static constexpr wq_config_t UART4{"wq:UART4", 1536, -25};
|
||||
static constexpr wq_config_t UART5{"wq:UART5", 1536, -26};
|
||||
static constexpr wq_config_t UART6{"wq:UART6", 1536, -27};
|
||||
static constexpr wq_config_t UART7{"wq:UART7", 1536, -28};
|
||||
static constexpr wq_config_t UART8{"wq:UART8", 1536, -29};
|
||||
static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1536, -30};
|
||||
static constexpr wq_config_t UART0{"wq:UART0", 1632, -21};
|
||||
static constexpr wq_config_t UART1{"wq:UART1", 1632, -22};
|
||||
static constexpr wq_config_t UART2{"wq:UART2", 1632, -23};
|
||||
static constexpr wq_config_t UART3{"wq:UART3", 1632, -24};
|
||||
static constexpr wq_config_t UART4{"wq:UART4", 1632, -25};
|
||||
static constexpr wq_config_t UART5{"wq:UART5", 1632, -26};
|
||||
static constexpr wq_config_t UART6{"wq:UART6", 1632, -27};
|
||||
static constexpr wq_config_t UART7{"wq:UART7", 1632, -28};
|
||||
static constexpr wq_config_t UART8{"wq:UART8", 1632, -29};
|
||||
static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1632, -30};
|
||||
|
||||
static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};
|
||||
|
||||
|
|
|
|||
|
|
@ -8,7 +8,6 @@ set(tests
|
|||
bezier
|
||||
bitset
|
||||
bson
|
||||
conv
|
||||
dataman
|
||||
file2
|
||||
float
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
|
||||
param load
|
||||
param set CBRK_SUPPLY_CHK 894281
|
||||
param set SYS_RESTART_TYPE 0
|
||||
|
||||
dataman start
|
||||
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
|
||||
param load
|
||||
param set CBRK_SUPPLY_CHK 894281
|
||||
param set SYS_RESTART_TYPE 0
|
||||
|
||||
dataman start
|
||||
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
|
||||
param load
|
||||
param set CBRK_SUPPLY_CHK 894281
|
||||
param set SYS_RESTART_TYPE 0
|
||||
|
||||
dataman start
|
||||
|
||||
|
|
|
|||
|
|
@ -9,7 +9,6 @@ param set BAT1_N_CELLS 3
|
|||
param set CBRK_SUPPLY_CHK 894281
|
||||
param set MAV_TYPE 22
|
||||
param set VT_TYPE 2
|
||||
param set SYS_RESTART_TYPE 0
|
||||
|
||||
dataman start
|
||||
|
||||
|
|
|
|||
|
|
@ -5,7 +5,6 @@
|
|||
|
||||
param load
|
||||
param set CBRK_SUPPLY_CHK 894281
|
||||
param set SYS_RESTART_TYPE 0
|
||||
|
||||
dataman start
|
||||
|
||||
|
|
|
|||
|
|
@ -215,9 +215,6 @@ typedef uint16_t servo_position_t;
|
|||
/** force safety switch on (to enable use of safety switch) */
|
||||
#define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 28)
|
||||
|
||||
/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
|
||||
#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _PX4_IOC(_PWM_SERVO_BASE, 32)
|
||||
|
||||
/** set auxillary output mode */
|
||||
#define PWM_SERVO_ENTER_TEST_MODE 18
|
||||
#define PWM_SERVO_EXIT_TEST_MODE 19
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
@ -42,7 +42,7 @@ px4_add_module(
|
|||
DEPENDS
|
||||
arch_px4io_serial
|
||||
circuit_breaker
|
||||
mixer
|
||||
mixer_module
|
||||
)
|
||||
|
||||
# include the px4io binary in ROMFS
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -45,11 +45,10 @@
|
|||
/**
|
||||
* Set usage of IO board
|
||||
*
|
||||
* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
|
||||
* Can be used to use a configure the use of the IO board.
|
||||
*
|
||||
* @boolean
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @value 0 IO PWM disabled (RC only)
|
||||
* @value 1 IO enabled (RC & PWM)
|
||||
* @reboot_required true
|
||||
* @group System
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -51,24 +51,14 @@ device::Device
|
|||
|
||||
PX4IO_serial::PX4IO_serial() :
|
||||
Device("PX4IO_serial"),
|
||||
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns")),
|
||||
#if 0
|
||||
_pc_retries(perf_alloc(PC_COUNT, "io_retries ")),
|
||||
_pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")),
|
||||
_pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")),
|
||||
_pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")),
|
||||
_pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")),
|
||||
_pc_idle(perf_alloc(PC_COUNT, "io_idle ")),
|
||||
_pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")),
|
||||
#else
|
||||
_pc_retries(nullptr),
|
||||
_pc_timeouts(nullptr),
|
||||
_pc_crcerrs(nullptr),
|
||||
_pc_protoerrs(nullptr),
|
||||
_pc_uerrs(nullptr),
|
||||
_pc_idle(nullptr),
|
||||
_pc_badidle(nullptr),
|
||||
#endif
|
||||
_pc_txns(perf_alloc(PC_ELAPSED, MODULE_NAME": txns")),
|
||||
_pc_retries(perf_alloc(PC_COUNT, MODULE_NAME": retries")),
|
||||
_pc_timeouts(perf_alloc(PC_COUNT, MODULE_NAME": timeouts")),
|
||||
_pc_crcerrs(perf_alloc(PC_COUNT, MODULE_NAME": crcerrs")),
|
||||
_pc_protoerrs(perf_alloc(PC_COUNT, MODULE_NAME": protoerrs")),
|
||||
_pc_uerrs(perf_alloc(PC_COUNT, MODULE_NAME": uarterrs")),
|
||||
_pc_idle(perf_alloc(PC_COUNT, MODULE_NAME": idle")),
|
||||
_pc_badidle(perf_alloc(PC_COUNT, MODULE_NAME": badidle")),
|
||||
_bus_semaphore(SEM_INITIALIZER(0))
|
||||
{
|
||||
g_interface = this;
|
||||
|
|
|
|||
|
|
@ -355,9 +355,8 @@ int
|
|||
PX4IO_Uploader::get_sync(unsigned timeout)
|
||||
{
|
||||
uint8_t c[2];
|
||||
int ret;
|
||||
|
||||
ret = recv_byte_with_timeout(c, timeout);
|
||||
int ret = recv_byte_with_timeout(c, timeout);
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
|
|
@ -395,13 +394,11 @@ PX4IO_Uploader::sync()
|
|||
int
|
||||
PX4IO_Uploader::get_info(int param, uint32_t &val)
|
||||
{
|
||||
int ret;
|
||||
|
||||
send(PROTO_GET_DEVICE);
|
||||
send(param);
|
||||
send(PROTO_EOC);
|
||||
|
||||
ret = recv_bytes((uint8_t *)&val, sizeof(val));
|
||||
int ret = recv_bytes((uint8_t *)&val, sizeof(val));
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
|
|
@ -419,7 +416,6 @@ PX4IO_Uploader::erase()
|
|||
return get_sync(10000); /* allow 10s timeout */
|
||||
}
|
||||
|
||||
|
||||
static int read_with_retry(int fd, void *buf, size_t n)
|
||||
{
|
||||
int ret;
|
||||
|
|
|
|||
|
|
@ -509,7 +509,7 @@ void RCInput::Run()
|
|||
|
||||
for (unsigned i = 0; i < (unsigned)newBytes; i++) {
|
||||
/* set updated flag if one complete packet was parsed */
|
||||
st24_rssi = RC_INPUT_RSSI_MAX;
|
||||
st24_rssi = input_rc_s::RSSI_MAX;
|
||||
rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
|
||||
&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
|
||||
}
|
||||
|
|
@ -561,7 +561,7 @@ void RCInput::Run()
|
|||
|
||||
for (unsigned i = 0; i < (unsigned)newBytes; i++) {
|
||||
/* set updated flag if one complete packet was parsed */
|
||||
sumd_rssi = RC_INPUT_RSSI_MAX;
|
||||
sumd_rssi = input_rc_s::RSSI_MAX;
|
||||
rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
|
||||
&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -38,7 +38,6 @@
|
|||
#include <board_config.h>
|
||||
#include <drivers/drv_adc.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <lib/rc/crsf.h>
|
||||
#include <lib/rc/ghst.hpp>
|
||||
|
|
|
|||
|
|
@ -46,7 +46,6 @@
|
|||
#include <px4_platform_common/getopt.h>
|
||||
|
||||
#include <lib/rc/dsm.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
|
|
|
|||
|
|
@ -32,5 +32,5 @@
|
|||
############################################################################
|
||||
|
||||
px4_add_library(mixer_module mixer_module.cpp)
|
||||
|
||||
target_link_libraries(mixer_module PRIVATE output_limit)
|
||||
target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
|
|
|
|||
|
|
@ -89,7 +89,11 @@ MixingOutput::~MixingOutput()
|
|||
void MixingOutput::printStatus() const
|
||||
{
|
||||
perf_print_counter(_control_latency_perf);
|
||||
PX4_INFO("Switched to rate_ctrl work queue: %i", (int)_wq_switched);
|
||||
|
||||
if (_wq_switched) {
|
||||
PX4_INFO("Switched to rate_ctrl work queue");
|
||||
}
|
||||
|
||||
PX4_INFO("Mixer loaded: %s", _mixers ? "yes" : "no");
|
||||
PX4_INFO("Driver instance: %i", _driver_instance);
|
||||
|
||||
|
|
|
|||
|
|
@ -50,6 +50,7 @@
|
|||
#include "sbus.h"
|
||||
#include "common_rc.h"
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
|
|
@ -689,13 +690,6 @@ sbus_decode(uint64_t frame_time, uint8_t *frame, uint16_t *values, uint16_t *num
|
|||
*/
|
||||
void sbus1_set_output_rate_hz(uint16_t rate_hz)
|
||||
{
|
||||
if (rate_hz > SBUS1_MAX_RATE_HZ) {
|
||||
rate_hz = SBUS1_MAX_RATE_HZ;
|
||||
}
|
||||
|
||||
if (rate_hz < SBUS1_MIN_RATE_HZ) {
|
||||
rate_hz = SBUS1_MIN_RATE_HZ;
|
||||
}
|
||||
|
||||
sbus1_frame_delay = (1000U * 1000U) / rate_hz;
|
||||
sbus1_frame_delay = (1000U * 1000U) / math::constrain(rate_hz, (uint16_t)SBUS1_MIN_RATE_HZ,
|
||||
(uint16_t)SBUS1_MAX_RATE_HZ);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -83,22 +83,6 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
|
|||
*/
|
||||
PARAM_DEFINE_INT32(SYS_HITL, 0);
|
||||
|
||||
/**
|
||||
* Set restart type
|
||||
*
|
||||
* Set by px4io to indicate type of restart
|
||||
*
|
||||
* @min 0
|
||||
* @max 2
|
||||
* @value 0 Data survives resets
|
||||
* @value 1 Data survives in-flight resets only
|
||||
* @value 2 Data does not survive reset
|
||||
* @category system
|
||||
* @volatile
|
||||
* @group System
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
|
||||
|
||||
/**
|
||||
* Set multicopter estimator group
|
||||
*
|
||||
|
|
|
|||
|
|
@ -2467,7 +2467,7 @@ Commander::run()
|
|||
}
|
||||
|
||||
// evaluate the main state machine according to mode switches
|
||||
if (set_main_state(_status_changed) == TRANSITION_CHANGED) {
|
||||
if (set_main_state() == TRANSITION_CHANGED) {
|
||||
// play tune on mode change only if armed, blink LED always
|
||||
tune_positive(_armed.armed);
|
||||
_status_changed = true;
|
||||
|
|
@ -3074,29 +3074,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
|||
_leds_counter++;
|
||||
}
|
||||
|
||||
transition_result_t
|
||||
Commander::set_main_state(bool &changed)
|
||||
{
|
||||
if (_safety.override_available && _safety.override_enabled) {
|
||||
return set_main_state_override_on(changed);
|
||||
|
||||
} else {
|
||||
return set_main_state_rc();
|
||||
}
|
||||
}
|
||||
|
||||
transition_result_t
|
||||
Commander::set_main_state_override_on(bool &changed)
|
||||
{
|
||||
const transition_result_t res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags,
|
||||
_internal_state);
|
||||
changed = (res == TRANSITION_CHANGED);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
transition_result_t
|
||||
Commander::set_main_state_rc()
|
||||
transition_result_t Commander::set_main_state()
|
||||
{
|
||||
if ((_manual_control_switches.timestamp == 0)
|
||||
|| (_manual_control_switches.timestamp == _last_manual_control_switches.timestamp)) {
|
||||
|
|
@ -3113,14 +3091,8 @@ Commander::set_main_state_rc()
|
|||
bool should_evaluate_rc_mode_switch =
|
||||
(_last_manual_control_switches.offboard_switch != _manual_control_switches.offboard_switch)
|
||||
|| (_last_manual_control_switches.return_switch != _manual_control_switches.return_switch)
|
||||
|| (_last_manual_control_switches.mode_switch != _manual_control_switches.mode_switch)
|
||||
|| (_last_manual_control_switches.acro_switch != _manual_control_switches.acro_switch)
|
||||
|| (_last_manual_control_switches.posctl_switch != _manual_control_switches.posctl_switch)
|
||||
|| (_last_manual_control_switches.loiter_switch != _manual_control_switches.loiter_switch)
|
||||
|| (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot)
|
||||
|| (_last_manual_control_switches.stab_switch != _manual_control_switches.stab_switch)
|
||||
|| (_last_manual_control_switches.man_switch != _manual_control_switches.man_switch);
|
||||
|
||||
|| (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot);
|
||||
|
||||
if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
// if already armed don't evaluate first time RC
|
||||
|
|
@ -3216,84 +3188,6 @@ Commander::set_main_state_rc()
|
|||
}
|
||||
|
||||
return res;
|
||||
|
||||
} else if (_manual_control_switches.mode_switch != manual_control_switches_s::SWITCH_POS_NONE) {
|
||||
/* offboard and RTL switches off or denied, check main mode switch */
|
||||
switch (_manual_control_switches.mode_switch) {
|
||||
case manual_control_switches_s::SWITCH_POS_NONE:
|
||||
res = TRANSITION_NOT_CHANGED;
|
||||
break;
|
||||
|
||||
case manual_control_switches_s::SWITCH_POS_OFF: // MANUAL
|
||||
if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_NONE &&
|
||||
_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
|
||||
/*
|
||||
* Legacy mode:
|
||||
* Acro switch being used as stabilized switch in FW.
|
||||
*/
|
||||
if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
/* manual mode is stabilized already for multirotors, so switch to acro
|
||||
* for any non-manual mode
|
||||
*/
|
||||
if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_status.is_vtol) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state);
|
||||
|
||||
} else if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
|
||||
|
||||
} else {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
|
||||
}
|
||||
|
||||
} else {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* New mode:
|
||||
* - Acro is Acro
|
||||
* - Manual is not default anymore when the manual switch is assigned
|
||||
*/
|
||||
if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
|
||||
|
||||
} else if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, _internal_state);
|
||||
|
||||
} else if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
|
||||
|
||||
} else if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
|
||||
// default to MANUAL when no manual switch is set
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state);
|
||||
|
||||
} else {
|
||||
// default to STAB when the manual switch is assigned (but off)
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, _internal_state);
|
||||
}
|
||||
}
|
||||
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
case manual_control_switches_s::SWITCH_POS_MIDDLE: // ASSIST
|
||||
if (_manual_control_switches.posctl_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = try_mode_change(commander_state_s::MAIN_STATE_POSCTL);
|
||||
|
||||
} else {
|
||||
res = try_mode_change(commander_state_s::MAIN_STATE_ALTCTL);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case manual_control_switches_s::SWITCH_POS_ON: // AUTO
|
||||
res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_MISSION);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return res;
|
||||
|
|
@ -3374,9 +3268,6 @@ Commander::update_control_mode()
|
|||
/* set vehicle_control_mode according to set_navigation_state */
|
||||
_vehicle_control_mode.flag_armed = _armed.armed;
|
||||
|
||||
_vehicle_control_mode.flag_external_manual_override_ok =
|
||||
(_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol);
|
||||
|
||||
switch (_status.nav_state) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
|
||||
_vehicle_control_mode.flag_control_manual_enabled = true;
|
||||
|
|
@ -3409,10 +3300,6 @@ Commander::update_control_mode()
|
|||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
/* override is not ok for the RTL and recovery mode */
|
||||
_vehicle_control_mode.flag_external_manual_override_ok = false;
|
||||
|
||||
/* fallthrough */
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
|
||||
|
|
|
|||
|
|
@ -173,14 +173,8 @@ private:
|
|||
|
||||
void UpdateEstimateValidity();
|
||||
|
||||
// Set the main system state based on RC and override device inputs
|
||||
transition_result_t set_main_state(bool &changed);
|
||||
|
||||
// Enable override (manual reversion mode) on the system
|
||||
transition_result_t set_main_state_override_on(bool &changed);
|
||||
|
||||
// Set the system main state based on the current RC inputs
|
||||
transition_result_t set_main_state_rc();
|
||||
transition_result_t set_main_state();
|
||||
|
||||
bool shutdown_if_allowed();
|
||||
|
||||
|
|
|
|||
|
|
@ -40,10 +40,9 @@
|
|||
* @author Thomas Gubler <thomas@px4.io>
|
||||
*/
|
||||
|
||||
#include <airspeed/airspeed.h>
|
||||
#include <conversion/rotation.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
#include <lib/airspeed/airspeed.h>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <lib/systemlib/px4_macros.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
|
|
@ -1889,7 +1888,7 @@ MavlinkReceiver::handle_message_rc_channels(mavlink_message_t *msg)
|
|||
input_rc_s rc{};
|
||||
|
||||
rc.timestamp_last_signal = hrt_absolute_time();
|
||||
rc.rssi = RC_INPUT_RSSI_MAX;
|
||||
rc.rssi = input_rc_s::RSSI_MAX;
|
||||
|
||||
// TODO: fake RSSI from dropped messages?
|
||||
// for (auto &component_state : _component_states) {
|
||||
|
|
@ -1959,7 +1958,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
|
|||
// metadata
|
||||
rc.timestamp = hrt_absolute_time();
|
||||
rc.timestamp_last_signal = rc.timestamp;
|
||||
rc.rssi = RC_INPUT_RSSI_MAX;
|
||||
rc.rssi = input_rc_s::RSSI_MAX;
|
||||
rc.rc_failsafe = false;
|
||||
rc.rc_lost = false;
|
||||
rc.rc_lost_frame_count = 0;
|
||||
|
|
@ -2031,7 +2030,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
|||
rc.rc_total_frame_count = 1;
|
||||
rc.rc_ppm_frame_length = 0;
|
||||
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
|
||||
rc.rssi = RC_INPUT_RSSI_MAX;
|
||||
rc.rssi = input_rc_s::RSSI_MAX;
|
||||
|
||||
rc.values[0] = man.x / 2 + 1500; // roll
|
||||
rc.values[1] = man.y / 2 + 1500; // pitch
|
||||
|
|
|
|||
|
|
@ -78,11 +78,8 @@ private:
|
|||
if (_manual_control_switches_sub.copy(&manual_control_switches)) {
|
||||
unsigned shift = 2;
|
||||
msg.buttons = 0;
|
||||
msg.buttons |= (manual_control_switches.mode_switch << (shift * 0));
|
||||
msg.buttons |= (manual_control_switches.return_switch << (shift * 1));
|
||||
msg.buttons |= (manual_control_switches.posctl_switch << (shift * 2));
|
||||
msg.buttons |= (manual_control_switches.loiter_switch << (shift * 3));
|
||||
msg.buttons |= (manual_control_switches.acro_switch << (shift * 4));
|
||||
msg.buttons |= (manual_control_switches.offboard_switch << (shift * 5));
|
||||
msg.buttons |= (manual_control_switches.kill_switch << (shift * 6));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -51,9 +51,7 @@ target_link_libraries(px4iofirmware
|
|||
nuttx_apps
|
||||
nuttx_arch
|
||||
nuttx_c
|
||||
mixer
|
||||
rc
|
||||
output_limit
|
||||
)
|
||||
|
||||
if(PX4IO_PERF)
|
||||
|
|
|
|||
|
|
@ -43,12 +43,12 @@
|
|||
#include <stdbool.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <systemlib/ppm_decode.h>
|
||||
#include <rc/st24.h>
|
||||
#include <rc/sumd.h>
|
||||
#include <rc/sbus.h>
|
||||
#include <rc/dsm.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
|
||||
#if defined(PX4IO_PERF)
|
||||
# include <perf/perf_counter.h>
|
||||
|
|
@ -56,9 +56,6 @@
|
|||
|
||||
#include "px4io.h"
|
||||
|
||||
#define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */
|
||||
#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */
|
||||
|
||||
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
|
||||
static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated);
|
||||
|
||||
|
|
@ -71,13 +68,11 @@ static perf_counter_t c_gather_ppm;
|
|||
static int _dsm_fd = -1;
|
||||
int _sbus_fd = -1;
|
||||
|
||||
static uint16_t rc_value_override = 0;
|
||||
|
||||
#ifdef ADC_RSSI
|
||||
static unsigned _rssi_adc_counts = 0;
|
||||
#endif
|
||||
|
||||
/* receive signal strenght indicator (RSSI). 0 = no connection, 100 (RC_INPUT_RSSI_MAX): perfect connection */
|
||||
/* receive signal strenght indicator (RSSI). 0 = no connection, 100 (input_rc_s::RSSI_MAX): perfect connection */
|
||||
/* Note: this is static because RC-provided telemetry does not occur every tick */
|
||||
static uint16_t _rssi = 0;
|
||||
static unsigned _frame_drops = 0;
|
||||
|
|
@ -136,7 +131,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
|
|||
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SUMD))) {
|
||||
for (unsigned i = 0; i < n_bytes; i++) {
|
||||
/* set updated flag if one complete packet was parsed */
|
||||
st24_rssi = RC_INPUT_RSSI_MAX;
|
||||
st24_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX;
|
||||
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count,
|
||||
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
|
||||
}
|
||||
|
|
@ -166,7 +161,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
|
|||
if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24))) {
|
||||
for (unsigned i = 0; i < n_bytes; i++) {
|
||||
/* set updated flag if one complete packet was parsed */
|
||||
sumd_rssi = RC_INPUT_RSSI_MAX;
|
||||
sumd_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX;
|
||||
*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
|
||||
&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state));
|
||||
}
|
||||
|
|
@ -204,19 +199,6 @@ controls_init(void)
|
|||
/* S.bus input (USART3) */
|
||||
_sbus_fd = sbus_init("/dev/ttyS2", false);
|
||||
|
||||
/* default to a 1:1 input map, all enabled */
|
||||
for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
|
||||
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
|
||||
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
}
|
||||
|
||||
#if defined(PX4IO_PERF)
|
||||
c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
|
||||
c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
|
||||
|
|
@ -246,11 +228,11 @@ controls_tick()
|
|||
_rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f);
|
||||
/* use 1:1 scaling on 3.3V, 12-Bit ADC input */
|
||||
unsigned mV = _rssi_adc_counts * 3300 / 4095;
|
||||
/* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */
|
||||
_rssi = (mV * RC_INPUT_RSSI_MAX / 3300);
|
||||
/* scale to 0..100 (input_rc_s::RSSI_MAX == 100) */
|
||||
_rssi = (mV * INPUT_RC_RSSI_MAX / 3300);
|
||||
|
||||
if (_rssi > RC_INPUT_RSSI_MAX) {
|
||||
_rssi = RC_INPUT_RSSI_MAX;
|
||||
if (_rssi > INPUT_RC_RSSI_MAX) {
|
||||
_rssi = INPUT_RC_RSSI_MAX;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -276,11 +258,11 @@ controls_tick()
|
|||
if (sbus_updated) {
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS);
|
||||
|
||||
unsigned sbus_rssi = RC_INPUT_RSSI_MAX;
|
||||
unsigned sbus_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX
|
||||
|
||||
if (sbus_frame_drop) {
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
|
||||
sbus_rssi = RC_INPUT_RSSI_MAX / 2;
|
||||
sbus_rssi = INPUT_RC_RSSI_MAX / 2;
|
||||
|
||||
} else {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
|
||||
|
|
@ -297,7 +279,6 @@ controls_tick()
|
|||
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
|
||||
_rssi = sbus_rssi;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -377,103 +358,6 @@ controls_tick()
|
|||
/* update RC-received timestamp */
|
||||
system_state.rc_channels_timestamp_received = hrt_absolute_time();
|
||||
|
||||
/* map raw inputs to mapped inputs */
|
||||
/* XXX mapping should be atomic relative to protocol */
|
||||
for (unsigned i = 0; i < r_raw_rc_count; i++) {
|
||||
|
||||
/* map the input channel */
|
||||
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
||||
|
||||
uint16_t raw = r_raw_rc_values[i];
|
||||
|
||||
int16_t scaled;
|
||||
|
||||
/*
|
||||
* 1) Constrain to min/max values, as later processing depends on bounds.
|
||||
*/
|
||||
if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) {
|
||||
raw = conf[PX4IO_P_RC_CONFIG_MIN];
|
||||
}
|
||||
|
||||
if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) {
|
||||
raw = conf[PX4IO_P_RC_CONFIG_MAX];
|
||||
}
|
||||
|
||||
/*
|
||||
* 2) Scale around the mid point differently for lower and upper range.
|
||||
*
|
||||
* This is necessary as they don't share the same endpoints and slope.
|
||||
*
|
||||
* First normalize to 0..1 range with correct sign (below or above center),
|
||||
* then scale to 20000 range (if center is an actual center, -10000..10000,
|
||||
* if parameters only support half range, scale to 10000 range, e.g. if
|
||||
* center == min 0..10000, if center == max -10000..0).
|
||||
*
|
||||
* As the min and max bounds were enforced in step 1), division by zero
|
||||
* cannot occur, as for the case of center == min or center == max the if
|
||||
* statement is mutually exclusive with the arithmetic NaN case.
|
||||
*
|
||||
* DO NOT REMOVE OR ALTER STEP 1!
|
||||
*/
|
||||
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
|
||||
conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
|
||||
|
||||
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
|
||||
conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
|
||||
|
||||
} else {
|
||||
/* in the configured dead zone, output zero */
|
||||
scaled = 0;
|
||||
}
|
||||
|
||||
/* invert channel if requested */
|
||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
|
||||
scaled = -scaled;
|
||||
}
|
||||
|
||||
/* and update the scaled/mapped version */
|
||||
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
||||
|
||||
if (mapped < PX4IO_CONTROL_CHANNELS) {
|
||||
|
||||
/* invert channel if pitch - pulling the lever down means pitching up by convention */
|
||||
if (mapped == 1) {
|
||||
/* roll, pitch, yaw, throttle, override is the standard order */
|
||||
scaled = -scaled;
|
||||
}
|
||||
|
||||
if (mapped == 3 && r_setup_rc_thr_failsafe) {
|
||||
/* throttle failsafe detection */
|
||||
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
|
||||
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
|
||||
|
||||
} else {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
|
||||
}
|
||||
}
|
||||
|
||||
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
|
||||
assigned_channels |= (1 << mapped);
|
||||
|
||||
} else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) {
|
||||
/* pick out override channel, indicated by special mapping */
|
||||
rc_value_override = SIGNED_TO_REG(scaled);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* set un-assigned controls to zero */
|
||||
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
|
||||
if (!(assigned_channels & (1 << i))) {
|
||||
r_rc_values[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* set RC OK flag, as we got an update */
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK);
|
||||
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK;
|
||||
|
|
@ -485,11 +369,6 @@ controls_tick()
|
|||
} else {
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
|
||||
}
|
||||
|
||||
/*
|
||||
* Export the valid channel bitmap
|
||||
*/
|
||||
r_rc_valid = assigned_channels;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -513,85 +392,20 @@ controls_tick()
|
|||
* Handle losing RC input
|
||||
*/
|
||||
|
||||
/* if we are in failsafe, clear the override flag */
|
||||
if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE);
|
||||
}
|
||||
|
||||
/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
|
||||
if (rc_input_lost) {
|
||||
/* Clear the RC input status flag, clear manual override flag */
|
||||
atomic_modify_clear(&r_status_flags, (
|
||||
PX4IO_P_STATUS_FLAGS_OVERRIDE |
|
||||
PX4IO_P_STATUS_FLAGS_RC_OK));
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK);
|
||||
|
||||
/* flag raw RC as lost */
|
||||
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
|
||||
|
||||
/* Mark all channels as invalid, as we just lost the RX */
|
||||
r_rc_valid = 0;
|
||||
|
||||
/* Set raw channel count to zero */
|
||||
r_raw_rc_count = 0;
|
||||
|
||||
/* Set the RC_LOST alarm */
|
||||
atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_RC_LOST);
|
||||
}
|
||||
|
||||
/*
|
||||
* Check for manual override.
|
||||
*
|
||||
* Firstly, manual override must be enabled, RC input available and a mixer loaded.
|
||||
*/
|
||||
if (/* condition 1: Override is always allowed */
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
|
||||
/* condition 2: We have valid RC control inputs from the user */
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
|
||||
/* condition 3: The system didn't go already into failsafe mode with fixed outputs */
|
||||
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) &&
|
||||
/* condition 4: RC handling wasn't generally disabled */
|
||||
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
|
||||
/* condition 5: We have a valid mixer to map RC inputs to actuator outputs */
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
|
||||
bool override = false;
|
||||
|
||||
/*
|
||||
* Check mapped channel 5 (can be any remote channel,
|
||||
* depends on RC_MAP_OVER parameter);
|
||||
* If the value is 'high' then the pilot has
|
||||
* requested override.
|
||||
*
|
||||
*/
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
|
||||
(REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) {
|
||||
override = true;
|
||||
}
|
||||
|
||||
/*
|
||||
* If the FMU is dead then enable override if we have a mixer
|
||||
* and we want to immediately override (instead of using the RC channel
|
||||
* as in the case above.
|
||||
*
|
||||
* Also, do not enter manual override if we asked for termination
|
||||
* failsafe and FMU is lost.
|
||||
*/
|
||||
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
|
||||
!(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
|
||||
override = true;
|
||||
}
|
||||
|
||||
if (override) {
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE);
|
||||
|
||||
} else {
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE);
|
||||
}
|
||||
|
||||
} else {
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE);
|
||||
}
|
||||
}
|
||||
|
||||
static bool
|
||||
|
|
|
|||
|
|
@ -51,14 +51,8 @@
|
|||
#include <drivers/drv_pwm_output.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <lib/mixer/MixerGroup.hpp>
|
||||
#include <output_limit/output_limit.h>
|
||||
#include <rc/sbus.h>
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
|
||||
#include "mixer.h"
|
||||
|
||||
extern "C" {
|
||||
/* #define DEBUG */
|
||||
#include "px4io.h"
|
||||
|
|
@ -74,7 +68,6 @@ static volatile bool mixer_servos_armed = false;
|
|||
static volatile bool should_arm = false;
|
||||
static volatile bool should_arm_nothrottle = false;
|
||||
static volatile bool should_always_enable_pwm = false;
|
||||
static volatile bool mix_failsafe = false;
|
||||
|
||||
static bool new_fmu_data = false;
|
||||
static uint64_t last_fmu_update = 0;
|
||||
|
|
@ -85,31 +78,12 @@ extern int _sbus_fd;
|
|||
enum mixer_source {
|
||||
MIX_NONE,
|
||||
MIX_DISARMED,
|
||||
MIX_FMU,
|
||||
MIX_OVERRIDE,
|
||||
MIX_FAILSAFE,
|
||||
MIX_OVERRIDE_FMU_OK
|
||||
};
|
||||
|
||||
static volatile mixer_source source;
|
||||
|
||||
static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
|
||||
static int mixer_handle_text_create_mixer();
|
||||
static void mixer_mix_failsafe();
|
||||
|
||||
static MixerGroup mixer_group;
|
||||
|
||||
void
|
||||
mixer_tick()
|
||||
{
|
||||
/* check if the mixer got modified */
|
||||
mixer_handle_text_create_mixer();
|
||||
|
||||
if (mix_failsafe) {
|
||||
mixer_mix_failsafe();
|
||||
mix_failsafe = false;
|
||||
}
|
||||
|
||||
/* check that we are receiving fresh data from the FMU */
|
||||
irqstate_t irq_flags = enter_critical_section();
|
||||
const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time;
|
||||
|
|
@ -123,8 +97,7 @@ mixer_tick()
|
|||
isr_debug(1, "AP RX timeout");
|
||||
}
|
||||
|
||||
atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FMU_OK));
|
||||
atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_FMU_LOST);
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
|
||||
|
||||
} else {
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
|
||||
|
|
@ -139,7 +112,7 @@ mixer_tick()
|
|||
}
|
||||
|
||||
/* default to disarmed mixing */
|
||||
source = MIX_DISARMED;
|
||||
mixer_source source = MIX_DISARMED;
|
||||
|
||||
/*
|
||||
* Decide which set of controls we're using.
|
||||
|
|
@ -149,41 +122,11 @@ mixer_tick()
|
|||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
|
||||
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) > 0) {
|
||||
/* a channel based override has been
|
||||
* triggered, with FMU active */
|
||||
source = MIX_OVERRIDE_FMU_OK;
|
||||
|
||||
} else {
|
||||
/* don't actually mix anything - copy values from r_page_direct_pwm */
|
||||
source = MIX_NONE;
|
||||
memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
|
||||
/* mix from FMU controls */
|
||||
source = MIX_FMU;
|
||||
}
|
||||
|
||||
else if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
|
||||
|
||||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
|
||||
|
||||
/* if allowed, mix from RC inputs directly up to available rc channels */
|
||||
source = MIX_OVERRIDE_FMU_OK;
|
||||
|
||||
} else {
|
||||
/* if allowed, mix from RC inputs directly */
|
||||
source = MIX_OVERRIDE;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Decide whether the servos should be armed right now.
|
||||
*
|
||||
|
|
@ -191,30 +134,22 @@ mixer_tick()
|
|||
* FMU or from the mixer.
|
||||
*
|
||||
*/
|
||||
should_arm = (
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
|
||||
should_arm = (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */
|
||||
&& (
|
||||
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */
|
||||
|| (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
|
||||
)
|
||||
);
|
||||
&& (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */
|
||||
;
|
||||
|
||||
should_arm_nothrottle = (
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
|
||||
should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */
|
||||
&& (((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */
|
||||
&& ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */
|
||||
|| (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
|
||||
)
|
||||
);
|
||||
));
|
||||
|
||||
should_always_enable_pwm = (
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)
|
||||
);
|
||||
/* we enable PWM output always on the IO side if FMU is up and running
|
||||
* as zero-outputs can be controlled by FMU by sending a 0 PWM command
|
||||
*/
|
||||
should_always_enable_pwm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK));
|
||||
|
||||
/*
|
||||
* Check if FMU is still alive, if not, terminate the flight
|
||||
|
|
@ -243,100 +178,19 @@ mixer_tick()
|
|||
atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE));
|
||||
}
|
||||
|
||||
/*
|
||||
* Set simple mixer trim values. If the OK flag is set the mixer is fully loaded.
|
||||
*/
|
||||
if (update_trims && r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
|
||||
update_trims = false;
|
||||
mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT);
|
||||
}
|
||||
|
||||
/*
|
||||
* Update air-mode parameter
|
||||
*/
|
||||
mixer_group.set_airmode((Mixer::Airmode)REG_TO_SIGNED(r_setup_airmode));
|
||||
|
||||
|
||||
/*
|
||||
* Run the mixers.
|
||||
*/
|
||||
if (source == MIX_FAILSAFE) {
|
||||
|
||||
/* copy failsafe values to the servo outputs */
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servos[i] = r_page_servo_failsafe[i];
|
||||
|
||||
/* safe actuators for FMU feedback */
|
||||
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
|
||||
}
|
||||
|
||||
} else if (source == MIX_DISARMED) {
|
||||
|
||||
/* copy disarmed values to the servo outputs */
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servos[i] = r_page_servo_disarmed[i];
|
||||
|
||||
/* safe actuators for FMU feedback */
|
||||
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
|
||||
}
|
||||
|
||||
} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)
|
||||
&& !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) {
|
||||
|
||||
float outputs[PX4IO_SERVO_COUNT];
|
||||
unsigned mixed;
|
||||
|
||||
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
|
||||
/* maximum value the outputs of the multirotor mixer are allowed to change in this cycle
|
||||
* factor 2 is needed because actuator outputs are in the range [-1,1]
|
||||
*/
|
||||
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
|
||||
r_setup_slew_max);
|
||||
mixer_group.set_max_delta_out_once(delta_out_max);
|
||||
}
|
||||
|
||||
/* set dt to be used in simple mixer for slew rate limiting */
|
||||
mixer_group.set_dt_once(dt);
|
||||
|
||||
/* update parameter for mc thrust model if it updated */
|
||||
if (update_mc_thrust_param) {
|
||||
mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
|
||||
update_mc_thrust_param = false;
|
||||
}
|
||||
|
||||
/* mix */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
|
||||
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
|
||||
r_mixer_limits = mixer_group.get_saturation_status();
|
||||
|
||||
} else {
|
||||
mixed = 0;
|
||||
}
|
||||
|
||||
/* the pwm limit call takes care of out of band errors */
|
||||
output_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
|
||||
r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
|
||||
/* clamp unused outputs to zero */
|
||||
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servos[i] = 0;
|
||||
outputs[i] = 0.0f;
|
||||
}
|
||||
|
||||
/* store normalized outputs */
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
|
||||
}
|
||||
|
||||
|
||||
if (mixed && new_fmu_data) {
|
||||
new_fmu_data = false;
|
||||
|
||||
/* Trigger all timer's channels in Oneshot mode to fire
|
||||
* the oneshots with updated values.
|
||||
*/
|
||||
|
||||
up_pwm_update();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -372,7 +226,6 @@ mixer_tick()
|
|||
}
|
||||
|
||||
/* set S.BUS1 or S.BUS2 outputs */
|
||||
|
||||
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
|
||||
sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
|
||||
|
||||
|
|
@ -380,8 +233,7 @@ mixer_tick()
|
|||
sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
|
||||
}
|
||||
|
||||
} else if (mixer_servos_armed && (should_always_enable_pwm
|
||||
|| (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
|
||||
} else if (mixer_servos_armed && (should_always_enable_pwm || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
|
||||
/* set the disarmed servo outputs. */
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
|
||||
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
|
||||
|
|
@ -399,283 +251,3 @@ mixer_tick()
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int
|
||||
mixer_callback(uintptr_t handle,
|
||||
uint8_t control_group,
|
||||
uint8_t control_index,
|
||||
float &control)
|
||||
{
|
||||
control = 0.0f;
|
||||
|
||||
if (control_group >= PX4IO_CONTROL_GROUPS) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
switch (source) {
|
||||
case MIX_FMU:
|
||||
if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
|
||||
if (r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)] == INT16_MAX) {
|
||||
//catch NAN values encoded as INT16 max for disarmed outputs
|
||||
control = NAN;
|
||||
|
||||
} else {
|
||||
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
return -1;
|
||||
|
||||
case MIX_OVERRIDE:
|
||||
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
|
||||
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
|
||||
break;
|
||||
}
|
||||
|
||||
return -1;
|
||||
|
||||
case MIX_OVERRIDE_FMU_OK:
|
||||
|
||||
/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
|
||||
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
|
||||
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
|
||||
break;
|
||||
|
||||
} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
|
||||
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
|
||||
break;
|
||||
}
|
||||
|
||||
return -1;
|
||||
|
||||
case MIX_DISARMED:
|
||||
case MIX_FAILSAFE:
|
||||
case MIX_NONE:
|
||||
control = 0.0f;
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* apply trim offsets for override channels */
|
||||
if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) {
|
||||
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
|
||||
control_index == actuator_controls_s::INDEX_ROLL) {
|
||||
control *= REG_TO_FLOAT(r_setup_scale_roll);
|
||||
control += REG_TO_FLOAT(r_setup_trim_roll);
|
||||
|
||||
} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
|
||||
control_index == actuator_controls_s::INDEX_PITCH) {
|
||||
control *= REG_TO_FLOAT(r_setup_scale_pitch);
|
||||
control += REG_TO_FLOAT(r_setup_trim_pitch);
|
||||
|
||||
} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
|
||||
control_index == actuator_controls_s::INDEX_YAW) {
|
||||
control *= REG_TO_FLOAT(r_setup_scale_yaw);
|
||||
control += REG_TO_FLOAT(r_setup_trim_yaw);
|
||||
}
|
||||
}
|
||||
|
||||
/* limit output */
|
||||
control = math::constrain(control, -1.f, 1.f);
|
||||
|
||||
/* motor spinup phase - lock throttle to zero */
|
||||
if ((pwm_limit.state == OUTPUT_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
|
||||
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
|
||||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
|
||||
control_index == actuator_controls_s::INDEX_THROTTLE) {
|
||||
/* limit the throttle output to zero during motor spinup,
|
||||
* as the motors cannot follow any demand yet
|
||||
*/
|
||||
control = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* only safety off, but not armed - set throttle as invalid */
|
||||
if (should_arm_nothrottle && !should_arm) {
|
||||
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
|
||||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
|
||||
control_index == actuator_controls_s::INDEX_THROTTLE) {
|
||||
/* mark the throttle as invalid */
|
||||
control = NAN;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* XXX error handling here should be more aggressive; currently it is
|
||||
* possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has
|
||||
* not loaded faithfully.
|
||||
*/
|
||||
|
||||
static char mixer_text[PX4IO_MAX_MIXER_LENGTH]; /* large enough for one mixer */
|
||||
static unsigned mixer_text_length = 0;
|
||||
static volatile bool mixer_update_pending = false;
|
||||
static volatile bool mixer_reset_pending = false;
|
||||
|
||||
int
|
||||
mixer_handle_text_create_mixer()
|
||||
{
|
||||
/* do not allow a mixer change while safety off and FMU armed */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (mixer_reset_pending) {
|
||||
mixer_group.reset();
|
||||
mixer_reset_pending = false;
|
||||
}
|
||||
|
||||
/* only run on update */
|
||||
if (!mixer_update_pending || (mixer_text_length == 0)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* process the text buffer, adding new mixers as their descriptions can be parsed */
|
||||
unsigned resid = mixer_text_length;
|
||||
mixer_group.load_from_buf(mixer_callback, 0, &mixer_text[0], resid);
|
||||
|
||||
/* if anything was parsed */
|
||||
if (resid != mixer_text_length) {
|
||||
|
||||
isr_debug(2, "used %u", mixer_text_length - resid);
|
||||
|
||||
/* copy any leftover text to the base of the buffer for re-use */
|
||||
if (resid > 0) {
|
||||
memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
|
||||
/* enforce null termination */
|
||||
mixer_text[resid] = '\0';
|
||||
}
|
||||
|
||||
mixer_text_length = resid;
|
||||
}
|
||||
|
||||
mixer_update_pending = false;
|
||||
|
||||
update_trims = true;
|
||||
update_mc_thrust_param = true;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int interrupt_mixer_handle_text(const void *buffer, size_t length)
|
||||
{
|
||||
/* do not allow a mixer change while safety off and FMU armed */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* disable mixing, will be enabled once load is complete */
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK);
|
||||
|
||||
px4io_mixdata *msg = (px4io_mixdata *)buffer;
|
||||
|
||||
isr_debug(2, "mix txt %u", length);
|
||||
|
||||
if (length < sizeof(px4io_mixdata)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
unsigned text_length = length - sizeof(px4io_mixdata);
|
||||
|
||||
switch (msg->action) {
|
||||
case F2I_MIXER_ACTION_RESET:
|
||||
isr_debug(2, "reset");
|
||||
|
||||
/* THEN actually delete it */
|
||||
mixer_reset_pending = true;
|
||||
mixer_text_length = 0;
|
||||
|
||||
/* FALLTHROUGH */
|
||||
case F2I_MIXER_ACTION_APPEND:
|
||||
isr_debug(2, "append %d", length);
|
||||
|
||||
/* check for overflow - this would be really fatal */
|
||||
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* check if the last item has been processed - bail out if not */
|
||||
if (mixer_update_pending) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* append mixer text and nul-terminate, guard against overflow */
|
||||
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
|
||||
mixer_text_length += text_length;
|
||||
mixer_text[mixer_text_length] = '\0';
|
||||
isr_debug(2, "buflen %u", mixer_text_length);
|
||||
|
||||
/* flag the buffer as ready */
|
||||
mixer_update_pending = true;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void interrupt_mixer_set_failsafe()
|
||||
{
|
||||
mix_failsafe = true;
|
||||
}
|
||||
|
||||
void
|
||||
mixer_mix_failsafe()
|
||||
{
|
||||
/*
|
||||
* Check if a custom failsafe value has been written,
|
||||
* or if the mixer is not ok and bail out.
|
||||
*/
|
||||
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* set failsafe defaults to the values for all inputs = 0 */
|
||||
float outputs[PX4IO_SERVO_COUNT];
|
||||
unsigned mixed;
|
||||
|
||||
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
|
||||
/* maximum value the outputs of the multirotor mixer are allowed to change in this cycle
|
||||
* factor 2 is needed because actuator outputs are in the range [-1,1]
|
||||
*/
|
||||
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
|
||||
r_setup_slew_max);
|
||||
mixer_group.set_max_delta_out_once(delta_out_max);
|
||||
}
|
||||
|
||||
/* set dt to be used in simple mixer for slew rate limiting */
|
||||
mixer_group.set_dt_once(dt);
|
||||
|
||||
/* update parameter for mc thrust model if it updated */
|
||||
if (update_mc_thrust_param) {
|
||||
mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
|
||||
update_mc_thrust_param = false;
|
||||
}
|
||||
|
||||
/* mix */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) != 0) {
|
||||
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
|
||||
r_mixer_limits = mixer_group.get_saturation_status();
|
||||
|
||||
} else {
|
||||
mixed = 0;
|
||||
}
|
||||
|
||||
/* scale to PWM and update the servo outputs as required */
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
/* scale to servo output */
|
||||
r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
|
||||
}
|
||||
|
||||
/* disable the rest of the outputs */
|
||||
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servo_failsafe[i] = 0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -61,7 +61,7 @@
|
|||
* the PX4 system are expressed as signed integer values scaled by 10000,
|
||||
* e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and
|
||||
* SIGNED_TO_REG macros to convert between register representation and
|
||||
* the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float.
|
||||
* the signed version.
|
||||
*
|
||||
* Note that the implementation of readable pages prefers registers within
|
||||
* readable pages to be densely packed. Page numbers do not need to be
|
||||
|
|
@ -75,12 +75,9 @@
|
|||
#define REG_TO_SIGNED(_reg) ((int16_t)(_reg))
|
||||
#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed))
|
||||
|
||||
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
|
||||
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f))
|
||||
|
||||
#define REG_TO_BOOL(_reg) ((bool)(_reg))
|
||||
|
||||
#define PX4IO_PROTOCOL_VERSION 4
|
||||
#define PX4IO_PROTOCOL_VERSION 5
|
||||
|
||||
/* maximum allowable sizes on this protocol version */
|
||||
#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
|
||||
|
|
@ -95,7 +92,6 @@
|
|||
#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */
|
||||
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
|
||||
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
|
||||
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */
|
||||
#define PX4IO_MAX_TRANSFER_LEN 64
|
||||
|
||||
/* dynamic status page */
|
||||
|
|
@ -105,40 +101,26 @@
|
|||
|
||||
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
|
||||
#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
|
||||
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */
|
||||
#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */
|
||||
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
|
||||
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
|
||||
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
|
||||
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
|
||||
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 15) /* SUMD input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 1) /* RC input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 2) /* PPM input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 5) /* controls from FMU are valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 6) /* raw PWM from FMU */
|
||||
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 7) /* the arming state between IO and FMU is in sync */
|
||||
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 8) /* initialisation of the IO completed without error */
|
||||
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 9) /* failsafe is active */
|
||||
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 10) /* safety is off */
|
||||
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 11) /* FMU was initialized and OK once */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 12) /* ST24 input is valid */
|
||||
#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 13) /* SUMD input is valid */
|
||||
|
||||
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
|
||||
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
|
||||
#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */
|
||||
#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */
|
||||
#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */
|
||||
#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */
|
||||
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */
|
||||
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */
|
||||
#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
|
||||
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 0) /* timed out waiting for RC input */
|
||||
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 1) /* PWM configuration or output was bad */
|
||||
|
||||
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
|
||||
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
|
||||
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
|
||||
|
||||
#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */
|
||||
|
||||
/* array of post-mix actuator outputs, -10000..10000 */
|
||||
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* array of PWM servo output values, microseconds */
|
||||
#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
|
@ -159,11 +141,6 @@
|
|||
#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */
|
||||
#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */
|
||||
|
||||
/* array of scaled RC input values, -10000..10000 */
|
||||
#define PX4IO_PAGE_RC_INPUT 5
|
||||
#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */
|
||||
#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */
|
||||
|
||||
/* array of raw ADC values */
|
||||
#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
|
||||
|
||||
|
|
@ -176,30 +153,22 @@
|
|||
#define PX4IO_P_SETUP_FEATURES 0
|
||||
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */
|
||||
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */
|
||||
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */
|
||||
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */
|
||||
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 2) /**< enable ADC RSSI parsing */
|
||||
|
||||
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
|
||||
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
|
||||
#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */
|
||||
#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */
|
||||
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 3) /* OK to switch to manual override via override RC channel */
|
||||
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* use custom failsafe values, not 0 values of mixer */
|
||||
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */
|
||||
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
|
||||
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 7) /* Disable the IO-internal evaluation of the RC */
|
||||
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 8) /* If set, the system operates normally, but won't actuate any servos */
|
||||
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 9) /* If set, the system will always output the failsafe values */
|
||||
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
|
||||
#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
|
||||
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values */
|
||||
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 4) /* If set, the system operates normally, but won't actuate any servos */
|
||||
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 5) /* If set, the system will always output the failsafe values */
|
||||
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 6) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
|
||||
|
||||
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
|
||||
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
|
||||
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
|
||||
#define PX4IO_P_SETUP_RELAYS_PAD 5
|
||||
|
||||
#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */
|
||||
#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */
|
||||
#define PX4IO_P_SETUP_VSERVO_SCALE 5 /* hardware rev [2] servo voltage correction factor (float) */
|
||||
#define PX4IO_P_SETUP_DSM 6 /* DSM bind state */
|
||||
enum { /* DSM bind states */
|
||||
dsm_bind_power_down = 0,
|
||||
dsm_bind_power_up,
|
||||
|
|
@ -218,113 +187,33 @@ enum { /* DSM bind states */
|
|||
#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into
|
||||
'armed' (PWM enabled) state - this is a non-data write and
|
||||
hence index 12 can safely be used. */
|
||||
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
|
||||
|
||||
#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */
|
||||
#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
|
||||
|
||||
#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */
|
||||
#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */
|
||||
#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */
|
||||
#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */
|
||||
#define PX4IO_P_SETUP_SCALE_ROLL 19 /**< Roll scale, in actuator units */
|
||||
#define PX4IO_P_SETUP_SCALE_PITCH 20 /**< Pitch scale, in actuator units */
|
||||
#define PX4IO_P_SETUP_SCALE_YAW 21 /**< Yaw scale, in actuator units */
|
||||
|
||||
#define PX4IO_P_SETUP_SBUS_RATE 22 /**< frame rate of SBUS1 output in Hz */
|
||||
|
||||
#define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /**< max motor slew rate */
|
||||
|
||||
#define PX4IO_P_SETUP_THR_MDL_FAC 25 /**< factor for modelling motor control signal output to static thrust relationship */
|
||||
|
||||
#define PX4IO_P_SETUP_THERMAL 26 /**< thermal management */
|
||||
|
||||
#define PX4IO_P_SETUP_AIRMODE 27 /**< air-mode */
|
||||
|
||||
#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 28 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
|
||||
#define PX4IO_P_SETUP_SBUS_RATE 16 /**< frame rate of SBUS1 output in Hz */
|
||||
#define PX4IO_P_SETUP_THERMAL 17 /**< thermal management */
|
||||
#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 18 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
|
||||
|
||||
#define PX4IO_THERMAL_IGNORE UINT16_MAX
|
||||
#define PX4IO_THERMAL_OFF 0
|
||||
#define PX4IO_THERMAL_FULL 10000
|
||||
|
||||
/* autopilot control values, -10000..10000 */
|
||||
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
|
||||
#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
|
||||
#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
|
||||
#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
|
||||
#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
|
||||
|
||||
#define PX4IO_P_CONTROLS_GROUP_VALID 64
|
||||
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */
|
||||
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */
|
||||
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */
|
||||
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */
|
||||
|
||||
/* raw text load to the mixer parser - ignores offset */
|
||||
#define PX4IO_PAGE_MIXERLOAD 52
|
||||
|
||||
/* R/C channel config */
|
||||
#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */
|
||||
#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */
|
||||
#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */
|
||||
#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */
|
||||
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */
|
||||
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */
|
||||
#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
|
||||
#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */
|
||||
|
||||
/* PWM output - overrides mixer */
|
||||
/* PWM output */
|
||||
#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM failsafe values - zero disables the output */
|
||||
#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM failsafe values - zero disables the output */
|
||||
#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */
|
||||
#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */
|
||||
|
||||
/* Debug and test page - not used in normal operation */
|
||||
#define PX4IO_PAGE_TEST 127
|
||||
#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */
|
||||
|
||||
/* PWM minimum values for certain ESCs */
|
||||
#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM maximum values for certain ESCs */
|
||||
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM mtrim values for central position */
|
||||
#define PX4IO_PAGE_CONTROL_TRIM_PWM 108 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM disarmed values that are active, even when SAFETY_SAFE */
|
||||
#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/**
|
||||
* As-needed mixer data upload.
|
||||
*
|
||||
* This message adds text to the mixer text buffer; the text
|
||||
* buffer is drained as the definitions are consumed.
|
||||
*/
|
||||
#pragma pack(push, 1)
|
||||
struct px4io_mixdata {
|
||||
uint16_t f2i_mixer_magic;
|
||||
#define F2I_MIXER_MAGIC 0x6d74
|
||||
|
||||
uint8_t action;
|
||||
#define F2I_MIXER_ACTION_RESET 0
|
||||
#define F2I_MIXER_ACTION_APPEND 1
|
||||
|
||||
char text[0]; /* actual text size may vary */
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
/**
|
||||
* Serial protocol encapsulation.
|
||||
*/
|
||||
|
||||
#define PKT_MAX_REGS 32 // by agreement w/FMU
|
||||
|
||||
#pragma pack(push, 1)
|
||||
|
|
|
|||
|
|
@ -59,8 +59,6 @@
|
|||
# include <lib/perf/perf_counter.h>
|
||||
#endif
|
||||
|
||||
#include <output_limit/output_limit.h>
|
||||
|
||||
#include <stm32_uart.h>
|
||||
|
||||
#define DEBUG
|
||||
|
|
@ -72,10 +70,6 @@ struct sys_state_s system_state;
|
|||
|
||||
static struct hrt_call serial_dma_call;
|
||||
|
||||
output_limit_t pwm_limit;
|
||||
|
||||
float dt;
|
||||
|
||||
/*
|
||||
* a set of debug buffers to allow us to send debug information from ISRs
|
||||
*/
|
||||
|
|
@ -313,11 +307,6 @@ user_start(int argc, char *argv[])
|
|||
LED_RING(false);
|
||||
#endif
|
||||
|
||||
/* turn on servo power (if supported) */
|
||||
#ifdef POWER_SERVO
|
||||
POWER_SERVO(true);
|
||||
#endif
|
||||
|
||||
/* turn off S.Bus out (if supported) */
|
||||
#ifdef ENABLE_SBUS_OUT
|
||||
ENABLE_SBUS_OUT(false);
|
||||
|
|
@ -350,9 +339,6 @@ user_start(int argc, char *argv[])
|
|||
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk;
|
||||
syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
|
||||
|
||||
/* initialize PWM limit lib */
|
||||
output_limit_init(&pwm_limit);
|
||||
|
||||
/* Start the failsafe led init */
|
||||
failsafe_led_init();
|
||||
|
||||
|
|
@ -362,23 +348,12 @@ user_start(int argc, char *argv[])
|
|||
|
||||
uint64_t last_debug_time = 0;
|
||||
uint64_t last_heartbeat_time = 0;
|
||||
uint64_t last_loop_time = 0;
|
||||
|
||||
watchdog_init();
|
||||
|
||||
for (;;) {
|
||||
watchdog_pet();
|
||||
|
||||
dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f;
|
||||
last_loop_time = hrt_absolute_time();
|
||||
|
||||
if (dt < 0.0001f) {
|
||||
dt = 0.0001f;
|
||||
|
||||
} else if (dt > 0.02f) {
|
||||
dt = 0.02f;
|
||||
}
|
||||
|
||||
#if defined(PX4IO_PERF)
|
||||
/* track the rate at which the loop is running */
|
||||
perf_count(loop_perf);
|
||||
|
|
@ -416,10 +391,6 @@ user_start(int argc, char *argv[])
|
|||
*/
|
||||
uint32_t heartbeat_period_us = 250 * 1000UL;
|
||||
|
||||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
|
||||
heartbeat_period_us /= 4;
|
||||
}
|
||||
|
||||
if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
|
||||
last_heartbeat_time = hrt_absolute_time();
|
||||
heartbeat_blink();
|
||||
|
|
|
|||
|
|
@ -50,15 +50,13 @@
|
|||
|
||||
#include "protocol.h"
|
||||
|
||||
#include <output_limit/output_limit.h>
|
||||
|
||||
/*
|
||||
* Constants and limits.
|
||||
*/
|
||||
#define PX4IO_BL_VERSION 3
|
||||
#define PX4IO_SERVO_COUNT 8
|
||||
#define PX4IO_CONTROL_CHANNELS 8
|
||||
#define PX4IO_CONTROL_GROUPS 4
|
||||
|
||||
#define PX4IO_RC_INPUT_CHANNELS 18
|
||||
#define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */
|
||||
|
||||
|
|
@ -77,20 +75,12 @@
|
|||
* Registers.
|
||||
*/
|
||||
extern volatile uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */
|
||||
extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */
|
||||
extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */
|
||||
extern uint16_t r_page_direct_pwm[]; /* PX4IO_PAGE_DIRECT_PWM */
|
||||
extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
|
||||
extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */
|
||||
extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */
|
||||
|
||||
extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
|
||||
extern uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
|
||||
extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
|
||||
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
|
||||
extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
|
||||
extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
|
||||
extern int16_t r_page_servo_control_trim[]; /* PX4IO_PAGE_CONTROL_TRIM_PWM */
|
||||
extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
|
||||
|
||||
/*
|
||||
|
|
@ -104,32 +94,16 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
|
|||
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
|
||||
#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
|
||||
#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
|
||||
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
|
||||
#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE])
|
||||
#define r_mixer_limits r_page_status[PX4IO_P_STATUS_MIXER]
|
||||
|
||||
#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
|
||||
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]
|
||||
#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES]
|
||||
#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
|
||||
#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
|
||||
#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
|
||||
|
||||
#define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE]
|
||||
|
||||
#define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL]
|
||||
#define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH]
|
||||
#define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW]
|
||||
#define r_setup_scale_roll r_page_setup[PX4IO_P_SETUP_SCALE_ROLL]
|
||||
#define r_setup_scale_pitch r_page_setup[PX4IO_P_SETUP_SCALE_PITCH]
|
||||
#define r_setup_scale_yaw r_page_setup[PX4IO_P_SETUP_SCALE_YAW]
|
||||
#define r_setup_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE]
|
||||
#define r_setup_thr_fac r_page_setup[PX4IO_P_SETUP_THR_MDL_FAC]
|
||||
#define r_setup_slew_max r_page_setup[PX4IO_P_SETUP_MOTOR_SLEW_MAX]
|
||||
#define r_setup_airmode r_page_setup[PX4IO_P_SETUP_AIRMODE]
|
||||
#define r_setup_flighttermination r_page_setup[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION]
|
||||
|
||||
#define r_control_values (&r_page_controls[0])
|
||||
#define r_setup_flighttermination r_page_setup[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION]
|
||||
|
||||
/*
|
||||
* System state structure.
|
||||
|
|
@ -146,15 +120,9 @@ struct sys_state_s {
|
|||
};
|
||||
|
||||
extern struct sys_state_s system_state;
|
||||
extern float dt;
|
||||
extern bool update_mc_thrust_param;
|
||||
extern bool update_trims;
|
||||
|
||||
/*
|
||||
* PWM limit structure
|
||||
*/
|
||||
extern output_limit_t pwm_limit;
|
||||
|
||||
/*
|
||||
* GPIO handling.
|
||||
*/
|
||||
|
|
@ -169,7 +137,6 @@ extern output_limit_t pwm_limit;
|
|||
#define LED_RING(_s) px4_arch_gpiowrite(GPIO_LED4, (_s))
|
||||
|
||||
|
||||
# define PX4IO_RELAY_CHANNELS 0
|
||||
# define ENABLE_SBUS_OUT(_s) px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
|
||||
|
||||
# define VDD_SERVO_FAULT (!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT))
|
||||
|
|
@ -180,8 +147,6 @@ extern output_limit_t pwm_limit;
|
|||
|
||||
#define BUTTON_SAFETY px4_arch_gpioread(GPIO_BTN_SAFETY)
|
||||
|
||||
#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel)
|
||||
|
||||
#define PX4_CRITICAL_SECTION(cmd) { irqstate_t flags = px4_enter_critical_section(); cmd; px4_leave_critical_section(flags); }
|
||||
|
||||
void atomic_modify_or(volatile uint16_t *target, uint16_t modification);
|
||||
|
|
@ -192,9 +157,6 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification);
|
|||
* Mixer
|
||||
*/
|
||||
extern void mixer_tick(void);
|
||||
extern int interrupt_mixer_handle_text(const void *buffer, size_t length);
|
||||
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
|
||||
extern void interrupt_mixer_set_failsafe(void);
|
||||
|
||||
/**
|
||||
* Safety switch/LED.
|
||||
|
|
|
|||
|
|
@ -74,7 +74,6 @@ static const uint16_t r_page_config[] = {
|
|||
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
|
||||
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS,
|
||||
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
|
||||
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -89,17 +88,8 @@ volatile uint16_t r_page_status[] = {
|
|||
[PX4IO_P_STATUS_ALARMS] = 0,
|
||||
[PX4IO_P_STATUS_VSERVO] = 0,
|
||||
[PX4IO_P_STATUS_VRSSI] = 0,
|
||||
[PX4IO_P_STATUS_PRSSI] = 0,
|
||||
[PX4IO_P_STATUS_MIXER] = 0,
|
||||
};
|
||||
|
||||
/**
|
||||
* PAGE 2
|
||||
*
|
||||
* Post-mixed actuator values.
|
||||
*/
|
||||
uint16_t r_page_actuators[PX4IO_SERVO_COUNT];
|
||||
|
||||
/**
|
||||
* PAGE 3
|
||||
*
|
||||
|
|
@ -122,16 +112,6 @@ uint16_t r_page_raw_rc_input[] = {
|
|||
[PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
* PAGE 5
|
||||
*
|
||||
* Scaled/routed RC input
|
||||
*/
|
||||
uint16_t r_page_rc_input[] = {
|
||||
[PX4IO_P_RC_VALID] = 0,
|
||||
[PX4IO_P_RC_BASE ...(PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
* Scratch page; used for registers that are constructed as-read.
|
||||
*
|
||||
|
|
@ -155,76 +135,29 @@ uint16_t r_page_direct_pwm[PX4IO_SERVO_COUNT];
|
|||
volatile uint16_t r_page_setup[] = {
|
||||
/* default to RSSI ADC functionality */
|
||||
[PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI,
|
||||
[PX4IO_P_SETUP_ARMING] = (PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE),
|
||||
[PX4IO_P_SETUP_ARMING] = 0,
|
||||
[PX4IO_P_SETUP_PWM_RATES] = 0,
|
||||
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
|
||||
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
|
||||
[PX4IO_P_SETUP_SBUS_RATE] = 72,
|
||||
/* this is unused, but we will pad it for readability (the compiler pads it automatically) */
|
||||
[PX4IO_P_SETUP_RELAYS_PAD] = 0,
|
||||
#ifdef ADC_VSERVO
|
||||
[PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
|
||||
#else
|
||||
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
|
||||
#endif
|
||||
[PX4IO_P_SETUP_SET_DEBUG] = 0,
|
||||
[PX4IO_P_SETUP_REBOOT_BL] = 0,
|
||||
[PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0,
|
||||
[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
|
||||
[PX4IO_P_SETUP_PWM_REVERSE] = 0,
|
||||
[PX4IO_P_SETUP_TRIM_ROLL] = 0,
|
||||
[PX4IO_P_SETUP_TRIM_PITCH] = 0,
|
||||
[PX4IO_P_SETUP_TRIM_YAW] = 0,
|
||||
[PX4IO_P_SETUP_SCALE_ROLL] = 10000,
|
||||
[PX4IO_P_SETUP_SCALE_PITCH] = 10000,
|
||||
[PX4IO_P_SETUP_SCALE_YAW] = 10000,
|
||||
[PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0,
|
||||
[PX4IO_P_SETUP_AIRMODE] = 0,
|
||||
[PX4IO_P_SETUP_THR_MDL_FAC] = 0,
|
||||
[PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE,
|
||||
[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0
|
||||
};
|
||||
|
||||
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
|
||||
PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
|
||||
PX4IO_P_SETUP_FEATURES_PWM_RSSI)
|
||||
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT | PX4IO_P_SETUP_FEATURES_ADC_RSSI)
|
||||
|
||||
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
|
||||
PX4IO_P_SETUP_ARMING_FMU_PREARMED | \
|
||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
|
||||
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
|
||||
PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
|
||||
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
|
||||
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
|
||||
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
|
||||
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
|
||||
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
|
||||
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
|
||||
PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
|
||||
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
|
||||
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
|
||||
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
|
||||
|
||||
/**
|
||||
* PAGE 101
|
||||
*
|
||||
* Control values from the FMU.
|
||||
*/
|
||||
uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
|
||||
|
||||
/*
|
||||
* PAGE 102 does not have a buffer.
|
||||
*/
|
||||
|
||||
/**
|
||||
* PAGE 103
|
||||
*
|
||||
* R/C channel input configuration.
|
||||
*/
|
||||
uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
/* valid options */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
|
||||
|
||||
/*
|
||||
* PAGE 104 uses r_page_servos.
|
||||
|
|
@ -239,30 +172,6 @@ uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STR
|
|||
*/
|
||||
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
|
||||
|
||||
/**
|
||||
* PAGE 106
|
||||
*
|
||||
* minimum PWM values when armed
|
||||
*
|
||||
*/
|
||||
uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN };
|
||||
|
||||
/**
|
||||
* PAGE 107
|
||||
*
|
||||
* maximum PWM values when armed
|
||||
*
|
||||
*/
|
||||
uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX };
|
||||
|
||||
/**
|
||||
* PAGE 108
|
||||
*
|
||||
* trim values for center position
|
||||
*
|
||||
*/
|
||||
int16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM };
|
||||
|
||||
/**
|
||||
* PAGE 109
|
||||
*
|
||||
|
|
@ -274,29 +183,7 @@ uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
|
|||
int
|
||||
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
|
||||
{
|
||||
|
||||
switch (page) {
|
||||
|
||||
/* handle bulk controls input */
|
||||
case PX4IO_PAGE_CONTROLS:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
|
||||
|
||||
/* XXX range-check value? */
|
||||
r_page_controls[offset] = *values;
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
}
|
||||
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
|
||||
|
||||
system_state.fmu_data_received_time = hrt_absolute_time();
|
||||
|
||||
break;
|
||||
|
||||
/* handle raw PWM input */
|
||||
case PX4IO_PAGE_DIRECT_PWM:
|
||||
|
||||
|
|
@ -351,92 +238,21 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
|||
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_MIN_PWM:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
if (*values == 0) {
|
||||
/* ignore 0 */
|
||||
} else if (*values > PWM_HIGHEST_MIN) {
|
||||
r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
|
||||
|
||||
} else if (*values < PWM_LOWEST_MIN) {
|
||||
r_page_servo_control_min[offset] = PWM_LOWEST_MIN;
|
||||
|
||||
} else {
|
||||
r_page_servo_control_min[offset] = *values;
|
||||
}
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_MAX_PWM:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
if (*values == 0) {
|
||||
/* ignore 0 */
|
||||
} else if (*values > PWM_HIGHEST_MAX) {
|
||||
r_page_servo_control_max[offset] = PWM_HIGHEST_MAX;
|
||||
|
||||
} else if (*values < PWM_LOWEST_MAX) {
|
||||
r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
|
||||
|
||||
} else {
|
||||
r_page_servo_control_max[offset] = *values;
|
||||
}
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_TRIM_PWM:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
r_page_servo_control_trim[offset] = *values;
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
}
|
||||
|
||||
update_trims = true;
|
||||
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_DISARMED_PWM: {
|
||||
/* flag for all outputs */
|
||||
bool all_disarmed_off = true;
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
if (*values == 0) {
|
||||
/* 0 means disabling always PWM */
|
||||
r_page_servo_disarmed[offset] = 0;
|
||||
|
||||
} else if (*values < PWM_LOWEST_MIN) {
|
||||
r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
|
||||
all_disarmed_off = false;
|
||||
|
||||
} else if (*values > PWM_HIGHEST_MAX) {
|
||||
r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
|
||||
all_disarmed_off = false;
|
||||
|
||||
} else {
|
||||
r_page_servo_disarmed[offset] = *values;
|
||||
all_disarmed_off = false;
|
||||
}
|
||||
|
||||
offset++;
|
||||
|
|
@ -444,25 +260,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
|||
values++;
|
||||
}
|
||||
|
||||
if (all_disarmed_off) {
|
||||
/* disable PWM output if disarmed */
|
||||
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
|
||||
|
||||
} else {
|
||||
/* enable PWM output always */
|
||||
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
|
||||
}
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
|
||||
}
|
||||
break;
|
||||
|
||||
/* handle text going to the mixer parser */
|
||||
case PX4IO_PAGE_MIXERLOAD:
|
||||
/* do not change the mixer if FMU is armed and IO's safety is off
|
||||
* this state defines an active system. This check is done in the
|
||||
* text handling function.
|
||||
*/
|
||||
return interrupt_mixer_handle_text(values, num_values * sizeof(*values));
|
||||
|
||||
default:
|
||||
|
||||
/* avoid offset wrap */
|
||||
|
|
@ -504,20 +305,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
* Allow FMU override of arming state (to allow in-air restores),
|
||||
* but only if the arming state is not in sync on the IO side.
|
||||
*/
|
||||
|
||||
if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
||||
|
||||
} else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
|
||||
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
|
||||
r_status_flags = value;
|
||||
|
||||
}
|
||||
|
||||
if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) {
|
||||
/* update failsafe values, now that the mixer is set to ok */
|
||||
interrupt_mixer_set_failsafe();
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -541,32 +333,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
|
||||
/* disable the conflicting options with SBUS 1 */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_ADC_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
}
|
||||
|
||||
/* disable the conflicting options with SBUS 2 */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_ADC_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* disable the conflicting options with ADC RSSI */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
}
|
||||
|
||||
/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
}
|
||||
|
||||
/* apply changes */
|
||||
|
|
@ -578,18 +357,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
|
||||
value &= PX4IO_P_SETUP_ARMING_VALID;
|
||||
|
||||
/*
|
||||
* Update arming state - disarm if no longer OK.
|
||||
* This builds on the requirement that the FMU driver
|
||||
* asks about the FMU arming state on initialization,
|
||||
* so that an in-air reset of FMU can not lead to a
|
||||
* lockup of the IO arming state.
|
||||
*/
|
||||
|
||||
if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
|
||||
}
|
||||
|
||||
/*
|
||||
* If the failsafe termination flag is set, do not allow the autopilot to unset it
|
||||
*/
|
||||
|
|
@ -688,32 +455,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_RC_THR_FAILSAFE_US:
|
||||
if (value > 650 && value < 2350) {
|
||||
r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_SBUS_RATE:
|
||||
r_page_setup[offset] = value;
|
||||
sbus1_set_output_rate_hz(value);
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_THR_MDL_FAC:
|
||||
update_mc_thrust_param = true;
|
||||
r_page_setup[offset] = value;
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_PWM_REVERSE:
|
||||
case PX4IO_P_SETUP_TRIM_ROLL:
|
||||
case PX4IO_P_SETUP_TRIM_PITCH:
|
||||
case PX4IO_P_SETUP_TRIM_YAW:
|
||||
case PX4IO_P_SETUP_SCALE_ROLL:
|
||||
case PX4IO_P_SETUP_SCALE_PITCH:
|
||||
case PX4IO_P_SETUP_SCALE_YAW:
|
||||
case PX4IO_P_SETUP_MOTOR_SLEW_MAX:
|
||||
case PX4IO_P_SETUP_AIRMODE:
|
||||
case PX4IO_P_SETUP_THERMAL:
|
||||
case PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION:
|
||||
r_page_setup[offset] = value;
|
||||
|
|
@ -725,101 +471,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_RC_CONFIG: {
|
||||
|
||||
/**
|
||||
* do not allow a RC config change while safety is off
|
||||
*/
|
||||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
|
||||
break;
|
||||
}
|
||||
|
||||
unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
|
||||
unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
|
||||
uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
if (channel >= PX4IO_RC_INPUT_CHANNELS) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* disable the channel until we have a chance to sanity-check it */
|
||||
conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
|
||||
switch (index) {
|
||||
|
||||
case PX4IO_P_RC_CONFIG_MIN:
|
||||
case PX4IO_P_RC_CONFIG_CENTER:
|
||||
case PX4IO_P_RC_CONFIG_MAX:
|
||||
case PX4IO_P_RC_CONFIG_DEADZONE:
|
||||
case PX4IO_P_RC_CONFIG_ASSIGNMENT:
|
||||
conf[index] = value;
|
||||
break;
|
||||
|
||||
case PX4IO_P_RC_CONFIG_OPTIONS:
|
||||
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
|
||||
|
||||
/* clear any existing RC disabled flag */
|
||||
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
|
||||
|
||||
/* set all options except the enabled option */
|
||||
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
|
||||
/* should the channel be enabled? */
|
||||
/* this option is normally set last */
|
||||
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
||||
uint8_t count = 0;
|
||||
bool disabled = false;
|
||||
|
||||
/* assert min..center..max ordering */
|
||||
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
|
||||
count++;
|
||||
}
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
|
||||
count++;
|
||||
}
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
|
||||
count++;
|
||||
}
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
|
||||
count++;
|
||||
}
|
||||
|
||||
/* assert deadzone is sane */
|
||||
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
|
||||
count++;
|
||||
}
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
|
||||
disabled = true;
|
||||
|
||||
} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
|
||||
(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
|
||||
count++;
|
||||
}
|
||||
|
||||
/* sanity checks pass, enable channel */
|
||||
if (count) {
|
||||
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
|
||||
|
||||
} else if (!disabled) {
|
||||
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
/* case PX4IO_RC_PAGE_CONFIG */
|
||||
}
|
||||
|
||||
case PX4IO_PAGE_TEST:
|
||||
switch (offset) {
|
||||
case PX4IO_P_TEST_LED:
|
||||
|
|
@ -862,49 +513,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
|
||||
/* PX4IO_P_STATUS_ALARMS maintained externally */
|
||||
|
||||
#ifdef ADC_VBATT
|
||||
/* PX4IO_P_STATUS_VBATT */
|
||||
{
|
||||
/*
|
||||
* Coefficients here derived by measurement of the 5-16V
|
||||
* range on one unit, validated on sample points of another unit
|
||||
*
|
||||
* Data in Tools/tests-host/data folder.
|
||||
*
|
||||
* measured slope = 0.004585267878277 (int: 4585)
|
||||
* nominal theoretic slope: 0.00459340659 (int: 4593)
|
||||
* intercept = 0.016646394188076 (int: 16646)
|
||||
* nominal theoretic intercept: 0.00 (int: 0)
|
||||
*
|
||||
*/
|
||||
unsigned counts = adc_measure(ADC_VBATT);
|
||||
|
||||
if (counts != 0xffff) {
|
||||
unsigned mV = (166460 + (counts * 45934)) / 10000;
|
||||
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
|
||||
|
||||
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#ifdef ADC_IBATT
|
||||
/* PX4IO_P_STATUS_IBATT */
|
||||
{
|
||||
/*
|
||||
note that we have no idea what sort of
|
||||
current sensor is attached, so we just
|
||||
return the raw 12 bit ADC value and let the
|
||||
FMU sort it out, with user selectable
|
||||
configuration for their sensor
|
||||
*/
|
||||
unsigned counts = adc_measure(ADC_IBATT);
|
||||
|
||||
if (counts != 0xffff) {
|
||||
r_page_status[PX4IO_P_STATUS_IBATT] = counts;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef ADC_VSERVO
|
||||
/* PX4IO_P_STATUS_VSERVO */
|
||||
{
|
||||
|
|
@ -916,6 +524,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#ifdef ADC_RSSI
|
||||
/* PX4IO_P_STATUS_VRSSI */
|
||||
|
|
@ -936,12 +545,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
|
||||
case PX4IO_PAGE_RAW_ADC_INPUT:
|
||||
memset(r_page_scratch, 0, sizeof(r_page_scratch));
|
||||
#ifdef ADC_VBATT
|
||||
r_page_scratch[0] = adc_measure(ADC_VBATT);
|
||||
#endif
|
||||
#ifdef ADC_IBATT
|
||||
r_page_scratch[1] = adc_measure(ADC_IBATT);
|
||||
#endif
|
||||
|
||||
#ifdef ADC_VSERVO
|
||||
r_page_scratch[0] = adc_measure(ADC_VSERVO);
|
||||
|
|
@ -971,10 +574,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
SELECT_PAGE(r_page_config);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_ACTUATORS:
|
||||
SELECT_PAGE(r_page_actuators);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_SERVOS:
|
||||
SELECT_PAGE(r_page_servos);
|
||||
break;
|
||||
|
|
@ -983,23 +582,11 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
SELECT_PAGE(r_page_raw_rc_input);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_RC_INPUT:
|
||||
SELECT_PAGE(r_page_rc_input);
|
||||
break;
|
||||
|
||||
/* readback of input pages */
|
||||
case PX4IO_PAGE_SETUP:
|
||||
SELECT_PAGE(r_page_setup);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROLS:
|
||||
SELECT_PAGE(r_page_controls);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_RC_CONFIG:
|
||||
SELECT_PAGE(r_page_rc_input_config);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_DIRECT_PWM:
|
||||
SELECT_PAGE(r_page_direct_pwm);
|
||||
break;
|
||||
|
|
@ -1008,18 +595,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
SELECT_PAGE(r_page_servo_failsafe);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_MIN_PWM:
|
||||
SELECT_PAGE(r_page_servo_control_min);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_MAX_PWM:
|
||||
SELECT_PAGE(r_page_servo_control_max);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_TRIM_PWM:
|
||||
SELECT_PAGE(r_page_servo_control_trim);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_DISARMED_PWM:
|
||||
SELECT_PAGE(r_page_servo_disarmed);
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -1329,39 +1329,6 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 0);
|
|||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_FLTMODE, 0);
|
||||
|
||||
/**
|
||||
* Mode switch channel mapping.
|
||||
*
|
||||
* This is the main flight mode selector.
|
||||
* The channel index (starting from 1 for channel 1) indicates
|
||||
* which channel should be used for deciding about the main mode.
|
||||
* A value of zero indicates the switch is not assigned.
|
||||
*
|
||||
* @min 0
|
||||
* @max 18
|
||||
* @group Radio Switches
|
||||
* @value 0 Unassigned
|
||||
* @value 1 Channel 1
|
||||
* @value 2 Channel 2
|
||||
* @value 3 Channel 3
|
||||
* @value 4 Channel 4
|
||||
* @value 5 Channel 5
|
||||
* @value 6 Channel 6
|
||||
* @value 7 Channel 7
|
||||
* @value 8 Channel 8
|
||||
* @value 9 Channel 9
|
||||
* @value 10 Channel 10
|
||||
* @value 11 Channel 11
|
||||
* @value 12 Channel 12
|
||||
* @value 13 Channel 13
|
||||
* @value 14 Channel 14
|
||||
* @value 15 Channel 15
|
||||
* @value 16 Channel 16
|
||||
* @value 17 Channel 17
|
||||
* @value 18 Channel 18
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
|
||||
|
||||
/**
|
||||
* Return switch channel
|
||||
*
|
||||
|
|
@ -1390,34 +1357,6 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
|
|||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
|
||||
|
||||
/**
|
||||
* Position Control switch channel
|
||||
*
|
||||
* @min 0
|
||||
* @max 18
|
||||
* @group Radio Switches
|
||||
* @value 0 Unassigned
|
||||
* @value 1 Channel 1
|
||||
* @value 2 Channel 2
|
||||
* @value 3 Channel 3
|
||||
* @value 4 Channel 4
|
||||
* @value 5 Channel 5
|
||||
* @value 6 Channel 6
|
||||
* @value 7 Channel 7
|
||||
* @value 8 Channel 8
|
||||
* @value 9 Channel 9
|
||||
* @value 10 Channel 10
|
||||
* @value 11 Channel 11
|
||||
* @value 12 Channel 12
|
||||
* @value 13 Channel 13
|
||||
* @value 14 Channel 14
|
||||
* @value 15 Channel 15
|
||||
* @value 16 Channel 16
|
||||
* @value 17 Channel 17
|
||||
* @value 18 Channel 18
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
|
||||
|
||||
/**
|
||||
* Loiter switch channel
|
||||
*
|
||||
|
|
@ -1446,34 +1385,6 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
|
|||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
|
||||
|
||||
/**
|
||||
* Acro switch channel
|
||||
*
|
||||
* @min 0
|
||||
* @max 18
|
||||
* @group Radio Switches
|
||||
* @value 0 Unassigned
|
||||
* @value 1 Channel 1
|
||||
* @value 2 Channel 2
|
||||
* @value 3 Channel 3
|
||||
* @value 4 Channel 4
|
||||
* @value 5 Channel 5
|
||||
* @value 6 Channel 6
|
||||
* @value 7 Channel 7
|
||||
* @value 8 Channel 8
|
||||
* @value 9 Channel 9
|
||||
* @value 10 Channel 10
|
||||
* @value 11 Channel 11
|
||||
* @value 12 Channel 12
|
||||
* @value 13 Channel 13
|
||||
* @value 14 Channel 14
|
||||
* @value 15 Channel 15
|
||||
* @value 16 Channel 16
|
||||
* @value 17 Channel 17
|
||||
* @value 18 Channel 18
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
|
||||
|
||||
/**
|
||||
* Offboard switch channel
|
||||
*
|
||||
|
|
@ -1645,62 +1556,6 @@ PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0);
|
|||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0);
|
||||
|
||||
/**
|
||||
* Stabilize switch channel mapping.
|
||||
*
|
||||
* @min 0
|
||||
* @max 18
|
||||
* @group Radio Switches
|
||||
* @value 0 Unassigned
|
||||
* @value 1 Channel 1
|
||||
* @value 2 Channel 2
|
||||
* @value 3 Channel 3
|
||||
* @value 4 Channel 4
|
||||
* @value 5 Channel 5
|
||||
* @value 6 Channel 6
|
||||
* @value 7 Channel 7
|
||||
* @value 8 Channel 8
|
||||
* @value 9 Channel 9
|
||||
* @value 10 Channel 10
|
||||
* @value 11 Channel 11
|
||||
* @value 12 Channel 12
|
||||
* @value 13 Channel 13
|
||||
* @value 14 Channel 14
|
||||
* @value 15 Channel 15
|
||||
* @value 16 Channel 16
|
||||
* @value 17 Channel 17
|
||||
* @value 18 Channel 18
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0);
|
||||
|
||||
/**
|
||||
* Manual switch channel mapping.
|
||||
*
|
||||
* @min 0
|
||||
* @max 18
|
||||
* @group Radio Switches
|
||||
* @value 0 Unassigned
|
||||
* @value 1 Channel 1
|
||||
* @value 2 Channel 2
|
||||
* @value 3 Channel 3
|
||||
* @value 4 Channel 4
|
||||
* @value 5 Channel 5
|
||||
* @value 6 Channel 6
|
||||
* @value 7 Channel 7
|
||||
* @value 8 Channel 8
|
||||
* @value 9 Channel 9
|
||||
* @value 10 Channel 10
|
||||
* @value 11 Channel 11
|
||||
* @value 12 Channel 12
|
||||
* @value 13 Channel 13
|
||||
* @value 14 Channel 14
|
||||
* @value 15 Channel 15
|
||||
* @value 16 Channel 16
|
||||
* @value 17 Channel 17
|
||||
* @value 18 Channel 18
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
|
||||
|
||||
/**
|
||||
* Button flight mode selection
|
||||
*
|
||||
|
|
@ -1732,7 +1587,6 @@ PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
|
|||
* @bit 16 Mask Channel 17 as a mode button
|
||||
* @bit 17 Mask Channel 18 as a mode button
|
||||
*/
|
||||
|
||||
PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0);
|
||||
|
||||
/**
|
||||
|
|
@ -2019,54 +1873,6 @@ PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0);
|
|||
*/
|
||||
PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
|
||||
|
||||
/**
|
||||
* Threshold for selecting assist mode
|
||||
*
|
||||
* 0-1 indicate where in the full channel range the threshold sits
|
||||
* 0 : min
|
||||
* 1 : max
|
||||
* sign indicates polarity of comparison
|
||||
* positive : true when channel>th
|
||||
* negative : true when 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
|
||||
*
|
||||
|
|
@ -2099,22 +1905,6 @@ PARAM_DEFINE_FLOAT(RC_RETURN_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
|
||||
*
|
||||
|
|
@ -2195,38 +1985,6 @@ PARAM_DEFINE_FLOAT(RC_TRANS_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.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
*
|
||||
|
|
@ -58,3 +91,116 @@
|
|||
* @value 18 Channel 18
|
||||
*/
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -48,18 +48,13 @@ namespace RCUpdate
|
|||
static bool operator ==(const manual_control_switches_s &a, const manual_control_switches_s &b)
|
||||
{
|
||||
return (a.mode_slot == b.mode_slot &&
|
||||
a.mode_switch == b.mode_switch &&
|
||||
a.return_switch == b.return_switch &&
|
||||
a.posctl_switch == b.posctl_switch &&
|
||||
a.loiter_switch == b.loiter_switch &&
|
||||
a.acro_switch == b.acro_switch &&
|
||||
a.offboard_switch == b.offboard_switch &&
|
||||
a.kill_switch == b.kill_switch &&
|
||||
a.arm_switch == b.arm_switch &&
|
||||
a.transition_switch == b.transition_switch &&
|
||||
a.gear_switch == b.gear_switch &&
|
||||
a.stab_switch == b.stab_switch &&
|
||||
a.man_switch == b.man_switch);
|
||||
a.gear_switch == b.gear_switch);
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
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 (rc_map_value != 0) {
|
||||
PX4_WARN("RC_MAP_RATT_SW deprecated");
|
||||
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_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_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_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_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_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_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;
|
||||
|
||||
|
|
@ -524,28 +549,6 @@ void RCUpdate::Run()
|
|||
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
|
||||
{
|
||||
if (_rc.function[func] >= 0) {
|
||||
|
|
@ -569,7 +572,7 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample)
|
|||
manual_control_switches_s switches{};
|
||||
switches.timestamp_sample = timestamp_sample;
|
||||
|
||||
// check mode slot (RC_MAP_FLTMODE) or legacy mode switch (RC_MAP_MODE_SW), but not both
|
||||
// check mode slot (RC_MAP_FLTMODE)
|
||||
if (_param_rc_map_fltmode.get() > 0) {
|
||||
// number of valid slots
|
||||
static constexpr int num_slots = manual_control_switches_s::MODE_SLOT_NUM;
|
||||
|
|
@ -594,16 +597,6 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample)
|
|||
switches.mode_slot = num_slots;
|
||||
}
|
||||
|
||||
} else if (_param_rc_map_mode_sw.get() > 0) {
|
||||
switches.mode_switch = get_rc_sw3pos_position(rc_channels_s::FUNCTION_MODE,
|
||||
_param_rc_auto_th.get(), _param_rc_assist_th.get());
|
||||
|
||||
// only used with legacy mode switch
|
||||
switches.man_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_MAN, _param_rc_man_th.get());
|
||||
switches.acro_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ACRO, _param_rc_acro_th.get());
|
||||
switches.stab_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_STAB, _param_rc_stab_th.get());
|
||||
switches.posctl_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_POSCTL, _param_rc_posctl_th.get());
|
||||
|
||||
} else if (_param_rc_map_flightmode_buttons.get() > 0) {
|
||||
switches.mode_slot = manual_control_switches_s::MODE_SLOT_NONE;
|
||||
bool is_consistent_button_press = false;
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2016-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -120,7 +120,6 @@ private:
|
|||
/**
|
||||
* Get switch position for specified function.
|
||||
*/
|
||||
switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, float mid_th) const;
|
||||
switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th) const;
|
||||
|
||||
/**
|
||||
|
|
@ -208,21 +207,16 @@ private:
|
|||
(ParamInt<px4::params::RC_MAP_FAILSAFE>) _param_rc_map_failsafe,
|
||||
|
||||
(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_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_ACRO_SW>) _param_rc_map_acro_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_ARM_SW>) _param_rc_map_arm_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_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_AUX1>) _param_rc_map_aux1,
|
||||
|
|
@ -234,18 +228,12 @@ private:
|
|||
|
||||
(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_ACRO_TH>) _param_rc_acro_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_ARMSWITCH_TH>) _param_rc_armswitch_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_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,
|
||||
|
||||
(ParamInt<px4::params::RC_CHAN_CNT>) _param_rc_chan_cnt
|
||||
|
|
|
|||
|
|
@ -43,7 +43,6 @@
|
|||
#pragma once
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/barometer/PX4Barometer.hpp>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
|
|
@ -60,6 +59,7 @@
|
|||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
#include <uORB/topics/irlock_report.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
|
|
|
|||
|
|
@ -36,7 +36,6 @@ set(srcs
|
|||
test_bezierQuad.cpp
|
||||
test_bitset.cpp
|
||||
test_bson.cpp
|
||||
test_conv.cpp
|
||||
test_dataman.c
|
||||
test_file.c
|
||||
test_file2.c
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -48,7 +48,7 @@
|
|||
#include <output_limit/output_limit.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <px4iofirmware/mixer.h>
|
||||
|
||||
#include <px4iofirmware/protocol.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 */
|
||||
mixer_group.reset();
|
||||
char mixer_text[PX4IO_MAX_MIXER_LENGTH]; /* large enough for one mixer */
|
||||
char mixer_text[330]; /* large enough for one mixer */
|
||||
|
||||
unsigned mixer_text_length = 0;
|
||||
unsigned transmitted = 0;
|
||||
|
|
@ -290,7 +290,7 @@ bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loade
|
|||
/* check for overflow - this would be really fatal */
|
||||
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
|
||||
PX4_ERR("Mixer text length overflow for file: %s. Is PX4IO_MAX_MIXER_LENGTH too small? (curr len: %d)", filename,
|
||||
PX4IO_MAX_MIXER_LENGTH);
|
||||
330);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@
|
|||
#include <errno.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 <systemlib/err.h>
|
||||
|
||||
|
|
|
|||
|
|
@ -48,9 +48,9 @@
|
|||
#include <errno.h>
|
||||
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <uORB/topics/input_rc.h>
|
||||
|
||||
#include "tests_main.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -81,7 +81,6 @@ const struct {
|
|||
{"bezier", test_bezierQuad, 0},
|
||||
{"bitset", test_bitset, 0},
|
||||
{"bson", test_bson, 0},
|
||||
{"conv", test_conv, 0},
|
||||
{"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"file2", test_file2, OPT_NOJIGTEST},
|
||||
{"float", test_float, 0},
|
||||
|
|
|
|||
|
|
@ -47,7 +47,6 @@ extern int test_atomic_bitset(int argc, char *argv[]);
|
|||
extern int test_bezierQuad(int argc, char *argv[]);
|
||||
extern int test_bitset(int argc, char *argv[]);
|
||||
extern int test_bson(int argc, char *argv[]);
|
||||
extern int test_conv(int argc, char *argv[]);
|
||||
extern int test_dataman(int argc, char *argv[]);
|
||||
extern int test_file(int argc, char *argv[]);
|
||||
extern int test_file2(int argc, char *argv[]);
|
||||
|
|
|
|||
Loading…
Reference in New Issue