Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

WIP: px4io lobotomy

Browse Source
This commit is contained in:
Daniel Agar
2021-02-05 12:47:31 -05:00
parent 410ca51a4a
commit d9e33b68cf
51 changed files with 892 additions and 3633 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/CMakeLists.txt
-1
View File
@@ -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
ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo
-1
View File
@@ -49,7 +49,6 @@ param set-default CBRK_IO_SAFETY 22027
param set-default CBRK_SUPPLY_CHK 894281
# RC configuration
param set-default RC_MAP_MODE_SW 5
param set-default RC_MAP_PITCH 2
param set-default RC_MAP_ROLL 1
param set-default RC_MAP_THROTTLE 3
ROMFS/px4fmu_common/init.d/rc.interface
+4 -8
View File
@@ -93,19 +93,15 @@ then
fi
fi
if [ $OUTPUT_MODE = io ]
then
. ${R}etc/init.d/rc.io
fi
#
# Start IO for RC input if needed.
#
if [ $IO_PRESENT = yes ]
if [ $IO_PRESENT = yes -a $OUTPUT_MODE = io ]
then
if [ $OUTPUT_MODE != io ]
if ! px4io start
then
. ${R}etc/init.d/rc.io
echo "PX4IO start failed"
tune_control play -t 18 # PROG_PX4IO_ERR
fi
fi
fi
ROMFS/px4fmu_common/init.d/rc.io
-23
View File
@@ -1,23 +0,0 @@
#!/bin/sh
#
# PX4IO interface init script.
#
# If $OUTPUT_MODE indicated Hardware-int-the-loop simulation, px4io should not publish actuator_outputs,
# instead, pwm_out_sim will publish that uORB
if [ $OUTPUT_MODE = hil ]
then
set HIL_ARG $OUTPUT_MODE
fi
if [ $IO_PRESENT = yes ]
then
if px4io start $HIL_ARG
then
# Allow PX4IO to recover from midair restarts.
px4io recovery
else
echo "PX4IO start failed"
tune_control play -t 18 # PROG_PX4IO_ERR
fi
fi
boards/bitcraze/crazyflie/syslink/syslink_main.cpp
+1 -2
View File
@@ -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);
msg/input_rc.msg
+1
View File
@@ -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.
msg/manual_control_switches.msg
-8
View File
@@ -26,12 +26,4 @@ uint8 kill_switch # throttle kill: _NORMAL_, KILL
uint8 gear_switch # landing gear switch: _DOWN_, UP
uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT
# legacy "advanced" switch configuration (will be removed soon)
uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL
uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO
uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE
uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED
uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL
uint32 switch_changes # number of switch changes
msg/px4io_status.msg
+17 -28
View File
@@ -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
msg/rc_channels.msg
+22 -28
View File
@@ -1,37 +1,31 @@
uint64 timestamp # time since system start (microseconds)
uint8 FUNCTION_THROTTLE=0
uint8 FUNCTION_ROLL=1
uint8 FUNCTION_PITCH=2
uint8 FUNCTION_YAW=3
uint8 FUNCTION_MODE=4
uint8 FUNCTION_RETURN=5
uint8 FUNCTION_POSCTL=6
uint8 FUNCTION_LOITER=7
uint8 FUNCTION_OFFBOARD=8
uint8 FUNCTION_ACRO=9
uint8 FUNCTION_FLAPS=10
uint8 FUNCTION_AUX_1=11
uint8 FUNCTION_AUX_2=12
uint8 FUNCTION_AUX_3=13
uint8 FUNCTION_AUX_4=14
uint8 FUNCTION_AUX_5=15
uint8 FUNCTION_PARAM_1=16
uint8 FUNCTION_PARAM_2=17
uint8 FUNCTION_PARAM_3_5=18
uint8 FUNCTION_RATTITUDE=19
uint8 FUNCTION_KILLSWITCH=20
uint8 FUNCTION_TRANSITION=21
uint8 FUNCTION_GEAR=22
uint8 FUNCTION_ARMSWITCH=23
uint8 FUNCTION_STAB=24
uint8 FUNCTION_AUX_6=25
uint8 FUNCTION_MAN=26
uint8 FUNCTION_THROTTLE = 0
uint8 FUNCTION_ROLL = 1
uint8 FUNCTION_PITCH = 2
uint8 FUNCTION_YAW = 3
uint8 FUNCTION_RETURN = 4
uint8 FUNCTION_LOITER = 5
uint8 FUNCTION_OFFBOARD = 6
uint8 FUNCTION_FLAPS = 7
uint8 FUNCTION_AUX_1 = 8
uint8 FUNCTION_AUX_2 = 9
uint8 FUNCTION_AUX_3 = 10
uint8 FUNCTION_AUX_4 = 11
uint8 FUNCTION_AUX_5 = 12
uint8 FUNCTION_AUX_6 = 13
uint8 FUNCTION_PARAM_1 = 14
uint8 FUNCTION_PARAM_2 = 15
uint8 FUNCTION_PARAM_3_5 = 16
uint8 FUNCTION_KILLSWITCH = 17
uint8 FUNCTION_TRANSITION = 18
uint8 FUNCTION_GEAR = 19
uint8 FUNCTION_ARMSWITCH = 20
uint64 timestamp_last_valid # Timestamp of last valid RC signal
float32[18] channels # Scaled to -1..1 (throttle: 0..1)
uint8 channel_count # Number of valid channels
int8[27] function # Functions mapping
int8[21] function # Functions mapping
uint8 rssi # Receive signal strength index
bool signal_lost # Control signal lost, should be checked together with topic timeout
uint32 frame_drop_count # Number of dropped frames
msg/safety.msg
-2
View File
@@ -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
msg/vehicle_control_mode.msg
-2
View File
@@ -1,8 +1,6 @@
uint64 timestamp # time since system start (microseconds)
bool flag_armed # synonym for actuator_armed.armed
bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing
bool flag_control_manual_enabled # true if manual input is mixed in
bool flag_control_auto_enabled # true if onboard autopilot should act
bool flag_control_offboard_enabled # true if offboard control should be used
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
+11 -11
View File
@@ -66,7 +66,7 @@ static constexpr wq_config_t I2C3{"wq:I2C3", 2336, -11};
static constexpr wq_config_t I2C4{"wq:I2C4", 2336, -12};
// PX4 att/pos controllers, highest priority after sensors.
static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1760, -13};
static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1792, -13};
static constexpr wq_config_t INS0{"wq:INS0", 6000, -14};
static constexpr wq_config_t INS1{"wq:INS1", 6000, -15};
@@ -77,16 +77,16 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};
static constexpr wq_config_t uavcan{"wq:uavcan", 2576, -19};
static constexpr wq_config_t UART0{"wq:UART0", 1400, -21};
static constexpr wq_config_t UART1{"wq:UART1", 1400, -22};
static constexpr wq_config_t UART2{"wq:UART2", 1400, -23};
static constexpr wq_config_t UART3{"wq:UART3", 1400, -24};
static constexpr wq_config_t UART4{"wq:UART4", 1400, -25};
static constexpr wq_config_t UART5{"wq:UART5", 1400, -26};
static constexpr wq_config_t UART6{"wq:UART6", 1400, -27};
static constexpr wq_config_t UART7{"wq:UART7", 1400, -28};
static constexpr wq_config_t UART8{"wq:UART8", 1400, -29};
static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -30};
static constexpr wq_config_t UART0{"wq:UART0", 1632, -21};
static constexpr wq_config_t UART1{"wq:UART1", 1632, -22};
static constexpr wq_config_t UART2{"wq:UART2", 1632, -23};
static constexpr wq_config_t UART3{"wq:UART3", 1632, -24};
static constexpr wq_config_t UART4{"wq:UART4", 1632, -25};
static constexpr wq_config_t UART5{"wq:UART5", 1632, -26};
static constexpr wq_config_t UART6{"wq:UART6", 1632, -27};
static constexpr wq_config_t UART7{"wq:UART7", 1632, -28};
static constexpr wq_config_t UART8{"wq:UART8", 1632, -29};
static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1632, -30};
static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50};
platforms/posix/cmake/sitl_tests.cmake
-1
View File
@@ -10,7 +10,6 @@ set(tests
bson
commander
controllib
conv
dataman
file2
float
platforms/qurt/src/px4/common/commands_hil.c
-2
View File
@@ -77,8 +77,6 @@ const char *get_commands()
"param set RC6_MIN 992\n"
"param set RC6_TRIM 1504\n"
"param set RC_CHAN_CNT 8\n"
"param set RC_MAP_MODE_SW 5\n"
"param set RC_MAP_POSCTL_SW 7\n"
"param set RC_MAP_RETURN_SW 8\n"
"param set MC_YAW_P 1.5\n"
"param set MC_PITCH_P 3.0\n"
src/drivers/drv_pwm_output.h
-3
View File
@@ -215,9 +215,6 @@ typedef uint16_t servo_position_t;
/** force safety switch on (to enable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 28)
/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _PX4_IOC(_PWM_SERVO_BASE, 32)
/** set auxillary output mode. These correspond to enum Mode in px4fmu/fmu.cpp */
#define PWM_SERVO_MODE_NONE 0
#define PWM_SERVO_MODE_1PWM 1
src/drivers/drv_rc_input.h
-80
View File
@@ -1,80 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_rc_input.h
*
* R/C input interface.
*/
#ifndef _DRV_RC_INPUT_H
#define _DRV_RC_INPUT_H
#include <stdint.h>
#include <sys/ioctl.h>
#include <uORB/topics/input_rc.h>
#include "drv_orb_dev.h"
/**
* Path for the default R/C input device.
*
* Note that on systems with more than one R/C input path (e.g.
* PX4FMU with PX4IO connected) there may be other devices that
* respond to this protocol.
*
* Input data may be obtained by subscribing to the input_rc
* object, or by poll/reading from the device.
*/
#define RC_INPUT0_DEVICE_PATH "/dev/input_rc0"
/**
* Maximum RSSI value
*/
#define RC_INPUT_RSSI_MAX 100
/**
* Input signal type, value is a control position from zero to 100
* percent.
*/
typedef uint16_t rc_input_t;
#define _RC_INPUT_BASE 0x2b00
/** Enable RSSI input via ADC */
#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1)
/** Enable RSSI input via PWM signal */
#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2)
#endif /* _DRV_RC_INPUT_H */
src/drivers/px4io/CMakeLists.txt
+5 -1
View File
@@ -35,6 +35,10 @@ px4_add_module(
MAIN px4io
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
-Wno-error
#-DDEBUG_BUILD
STACK_MAIN
10000
SRCS
px4io.cpp
px4io_serial.cpp
@@ -42,7 +46,7 @@ px4_add_module(
DEPENDS
arch_px4io_serial
circuit_breaker
mixer
mixer_module
)
# include the px4io binary in ROMFS
src/drivers/px4io/px4io.cpp
+474 -1519
View File
File diff suppressed because it is too large Load Diff
src/drivers/px4io/px4io_params.c
+4 -3
View File
@@ -47,9 +47,10 @@
*
* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
*
* @boolean
* @min 0
* @max 1
* @value 0 IO disabled
* @value 1 IO default (RC & PWM output)
* @value 2 RC only
* @value 3 PWM output only
* @reboot_required true
* @group System
*/
src/drivers/px4io/px4io_serial.cpp
+8 -18
View File
@@ -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;
src/drivers/px4io/px4io_uploader.cpp
+2 -6
View File
@@ -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;
src/drivers/rc_input/RCInput.cpp
+2 -2
View File
@@ -480,7 +480,7 @@ void RCInput::Run()
for (unsigned i = 0; i < (unsigned)newBytes; i++) {
/* set updated flag if one complete packet was parsed */
st24_rssi = RC_INPUT_RSSI_MAX;
st24_rssi = input_rc_s::RSSI_MAX;
rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count,
&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
}
@@ -529,7 +529,7 @@ void RCInput::Run()
for (unsigned i = 0; i < (unsigned)newBytes; i++) {
/* set updated flag if one complete packet was parsed */
sumd_rssi = RC_INPUT_RSSI_MAX;
sumd_rssi = input_rc_s::RSSI_MAX;
rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
&_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe));
}
src/drivers/rc_input/RCInput.hpp
-1
View File
@@ -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.h>
src/drivers/spektrum_rc/spektrum_rc.cpp
-1
View File
@@ -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>
src/lib/mixer_module/CMakeLists.txt
+1
View File
@@ -32,3 +32,4 @@
############################################################################
px4_add_library(mixer_module mixer_module.cpp)
target_link_libraries(mixer_module PRIVATE output_limit)
src/lib/mixer_module/mixer_module.cpp
+5 -1
View File
@@ -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);
src/lib/rc/sbus.cpp
+3 -9
View File
@@ -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);
}
src/modules/commander/Commander.cpp
+3 -180
View File
@@ -2300,7 +2300,7 @@ Commander::run()
}
// evaluate the main state machine according to mode switches
if (set_main_state(&_status_changed) == TRANSITION_CHANGED) {
if (set_main_state() == TRANSITION_CHANGED) {
// play tune on mode change only if armed, blink LED always
tune_positive(_armed.armed);
_status_changed = true;
@@ -2873,29 +2873,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
_leds_counter++;
}
transition_result_t
Commander::set_main_state(bool *changed)
{
if (_safety.override_available && _safety.override_enabled) {
return set_main_state_override_on(changed);
} else {
return set_main_state_rc();
}
}
transition_result_t
Commander::set_main_state_override_on(bool *changed)
{
const transition_result_t res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags,
&_internal_state);
*changed = (res == TRANSITION_CHANGED);
return res;
}
transition_result_t
Commander::set_main_state_rc()
transition_result_t Commander::set_main_state()
{
if ((_manual_control_switches.timestamp == 0)
|| (_manual_control_switches.timestamp == _last_manual_control_switches.timestamp)) {
@@ -2912,15 +2890,8 @@ Commander::set_main_state_rc()
bool should_evaluate_rc_mode_switch =
(_last_manual_control_switches.offboard_switch != _manual_control_switches.offboard_switch)
|| (_last_manual_control_switches.return_switch != _manual_control_switches.return_switch)
|| (_last_manual_control_switches.mode_switch != _manual_control_switches.mode_switch)
|| (_last_manual_control_switches.acro_switch != _manual_control_switches.acro_switch)
|| (_last_manual_control_switches.rattitude_switch != _manual_control_switches.rattitude_switch)
|| (_last_manual_control_switches.posctl_switch != _manual_control_switches.posctl_switch)
|| (_last_manual_control_switches.loiter_switch != _manual_control_switches.loiter_switch)
|| (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot)
|| (_last_manual_control_switches.stab_switch != _manual_control_switches.stab_switch)
|| (_last_manual_control_switches.man_switch != _manual_control_switches.man_switch);
|| (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot);
if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// if already armed don't evaluate first time RC
@@ -3137,147 +3108,6 @@ Commander::set_main_state_rc()
}
}
}
return res;
} else if (_manual_control_switches.mode_switch != manual_control_switches_s::SWITCH_POS_NONE) {
/* offboard and RTL switches off or denied, check main mode switch */
switch (_manual_control_switches.mode_switch) {
case manual_control_switches_s::SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
break;
case manual_control_switches_s::SWITCH_POS_OFF: // MANUAL
if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_NONE &&
_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
/*
* Legacy mode:
* Acro switch being used as stabilized switch in FW.
*/
if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
/* manual mode is stabilized already for multirotors, so switch to acro
* for any non-manual mode
*/
if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_status.is_vtol) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, &_internal_state);
} else if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
} else {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
}
} else if (_manual_control_switches.rattitude_switch == manual_control_switches_s::SWITCH_POS_ON) {
/* Similar to acro transitions for multirotors. FW aircraft don't need a
* rattitude mode.*/
if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_RATTITUDE, _status_flags, &_internal_state);
} else {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
}
} else {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
}
} else {
/* New mode:
* - Acro is Acro
* - Manual is not default anymore when the manual switch is assigned
*/
if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
} else if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, &_internal_state);
} else if (_manual_control_switches.rattitude_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_RATTITUDE, _status_flags, &_internal_state);
} else if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
} else if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
// default to MANUAL when no manual switch is set
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
} else {
// default to STAB when the manual switch is assigned (but off)
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
}
}
// TRANSITION_DENIED is not possible here
break;
case manual_control_switches_s::SWITCH_POS_MIDDLE: // ASSIST
if (_manual_control_switches.posctl_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
print_reject_mode("POSITION CONTROL");
}
// fallback to ALTCTL
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (_manual_control_switches.posctl_switch != manual_control_switches_s::SWITCH_POS_ON) {
print_reject_mode("ALTITUDE CONTROL");
}
// fallback to MANUAL
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
// TRANSITION_DENIED is not possible here
break;
case manual_control_switches_s::SWITCH_POS_ON: // AUTO
res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
print_reject_mode("AUTO MISSION");
// fallback to LOITER if home position not set
res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to POSCTL
res = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to ALTCTL
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// fallback to MANUAL
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
// TRANSITION_DENIED is not possible here
break;
default:
break;
}
}
return res;
@@ -3359,9 +3189,6 @@ Commander::update_control_mode()
/* set vehicle_control_mode according to set_navigation_state */
_vehicle_control_mode.flag_armed = _armed.armed;
_vehicle_control_mode.flag_external_manual_override_ok =
(_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol);
switch (_status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
_vehicle_control_mode.flag_control_manual_enabled = true;
@@ -3401,10 +3228,6 @@ Commander::update_control_mode()
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
/* override is not ok for the RTL and recovery mode */
_vehicle_control_mode.flag_external_manual_override_ok = false;
/* fallthrough */
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
src/modules/commander/Commander.hpp
+1 -7
View File
@@ -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();
src/modules/mavlink/mavlink_receiver.cpp
+2 -3
View File
@@ -43,7 +43,6 @@
#include <airspeed/airspeed.h>
#include <commander/px4_custom_mode.h>
#include <conversion/rotation.h>
#include <drivers/drv_rc_input.h>
#include <ecl/geo/geo.h>
#include <systemlib/px4_macros.h>
@@ -2015,7 +2014,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
// metadata
rc.timestamp = hrt_absolute_time();
rc.timestamp_last_signal = rc.timestamp;
rc.rssi = RC_INPUT_RSSI_MAX;
rc.rssi = input_rc_s::RSSI_MAX;
rc.rc_failsafe = false;
rc.rc_lost = false;
rc.rc_lost_frame_count = 0;
@@ -2087,7 +2086,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0;
rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
rc.rssi = RC_INPUT_RSSI_MAX;
rc.rssi = input_rc_s::RSSI_MAX;
rc.values[0] = man.x / 2 + 1500; // roll
rc.values[1] = man.y / 2 + 1500; // pitch
src/modules/mavlink/streams/MANUAL_CONTROL.hpp
-3
View File
@@ -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));
}
src/modules/px4iofirmware/CMakeLists.txt
-2
View File
@@ -50,9 +50,7 @@ target_link_libraries(px4iofirmware
nuttx_apps
nuttx_arch
nuttx_c
mixer
rc
output_limit
)
if(PX4IO_PERF)
src/modules/px4iofirmware/controls.c
+20 -164
View File
@@ -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));
}
@@ -210,10 +205,6 @@ controls_init(void)
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
@@ -247,11 +238,11 @@ controls_tick()
_rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f);
/* use 1:1 scaling on 3.3V, 12-Bit ADC input */
unsigned mV = _rssi_adc_counts * 3300 / 4095;
/* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */
_rssi = (mV * RC_INPUT_RSSI_MAX / 3300);
/* scale to 0..100 (input_rc_s::RSSI_MAX == 100) */
_rssi = (mV * INPUT_RC_RSSI_MAX / 3300);
if (_rssi > RC_INPUT_RSSI_MAX) {
_rssi = RC_INPUT_RSSI_MAX;
if (_rssi > INPUT_RC_RSSI_MAX) {
_rssi = INPUT_RC_RSSI_MAX;
}
}
}
@@ -277,11 +268,11 @@ controls_tick()
if (sbus_updated) {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS);
unsigned sbus_rssi = RC_INPUT_RSSI_MAX;
unsigned sbus_rssi = INPUT_RC_RSSI_MAX; // input_rc_s::RSSI_MAX
if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
sbus_rssi = RC_INPUT_RSSI_MAX / 2;
sbus_rssi = INPUT_RC_RSSI_MAX / 2;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
@@ -298,7 +289,6 @@ controls_tick()
if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
_rssi = sbus_rssi;
}
}
}
@@ -390,91 +380,27 @@ controls_tick()
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint16_t raw = r_raw_rc_values[i];
int16_t scaled;
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) {
raw = conf[PX4IO_P_RC_CONFIG_MIN];
}
if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) {
raw = conf[PX4IO_P_RC_CONFIG_MAX];
}
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
int16_t scaled = 10000.f * r_raw_rc_values[i];
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
scaled = -scaled;
}
// TODO: find kill switch
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) {
/* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
}
if (mapped == 3 && r_setup_rc_thr_failsafe) {
/* throttle failsafe detection */
if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
} else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) {
if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_KILLSWITCH) {
/* pick out override channel, indicated by special mapping */
rc_value_override = SIGNED_TO_REG(scaled);
//rc_value_override = SIGNED_TO_REG(scaled);
}
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i))) {
r_rc_values[i] = 0;
// kill engaged
// r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
// kill disengaged
// r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
@@ -489,11 +415,6 @@ controls_tick()
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
/*
* Export the valid channel bitmap
*/
r_rc_valid = assigned_channels;
}
/*
@@ -517,85 +438,20 @@ controls_tick()
* Handle losing RC input
*/
/* if we are in failsafe, clear the override flag */
if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */
atomic_modify_clear(&r_status_flags, (
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK));
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_OK);
/* flag raw RC as lost */
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
/* Mark all channels as invalid, as we just lost the RX */
r_rc_valid = 0;
/* Set raw channel count to zero */
r_raw_rc_count = 0;
/* Set the RC_LOST alarm */
atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_RC_LOST);
}
/*
* Check for manual override.
*
* Firstly, manual override must be enabled, RC input available and a mixer loaded.
*/
if (/* condition 1: Override is always allowed */
(r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
/* condition 2: We have valid RC control inputs from the user */
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
/* condition 3: The system didn't go already into failsafe mode with fixed outputs */
!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) &&
/* condition 4: RC handling wasn't generally disabled */
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
/* condition 5: We have a valid mixer to map RC inputs to actuator outputs */
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
bool override = false;
/*
* Check mapped channel 5 (can be any remote channel,
* depends on RC_MAP_OVER parameter);
* If the value is 'high' then the pilot has
* requested override.
*
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) {
override = true;
}
/*
* If the FMU is dead then enable override if we have a mixer
* and we want to immediately override (instead of using the RC channel
* as in the case above.
*
* Also, do not enter manual override if we asked for termination
* failsafe and FMU is lost.
*/
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) {
override = true;
}
if (override) {
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE);
} else {
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
} else {
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_OVERRIDE);
}
}
static bool
src/modules/px4iofirmware/mixer.cpp
+18 -453
View File
File diff suppressed because it is too large Load Diff
src/modules/px4iofirmware/mixer.h
-42
View File
@@ -1,42 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017-2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mixer.h
*
* PX4IO mixer definitions
*
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#define PX4IO_MAX_MIXER_LENGTH 330
src/modules/px4iofirmware/protocol.h
+47 -149
View File
@@ -61,7 +61,7 @@
* the PX4 system are expressed as signed integer values scaled by 10000,
* e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and
* SIGNED_TO_REG macros to convert between register representation and
* the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float.
* the signed version.
*
* Note that the implementation of readable pages prefers registers within
* readable pages to be densely packed. Page numbers do not need to be
@@ -75,76 +75,58 @@
#define REG_TO_SIGNED(_reg) ((int16_t)(_reg))
#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed))
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)floorf((_float + 0.00005f) * 10000.0f))
#define REG_TO_BOOL(_reg) ((bool)(_reg))
#define PX4IO_PROTOCOL_VERSION 4
#define PX4IO_PROTOCOL_VERSION 5
/* maximum allowable sizes on this protocol version */
#define PX4IO_PROTOCOL_MAX_CONTROL_COUNT 8 /**< The protocol does not support more than set here, individual units might support less - see PX4IO_P_CONFIG_CONTROL_COUNT */
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
#define PX4IO_PAGE_CONFIG 0
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */
#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */
#define PX4IO_MAX_TRANSFER_LEN 64
#define PX4IO_MAX_TRANSFER_LEN 64
/* dynamic status page */
#define PX4IO_PAGE_STATUS 1
#define PX4IO_PAGE_STATUS 1
#define PX4IO_P_STATUS_FREEMEM 0
#define PX4IO_P_STATUS_CPULOAD 1
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */
#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */
#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 15) /* SUMD input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 1) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 2) /* PPM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 5) /* controls from FMU are valid */
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 6) /* raw PWM from FMU is bypassing the mixer */
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 7) /* the arming state between IO and FMU is in sync */
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 8) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 9) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 10) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 11) /* FMU was initialized and OK once */
#define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 12) /* ST24 input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 13) /* SUMD input is valid */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */
#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */
#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */
#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */
#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 0) /* timed out waiting for RC input */
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 1) /* PWM configuration or output was bad */
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* array of PWM servo output values, microseconds */
#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* array of raw RC input values, microseconds */
#define PX4IO_PAGE_RAW_RC_INPUT 4
#define PX4IO_PAGE_RAW_RC_INPUT 4
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */
#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */
@@ -159,47 +141,34 @@
#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */
#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */
/* array of scaled RC input values, -10000..10000 */
#define PX4IO_PAGE_RC_INPUT 5
#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */
#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */
/* array of raw ADC values */
#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
/* PWM servo information */
#define PX4IO_PAGE_PWM_INFO 7
#define PX4IO_PAGE_PWM_INFO 7
#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
/* setup page */
#define PX4IO_PAGE_SETUP 50
#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 2) /**< enable ADC RSSI parsing */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */
#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 3) /* OK to switch to manual override via override RC channel */
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* use custom failsafe values, not 0 values of mixer */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 7) /* Disable the IO-internal evaluation of the RC */
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 8) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 9) /* If set, the system will always output the failsafe values */
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */
#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 4) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 5) /* If set, the system will always output the failsafe values */
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 6) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_RELAYS_PAD 5
#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */
#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_VSERVO_SCALE 5 /* hardware rev [2] servo voltage correction factor (float) */
#define PX4IO_P_SETUP_DSM 6 /* DSM bind state */
enum { /* DSM bind states */
dsm_bind_power_down = 0,
dsm_bind_power_up,
@@ -218,63 +187,25 @@ enum { /* DSM bind states */
#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into
'armed' (PWM enabled) state - this is a non-data write and
hence index 12 can safely be used. */
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */
#define PX4IO_P_SETUP_FORCE_SAFETY_ON 14 /* force safety switch into 'disarmed' (PWM disabled state) */
#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */
#define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */
#define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */
#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */
#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */
#define PX4IO_P_SETUP_SCALE_ROLL 19 /**< Roll scale, in actuator units */
#define PX4IO_P_SETUP_SCALE_PITCH 20 /**< Pitch scale, in actuator units */
#define PX4IO_P_SETUP_SCALE_YAW 21 /**< Yaw scale, in actuator units */
#define PX4IO_P_SETUP_SBUS_RATE 22 /**< frame rate of SBUS1 output in Hz */
#define PX4IO_P_SETUP_MOTOR_SLEW_MAX 24 /**< max motor slew rate */
#define PX4IO_P_SETUP_THR_MDL_FAC 25 /**< factor for modelling motor control signal output to static thrust relationship */
#define PX4IO_P_SETUP_THERMAL 26 /**< thermal management */
#define PX4IO_P_SETUP_AIRMODE 27 /**< air-mode */
#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 28 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
#define PX4IO_P_SETUP_SBUS_RATE 16 /**< frame rate of SBUS1 output in Hz */
#define PX4IO_P_SETUP_THERMAL 17 /**< thermal management */
#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION 18 /**< flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set */
#define PX4IO_THERMAL_IGNORE UINT16_MAX
#define PX4IO_THERMAL_OFF 0
#define PX4IO_THERMAL_FULL 10000
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_1 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_2 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
#define PX4IO_P_CONTROLS_GROUP_VALID 64
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */
#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */
/* raw text load to the mixer parser - ignores offset */
#define PX4IO_PAGE_MIXERLOAD 52
/* R/C channel config */
#define PX4IO_PAGE_RC_CONFIG 53 /**< R/C input configuration */
#define PX4IO_P_RC_CONFIG_MIN 0 /**< lowest input value */
#define PX4IO_P_RC_CONFIG_CENTER 1 /**< center input value */
#define PX4IO_P_RC_CONFIG_MAX 2 /**< highest input value */
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /**< band around center that is ignored */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /**< mapped input value */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH 100 /**< magic value for mode switch */
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /**< channel options bitmask */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 0 /**< mapped input value */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT_KILLSWITCH 100 /**< magic value for kill switch */
#define PX4IO_P_RC_CONFIG_OPTIONS 1 /**< channel options bitmask */
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
#define PX4IO_P_RC_CONFIG_STRIDE 6 /**< spacing between channel config data */
#define PX4IO_P_RC_CONFIG_STRIDE 2 /**< spacing between channel config data */
/* PWM output - overrides mixer */
#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -282,49 +213,16 @@ enum { /* DSM bind states */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_FAILSAFE_PWM 55 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_SENSORS 56 /**< Sensors connected to PX4IO */
#define PX4IO_P_SENSORS_ALTITUDE 0 /**< Altitude of an external sensor (HoTT or S.BUS2) */
/* Debug and test page - not used in normal operation */
#define PX4IO_PAGE_TEST 127
#define PX4IO_P_TEST_LED 0 /**< set the amber LED on/off */
/* PWM minimum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM mtrim values for central position */
#define PX4IO_PAGE_CONTROL_TRIM_PWM 108 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM disarmed values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
*
* This message adds text to the mixer text buffer; the text
* buffer is drained as the definitions are consumed.
*/
#pragma pack(push, 1)
struct px4io_mixdata {
uint16_t f2i_mixer_magic;
#define F2I_MIXER_MAGIC 0x6d74
uint8_t action;
#define F2I_MIXER_ACTION_RESET 0
#define F2I_MIXER_ACTION_APPEND 1
char text[0]; /* actual text size may vary */
};
#pragma pack(pop)
#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* Serial protocol encapsulation.
*/
#define PKT_MAX_REGS 32 // by agreement w/FMU
#pragma pack(push, 1)
src/modules/px4iofirmware/px4io.c
-64
View File
@@ -58,8 +58,6 @@
# include <lib/perf/perf_counter.h>
#endif
#include <output_limit/output_limit.h>
#include <stm32_uart.h>
#define DEBUG
@@ -71,10 +69,6 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call;
output_limit_t pwm_limit;
float dt;
/*
* a set of debug buffers to allow us to send debug information from ISRs
*/
@@ -312,11 +306,6 @@ user_start(int argc, char *argv[])
LED_RING(false);
#endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
@@ -349,44 +338,6 @@ user_start(int argc, char *argv[])
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk;
syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
output_limit_init(&pwm_limit);
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 550) {
syslog(LOG_ERR, "ERR: not enough MEM");
bool phase = false;
while (true) {
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
/* Start the failsafe led init */
failsafe_led_init();
@@ -396,19 +347,8 @@ user_start(int argc, char *argv[])
uint64_t last_debug_time = 0;
uint64_t last_heartbeat_time = 0;
uint64_t last_loop_time = 0;
for (;;) {
dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f;
last_loop_time = hrt_absolute_time();
if (dt < 0.0001f) {
dt = 0.0001f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
#if defined(PX4IO_PERF)
/* track the rate at which the loop is running */
perf_count(loop_perf);
@@ -446,10 +386,6 @@ user_start(int argc, char *argv[])
*/
uint32_t heartbeat_period_us = 250 * 1000UL;
if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
heartbeat_period_us /= 4;
}
if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
src/modules/px4iofirmware/px4io.h
+2 -40
View File
@@ -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,13 @@
* Registers.
*/
extern volatile uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */
extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */
extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */
extern uint16_t r_page_direct_pwm[]; /* PX4IO_PAGE_DIRECT_PWM */
extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */
extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */
extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
extern int16_t r_page_servo_control_trim[]; /* PX4IO_PAGE_CONTROL_TRIM_PWM */
extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
/*
@@ -104,32 +95,16 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE])
#define r_mixer_limits r_page_status[PX4IO_P_STATUS_MIXER]
#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]
#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES]
#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
#define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE]
#define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL]
#define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH]
#define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW]
#define r_setup_scale_roll r_page_setup[PX4IO_P_SETUP_SCALE_ROLL]
#define r_setup_scale_pitch r_page_setup[PX4IO_P_SETUP_SCALE_PITCH]
#define r_setup_scale_yaw r_page_setup[PX4IO_P_SETUP_SCALE_YAW]
#define r_setup_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE]
#define r_setup_thr_fac r_page_setup[PX4IO_P_SETUP_THR_MDL_FAC]
#define r_setup_slew_max r_page_setup[PX4IO_P_SETUP_MOTOR_SLEW_MAX]
#define r_setup_airmode r_page_setup[PX4IO_P_SETUP_AIRMODE]
#define r_setup_flighttermination r_page_setup[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION]
#define r_control_values (&r_page_controls[0])
#define r_setup_flighttermination r_page_setup[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION]
/*
* System state structure.
@@ -147,15 +122,9 @@ struct sys_state_s {
};
extern struct sys_state_s system_state;
extern float dt;
extern bool update_mc_thrust_param;
extern bool update_trims;
/*
* PWM limit structure
*/
extern output_limit_t pwm_limit;
/*
* GPIO handling.
*/
@@ -170,7 +139,6 @@ extern output_limit_t pwm_limit;
#define LED_RING(_s) px4_arch_gpiowrite(GPIO_LED4, (_s))
# define PX4IO_RELAY_CHANNELS 0
# define ENABLE_SBUS_OUT(_s) px4_arch_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
# define VDD_SERVO_FAULT (!px4_arch_gpioread(GPIO_SERVO_FAULT_DETECT))
@@ -181,8 +149,6 @@ extern output_limit_t pwm_limit;
#define BUTTON_SAFETY px4_arch_gpioread(GPIO_BTN_SAFETY)
#define CONTROL_PAGE_INDEX(_group, _channel) (_group * PX4IO_CONTROL_CHANNELS + _channel)
#define PX4_CRITICAL_SECTION(cmd) { irqstate_t flags = px4_enter_critical_section(); cmd; px4_leave_critical_section(flags); }
void atomic_modify_or(volatile uint16_t *target, uint16_t modification);
@@ -193,10 +159,6 @@ void atomic_modify_and(volatile uint16_t *target, uint16_t modification);
* Mixer
*/
extern void mixer_tick(void);
extern int mixer_handle_text_create_mixer(void);
extern int mixer_handle_text(const void *buffer, size_t length);
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
extern void mixer_set_failsafe(void);
/**
* Safety switch/LED.
src/modules/px4iofirmware/registers.c
+9 -358
View File
File diff suppressed because it is too large Load Diff
src/modules/rc_update/params.c
+1 -286
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -1330,39 +1330,6 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_FLTMODE, 0);
/**
* Mode switch channel mapping.
*
* This is the main flight mode selector.
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for deciding about the main mode.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
/**
* Return switch channel
*
@@ -1391,62 +1358,6 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
/**
* Rattitude switch channel
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0);
/**
* Position Control switch channel
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
/**
* Loiter switch channel
*
@@ -1475,34 +1386,6 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
/**
* Acro switch channel
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
/**
* Offboard switch channel
*
@@ -1674,62 +1557,6 @@ PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0);
/**
* Stabilize switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0);
/**
* Manual switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
/**
* AUX1 Passthrough RC channel
*
@@ -2012,70 +1839,6 @@ PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0);
*/
PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
/**
* Threshold for selecting assist mode
*
* 0-1 indicate where in the full channel range the threshold sits
* 0 : min
* 1 : max
* sign indicates polarity of comparison
* positive : true when channel>th
* negative : true when 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 rattitude 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_RATT_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
*
@@ -2108,22 +1871,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
*
@@ -2204,38 +1951,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.
*
src/modules/rc_update/params_deprecated.c
+174
View File
@@ -0,0 +1,174 @@
/**
* 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);
/**
* Rattitude switch channel
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0);
/**
* Position Control switch channel
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
/**
* 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);
/**
* 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);
src/modules/rc_update/rc_update.cpp
+49 -25
View File
@@ -48,19 +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.rattitude_switch == b.rattitude_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); }
@@ -156,6 +150,53 @@ void RCUpdate::parameters_updated()
}
update_rc_functions();
// deprecated parameters, will be removed post v1.12
{
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"));
}
}
}
}
void RCUpdate::update_rc_functions()
@@ -166,19 +207,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_RATTITUDE] = _param_rc_map_ratt_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;
@@ -509,7 +544,7 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_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;
@@ -533,17 +568,6 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
if (switches.mode_slot > num_slots) {
switches.mode_slot = num_slots;
}
} else if (_param_rc_map_mode_sw.get() > 0) {
switches.mode_switch = get_rc_sw3pos_position(rc_channels_s::FUNCTION_MODE,
_param_rc_auto_th.get(), _param_rc_assist_th.get());
// only used with legacy mode switch
switches.man_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_MAN, _param_rc_man_th.get());
switches.acro_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ACRO, _param_rc_acro_th.get());
switches.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RATTITUDE, _param_rc_ratt_th.get());
switches.stab_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_STAB, _param_rc_stab_th.get());
switches.posctl_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_POSCTL, _param_rc_posctl_th.get());
}
switches.return_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RETURN, _param_rc_return_th.get());
src/modules/rc_update/rc_update.h
-13
View File
@@ -198,22 +198,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_RATT_SW>) _param_rc_map_ratt_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_AUX1>) _param_rc_map_aux1,
(ParamInt<px4::params::RC_MAP_AUX2>) _param_rc_map_aux2,
@@ -224,19 +218,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_RATT_TH>) _param_rc_ratt_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
src/modules/simulator/simulator.h
+1 -1
View File
@@ -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>
src/systemcmds/tests/CMakeLists.txt
-1
View File
@@ -37,7 +37,6 @@ set(srcs
test_bezierQuad.cpp
test_bitset.cpp
test_bson.cpp
test_conv.cpp
test_dataman.c
test_file.c
test_file2.c
src/systemcmds/tests/test_conv.cpp
-74
View File
@@ -1,74 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file test_conv.cpp
* Tests conversions used across the system.
*/
#include <sys/types.h>
#include <stdio.h>
#include <poll.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include "tests_main.h"
#include <math.h>
#include <float.h>
#include <unit_test.h>
#include <px4iofirmware/protocol.h>
int test_conv(int argc, char *argv[])
{
//PX4_INFO("Testing system conversions");
for (int i = -10000; i <= 10000; i += 1) {
float f = i / 10000.0f;
float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
if (fabsf(f - fres) > 0.0001f) {
PX4_ERR("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)),
(double)fres);
return 1;
}
}
//PX4_INFO("All conversions clean");
return 0;
}
src/systemcmds/tests/test_mixer.cpp
+3 -3
View File
@@ -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;
}
src/systemcmds/tests/test_ppm_loopback.c
+1 -1
View File
@@ -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>
src/systemcmds/tests/test_rc.c
+1 -1
View File
@@ -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"
src/systemcmds/tests/tests_main.c
-1
View File
@@ -84,7 +84,6 @@ const struct {
{"bezier", test_bezierQuad, 0},
{"bitset", test_bitset, 0},
{"bson", test_bson, 0},
{"conv", test_conv, 0},
{"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST},
{"file2", test_file2, OPT_NOJIGTEST},
{"float", test_float, 0},

Some files were not shown because too many files have changed in this diff Show More