forked from Archive/PX4-Autopilot
WIP: px4io lobotomy
This commit is contained in:
parent
410ca51a4a
commit
d9e33b68cf
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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,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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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,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
|
||||
|
|
|
@ -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};
|
||||
|
||||
|
|
|
@ -10,7 +10,6 @@ set(tests
|
|||
bson
|
||||
commander
|
||||
controllib
|
||||
conv
|
||||
dataman
|
||||
file2
|
||||
float
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 */
|
|
@ -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
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -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
|
||||
*/
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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));
|
||||
}
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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,3 +32,4 @@
|
|||
############################################################################
|
||||
|
||||
px4_add_library(mixer_module mixer_module.cpp)
|
||||
target_link_libraries(mixer_module PRIVATE output_limit)
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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();
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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));
|
||||
}
|
||||
|
|
|
@ -50,9 +50,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));
|
||||
}
|
||||
|
@ -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
|
||||
|
|
|
@ -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"
|
||||
|
@ -85,40 +79,12 @@ extern int _sbus_fd;
|
|||
enum mixer_source {
|
||||
MIX_NONE,
|
||||
MIX_DISARMED,
|
||||
MIX_FMU,
|
||||
MIX_OVERRIDE,
|
||||
MIX_FAILSAFE,
|
||||
MIX_OVERRIDE_FMU_OK
|
||||
};
|
||||
|
||||
static volatile mixer_source source;
|
||||
|
||||
static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
|
||||
static int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits);
|
||||
|
||||
static MixerGroup mixer_group;
|
||||
|
||||
int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits)
|
||||
{
|
||||
/* poor mans mutex */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
in_mixer = true;
|
||||
int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
|
||||
*limits = mixer_group.get_saturation_status();
|
||||
in_mixer = false;
|
||||
|
||||
return mixcount;
|
||||
}
|
||||
|
||||
void
|
||||
mixer_tick()
|
||||
{
|
||||
/* check if the mixer got modified */
|
||||
mixer_handle_text_create_mixer();
|
||||
|
||||
/* check that we are receiving fresh data from the FMU */
|
||||
irqstate_t irq_flags = enter_critical_section();
|
||||
const hrt_abstime fmu_data_received_time = system_state.fmu_data_received_time;
|
||||
|
@ -132,8 +98,7 @@ mixer_tick()
|
|||
isr_debug(1, "AP RX timeout");
|
||||
}
|
||||
|
||||
atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FMU_OK));
|
||||
atomic_modify_or(&r_status_alarms, PX4IO_P_STATUS_ALARMS_FMU_LOST);
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
|
||||
|
||||
} else {
|
||||
atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_FMU_OK);
|
||||
|
@ -148,7 +113,7 @@ mixer_tick()
|
|||
}
|
||||
|
||||
/* default to disarmed mixing */
|
||||
source = MIX_DISARMED;
|
||||
mixer_source source = MIX_DISARMED;
|
||||
|
||||
/*
|
||||
* Decide which set of controls we're using.
|
||||
|
@ -158,39 +123,9 @@ mixer_tick()
|
|||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
|
||||
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) > 0) {
|
||||
/* a channel based override has been
|
||||
* triggered, with FMU active */
|
||||
source = MIX_OVERRIDE_FMU_OK;
|
||||
|
||||
} else {
|
||||
/* don't actually mix anything - copy values from r_page_direct_pwm */
|
||||
source = MIX_NONE;
|
||||
memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
|
||||
/* mix from FMU controls */
|
||||
source = MIX_FMU;
|
||||
}
|
||||
|
||||
else if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
|
||||
|
||||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
|
||||
|
||||
/* if allowed, mix from RC inputs directly up to available rc channels */
|
||||
source = MIX_OVERRIDE_FMU_OK;
|
||||
|
||||
} else {
|
||||
/* if allowed, mix from RC inputs directly */
|
||||
source = MIX_OVERRIDE;
|
||||
}
|
||||
}
|
||||
/* don't actually mix anything - copy values from r_page_direct_pwm */
|
||||
source = MIX_NONE;
|
||||
memcpy(r_page_servos, r_page_direct_pwm, sizeof(uint16_t)*PX4IO_SERVO_COUNT);
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -200,30 +135,21 @@ mixer_tick()
|
|||
* FMU or from the mixer.
|
||||
*
|
||||
*/
|
||||
should_arm = (
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */
|
||||
&& (
|
||||
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */
|
||||
|| (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
|
||||
)
|
||||
should_arm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */
|
||||
&& ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */
|
||||
|| (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
|
||||
)
|
||||
);
|
||||
|
||||
should_arm_nothrottle = (
|
||||
(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */
|
||||
&& (((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */
|
||||
|| (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
|
||||
)
|
||||
);
|
||||
should_arm_nothrottle = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */
|
||||
&& ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */
|
||||
|| (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
|
||||
));
|
||||
|
||||
should_always_enable_pwm = (
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)
|
||||
);
|
||||
should_always_enable_pwm = ((r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK));
|
||||
|
||||
/*
|
||||
* Check if FMU is still alive, if not, terminate the flight
|
||||
|
@ -252,94 +178,19 @@ mixer_tick()
|
|||
atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE));
|
||||
}
|
||||
|
||||
/*
|
||||
* Set simple mixer trim values. If the OK flag is set the mixer is fully loaded.
|
||||
*/
|
||||
if (update_trims && r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
|
||||
update_trims = false;
|
||||
mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT);
|
||||
}
|
||||
|
||||
/*
|
||||
* Update air-mode parameter
|
||||
*/
|
||||
mixer_group.set_airmode((Mixer::Airmode)REG_TO_SIGNED(r_setup_airmode));
|
||||
|
||||
|
||||
/*
|
||||
* Run the mixers.
|
||||
*/
|
||||
if (source == MIX_FAILSAFE) {
|
||||
|
||||
/* copy failsafe values to the servo outputs */
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servos[i] = r_page_servo_failsafe[i];
|
||||
|
||||
/* safe actuators for FMU feedback */
|
||||
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
|
||||
}
|
||||
|
||||
} else if (source == MIX_DISARMED) {
|
||||
|
||||
/* copy disarmed values to the servo outputs */
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servos[i] = r_page_servo_disarmed[i];
|
||||
|
||||
/* safe actuators for FMU feedback */
|
||||
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
|
||||
}
|
||||
|
||||
} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)
|
||||
&& !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) {
|
||||
|
||||
float outputs[PX4IO_SERVO_COUNT];
|
||||
unsigned mixed;
|
||||
|
||||
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
|
||||
/* maximum value the outputs of the multirotor mixer are allowed to change in this cycle
|
||||
* factor 2 is needed because actuator outputs are in the range [-1,1]
|
||||
*/
|
||||
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
|
||||
r_setup_slew_max);
|
||||
mixer_group.set_max_delta_out_once(delta_out_max);
|
||||
}
|
||||
|
||||
/* set dt to be used in simple mixer for slew rate limiting */
|
||||
mixer_group.set_dt_once(dt);
|
||||
|
||||
/* update parameter for mc thrust model if it updated */
|
||||
if (update_mc_thrust_param) {
|
||||
mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
|
||||
update_mc_thrust_param = false;
|
||||
}
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
|
||||
|
||||
/* the pwm limit call takes care of out of band errors */
|
||||
output_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed,
|
||||
r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
|
||||
/* clamp unused outputs to zero */
|
||||
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servos[i] = 0;
|
||||
outputs[i] = 0.0f;
|
||||
}
|
||||
|
||||
/* store normalized outputs */
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
|
||||
}
|
||||
|
||||
|
||||
if (mixed && new_fmu_data) {
|
||||
new_fmu_data = false;
|
||||
|
||||
/* Trigger all timer's channels in Oneshot mode to fire
|
||||
* the oneshots with updated values.
|
||||
*/
|
||||
|
||||
up_pwm_update();
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -375,7 +226,6 @@ mixer_tick()
|
|||
}
|
||||
|
||||
/* set S.BUS1 or S.BUS2 outputs */
|
||||
|
||||
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
|
||||
sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
|
||||
|
||||
|
@ -383,8 +233,7 @@ mixer_tick()
|
|||
sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
|
||||
}
|
||||
|
||||
} else if (mixer_servos_armed && (should_always_enable_pwm
|
||||
|| (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
|
||||
} else if (mixer_servos_armed && (should_always_enable_pwm || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) {
|
||||
/* set the disarmed servo outputs. */
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
|
||||
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
|
||||
|
@ -402,287 +251,3 @@ mixer_tick()
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
static int
|
||||
mixer_callback(uintptr_t handle,
|
||||
uint8_t control_group,
|
||||
uint8_t control_index,
|
||||
float &control)
|
||||
{
|
||||
control = 0.0f;
|
||||
|
||||
if (control_group >= PX4IO_CONTROL_GROUPS) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
switch (source) {
|
||||
case MIX_FMU:
|
||||
if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
|
||||
if (r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)] == INT16_MAX) {
|
||||
//catch NAN values encoded as INT16 max for disarmed outputs
|
||||
control = NAN;
|
||||
|
||||
} else {
|
||||
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
return -1;
|
||||
|
||||
case MIX_OVERRIDE:
|
||||
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
|
||||
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
|
||||
break;
|
||||
}
|
||||
|
||||
return -1;
|
||||
|
||||
case MIX_OVERRIDE_FMU_OK:
|
||||
|
||||
/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
|
||||
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
|
||||
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
|
||||
break;
|
||||
|
||||
} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
|
||||
control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
|
||||
break;
|
||||
}
|
||||
|
||||
return -1;
|
||||
|
||||
case MIX_DISARMED:
|
||||
case MIX_FAILSAFE:
|
||||
case MIX_NONE:
|
||||
control = 0.0f;
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* apply trim offsets for override channels */
|
||||
if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) {
|
||||
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
|
||||
control_index == actuator_controls_s::INDEX_ROLL) {
|
||||
control *= REG_TO_FLOAT(r_setup_scale_roll);
|
||||
control += REG_TO_FLOAT(r_setup_trim_roll);
|
||||
|
||||
} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
|
||||
control_index == actuator_controls_s::INDEX_PITCH) {
|
||||
control *= REG_TO_FLOAT(r_setup_scale_pitch);
|
||||
control += REG_TO_FLOAT(r_setup_trim_pitch);
|
||||
|
||||
} else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
|
||||
control_index == actuator_controls_s::INDEX_YAW) {
|
||||
control *= REG_TO_FLOAT(r_setup_scale_yaw);
|
||||
control += REG_TO_FLOAT(r_setup_trim_yaw);
|
||||
}
|
||||
}
|
||||
|
||||
/* limit output */
|
||||
if (control > 1.0f) {
|
||||
control = 1.0f;
|
||||
|
||||
} else if (control < -1.0f) {
|
||||
control = -1.0f;
|
||||
}
|
||||
|
||||
/* motor spinup phase - lock throttle to zero */
|
||||
if ((pwm_limit.state == OUTPUT_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
|
||||
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
|
||||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
|
||||
control_index == actuator_controls_s::INDEX_THROTTLE) {
|
||||
/* limit the throttle output to zero during motor spinup,
|
||||
* as the motors cannot follow any demand yet
|
||||
*/
|
||||
control = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* only safety off, but not armed - set throttle as invalid */
|
||||
if (should_arm_nothrottle && !should_arm) {
|
||||
if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE ||
|
||||
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
|
||||
control_index == actuator_controls_s::INDEX_THROTTLE) {
|
||||
/* mark the throttle as invalid */
|
||||
control = NAN;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* XXX error handling here should be more aggressive; currently it is
|
||||
* possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has
|
||||
* not loaded faithfully.
|
||||
*/
|
||||
|
||||
static char mixer_text[PX4IO_MAX_MIXER_LENGTH]; /* large enough for one mixer */
|
||||
static unsigned mixer_text_length = 0;
|
||||
static bool mixer_update_pending = false;
|
||||
|
||||
int
|
||||
mixer_handle_text_create_mixer()
|
||||
{
|
||||
/* only run on update */
|
||||
if (!mixer_update_pending) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* do not allow a mixer change while safety off and FMU armed */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* abort if we're in the mixer - it will be tried again in the next iteration */
|
||||
if (in_mixer) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* process the text buffer, adding new mixers as their descriptions can be parsed */
|
||||
unsigned resid = mixer_text_length;
|
||||
mixer_group.load_from_buf(mixer_callback, 0, &mixer_text[0], resid);
|
||||
|
||||
/* if anything was parsed */
|
||||
if (resid != mixer_text_length) {
|
||||
|
||||
isr_debug(2, "used %u", mixer_text_length - resid);
|
||||
|
||||
/* copy any leftover text to the base of the buffer for re-use */
|
||||
if (resid > 0) {
|
||||
memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
|
||||
/* enforce null termination */
|
||||
mixer_text[resid] = '\0';
|
||||
}
|
||||
|
||||
mixer_text_length = resid;
|
||||
}
|
||||
|
||||
mixer_update_pending = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
mixer_handle_text(const void *buffer, size_t length)
|
||||
{
|
||||
/* do not allow a mixer change while safety off and FMU armed */
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
|
||||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* disable mixing, will be enabled once load is complete */
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK);
|
||||
|
||||
/* set the update flags to dirty so we reload those values after a mixer change */
|
||||
update_trims = true;
|
||||
update_mc_thrust_param = true;
|
||||
|
||||
/* abort if we're in the mixer - the caller is expected to retry */
|
||||
if (in_mixer) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
px4io_mixdata *msg = (px4io_mixdata *)buffer;
|
||||
|
||||
isr_debug(2, "mix txt %u", length);
|
||||
|
||||
if (length < sizeof(px4io_mixdata)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
unsigned text_length = length - sizeof(px4io_mixdata);
|
||||
|
||||
switch (msg->action) {
|
||||
case F2I_MIXER_ACTION_RESET:
|
||||
isr_debug(2, "reset");
|
||||
|
||||
/* THEN actually delete it */
|
||||
mixer_group.reset();
|
||||
mixer_text_length = 0;
|
||||
|
||||
/* FALLTHROUGH */
|
||||
case F2I_MIXER_ACTION_APPEND:
|
||||
isr_debug(2, "append %d", length);
|
||||
|
||||
/* check for overflow - this would be really fatal */
|
||||
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
|
||||
atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_MIXER_OK);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* check if the last item has been processed - bail out if not */
|
||||
if (mixer_update_pending) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* append mixer text and nul-terminate, guard against overflow */
|
||||
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
|
||||
mixer_text_length += text_length;
|
||||
mixer_text[mixer_text_length] = '\0';
|
||||
isr_debug(2, "buflen %u", mixer_text_length);
|
||||
|
||||
/* flag the buffer as ready */
|
||||
mixer_update_pending = true;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
mixer_set_failsafe()
|
||||
{
|
||||
/*
|
||||
* Check if a custom failsafe value has been written,
|
||||
* or if the mixer is not ok and bail out.
|
||||
*/
|
||||
|
||||
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* set failsafe defaults to the values for all inputs = 0 */
|
||||
float outputs[PX4IO_SERVO_COUNT];
|
||||
unsigned mixed;
|
||||
|
||||
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
|
||||
/* maximum value the outputs of the multirotor mixer are allowed to change in this cycle
|
||||
* factor 2 is needed because actuator outputs are in the range [-1,1]
|
||||
*/
|
||||
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
|
||||
r_setup_slew_max);
|
||||
mixer_group.set_max_delta_out_once(delta_out_max);
|
||||
}
|
||||
|
||||
/* set dt to be used in simple mixer for slew rate limiting */
|
||||
mixer_group.set_dt_once(dt);
|
||||
|
||||
/* update parameter for mc thrust model if it updated */
|
||||
if (update_mc_thrust_param) {
|
||||
mixer_group.set_thrust_factor(REG_TO_FLOAT(r_setup_thr_fac));
|
||||
update_mc_thrust_param = false;
|
||||
}
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
|
||||
|
||||
/* scale to PWM and update the servo outputs as required */
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
|
||||
/* scale to servo output */
|
||||
r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
|
||||
|
||||
}
|
||||
|
||||
/* disable the rest of the outputs */
|
||||
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servo_failsafe[i] = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -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,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)
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -74,7 +74,6 @@ static const uint16_t r_page_config[] = {
|
|||
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
|
||||
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS,
|
||||
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
|
||||
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
|
||||
};
|
||||
|
||||
/**
|
||||
|
@ -89,17 +88,8 @@ volatile uint16_t r_page_status[] = {
|
|||
[PX4IO_P_STATUS_ALARMS] = 0,
|
||||
[PX4IO_P_STATUS_VSERVO] = 0,
|
||||
[PX4IO_P_STATUS_VRSSI] = 0,
|
||||
[PX4IO_P_STATUS_PRSSI] = 0,
|
||||
[PX4IO_P_STATUS_MIXER] = 0,
|
||||
};
|
||||
|
||||
/**
|
||||
* PAGE 2
|
||||
*
|
||||
* Post-mixed actuator values.
|
||||
*/
|
||||
uint16_t r_page_actuators[PX4IO_SERVO_COUNT];
|
||||
|
||||
/**
|
||||
* PAGE 3
|
||||
*
|
||||
|
@ -122,16 +112,6 @@ uint16_t r_page_raw_rc_input[] = {
|
|||
[PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
* PAGE 5
|
||||
*
|
||||
* Scaled/routed RC input
|
||||
*/
|
||||
uint16_t r_page_rc_input[] = {
|
||||
[PX4IO_P_RC_VALID] = 0,
|
||||
[PX4IO_P_RC_BASE ...(PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
* Scratch page; used for registers that are constructed as-read.
|
||||
*
|
||||
|
@ -155,66 +135,29 @@ uint16_t r_page_direct_pwm[PX4IO_SERVO_COUNT];
|
|||
volatile uint16_t r_page_setup[] = {
|
||||
/* default to RSSI ADC functionality */
|
||||
[PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI,
|
||||
[PX4IO_P_SETUP_ARMING] = (PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE),
|
||||
[PX4IO_P_SETUP_ARMING] = 0,
|
||||
[PX4IO_P_SETUP_PWM_RATES] = 0,
|
||||
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
|
||||
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
|
||||
[PX4IO_P_SETUP_SBUS_RATE] = 72,
|
||||
/* this is unused, but we will pad it for readability (the compiler pads it automatically) */
|
||||
[PX4IO_P_SETUP_RELAYS_PAD] = 0,
|
||||
#ifdef ADC_VSERVO
|
||||
[PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
|
||||
#else
|
||||
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
|
||||
#endif
|
||||
[PX4IO_P_SETUP_SET_DEBUG] = 0,
|
||||
[PX4IO_P_SETUP_REBOOT_BL] = 0,
|
||||
[PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0,
|
||||
[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
|
||||
[PX4IO_P_SETUP_PWM_REVERSE] = 0,
|
||||
[PX4IO_P_SETUP_TRIM_ROLL] = 0,
|
||||
[PX4IO_P_SETUP_TRIM_PITCH] = 0,
|
||||
[PX4IO_P_SETUP_TRIM_YAW] = 0,
|
||||
[PX4IO_P_SETUP_SCALE_ROLL] = 10000,
|
||||
[PX4IO_P_SETUP_SCALE_PITCH] = 10000,
|
||||
[PX4IO_P_SETUP_SCALE_YAW] = 10000,
|
||||
[PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0,
|
||||
[PX4IO_P_SETUP_AIRMODE] = 0,
|
||||
[PX4IO_P_SETUP_THR_MDL_FAC] = 0,
|
||||
[PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE,
|
||||
[PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION] = 0
|
||||
};
|
||||
|
||||
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
|
||||
PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
|
||||
PX4IO_P_SETUP_FEATURES_PWM_RSSI)
|
||||
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT | PX4IO_P_SETUP_FEATURES_ADC_RSSI)
|
||||
|
||||
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
|
||||
PX4IO_P_SETUP_ARMING_FMU_PREARMED | \
|
||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
|
||||
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
|
||||
PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
|
||||
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
|
||||
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
|
||||
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
|
||||
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
|
||||
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
|
||||
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
|
||||
PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
|
||||
PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
|
||||
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
|
||||
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
|
||||
|
||||
/**
|
||||
* PAGE 101
|
||||
*
|
||||
* Control values from the FMU.
|
||||
*/
|
||||
uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
|
||||
|
||||
/*
|
||||
* PAGE 102 does not have a buffer.
|
||||
*/
|
||||
|
||||
/**
|
||||
* PAGE 103
|
||||
|
@ -239,30 +182,6 @@ uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STR
|
|||
*/
|
||||
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
|
||||
|
||||
/**
|
||||
* PAGE 106
|
||||
*
|
||||
* minimum PWM values when armed
|
||||
*
|
||||
*/
|
||||
uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN };
|
||||
|
||||
/**
|
||||
* PAGE 107
|
||||
*
|
||||
* maximum PWM values when armed
|
||||
*
|
||||
*/
|
||||
uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX };
|
||||
|
||||
/**
|
||||
* PAGE 108
|
||||
*
|
||||
* trim values for center position
|
||||
*
|
||||
*/
|
||||
int16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM };
|
||||
|
||||
/**
|
||||
* PAGE 109
|
||||
*
|
||||
|
@ -274,29 +193,7 @@ uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
|
|||
int
|
||||
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
|
||||
{
|
||||
|
||||
switch (page) {
|
||||
|
||||
/* handle bulk controls input */
|
||||
case PX4IO_PAGE_CONTROLS:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
|
||||
|
||||
/* XXX range-check value? */
|
||||
r_page_controls[offset] = *values;
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
}
|
||||
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
|
||||
|
||||
system_state.fmu_data_received_time = hrt_absolute_time();
|
||||
|
||||
break;
|
||||
|
||||
/* handle raw PWM input */
|
||||
case PX4IO_PAGE_DIRECT_PWM:
|
||||
|
||||
|
@ -351,118 +248,30 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
|||
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_MIN_PWM:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
if (*values == 0) {
|
||||
/* ignore 0 */
|
||||
} else if (*values > PWM_HIGHEST_MIN) {
|
||||
r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
|
||||
|
||||
} else if (*values < PWM_LOWEST_MIN) {
|
||||
r_page_servo_control_min[offset] = PWM_LOWEST_MIN;
|
||||
|
||||
} else {
|
||||
r_page_servo_control_min[offset] = *values;
|
||||
}
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_MAX_PWM:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
if (*values == 0) {
|
||||
/* ignore 0 */
|
||||
} else if (*values > PWM_HIGHEST_MAX) {
|
||||
r_page_servo_control_max[offset] = PWM_HIGHEST_MAX;
|
||||
|
||||
} else if (*values < PWM_LOWEST_MAX) {
|
||||
r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
|
||||
|
||||
} else {
|
||||
r_page_servo_control_max[offset] = *values;
|
||||
}
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_TRIM_PWM:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
r_page_servo_control_trim[offset] = *values;
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
}
|
||||
|
||||
update_trims = true;
|
||||
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_DISARMED_PWM: {
|
||||
/* flag for all outputs */
|
||||
bool all_disarmed_off = true;
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
if (*values == 0) {
|
||||
/* 0 means disabling always PWM */
|
||||
r_page_servo_disarmed[offset] = 0;
|
||||
|
||||
} else if (*values < PWM_LOWEST_MIN) {
|
||||
r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
|
||||
all_disarmed_off = false;
|
||||
|
||||
} else if (*values > PWM_HIGHEST_MAX) {
|
||||
r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
|
||||
all_disarmed_off = false;
|
||||
|
||||
} else {
|
||||
r_page_servo_disarmed[offset] = *values;
|
||||
all_disarmed_off = false;
|
||||
}
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
}
|
||||
|
||||
if (all_disarmed_off) {
|
||||
/* disable PWM output if disarmed */
|
||||
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
|
||||
|
||||
} else {
|
||||
/* enable PWM output always */
|
||||
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
/* handle text going to the mixer parser */
|
||||
case PX4IO_PAGE_MIXERLOAD:
|
||||
/* do not change the mixer if FMU is armed and IO's safety is off
|
||||
* this state defines an active system. This check is done in the
|
||||
* text handling function.
|
||||
*/
|
||||
return mixer_handle_text(values, num_values * sizeof(*values));
|
||||
|
||||
default:
|
||||
|
||||
/* avoid offset wrap */
|
||||
|
@ -504,21 +313,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
* Allow FMU override of arming state (to allow in-air restores),
|
||||
* but only if the arming state is not in sync on the IO side.
|
||||
*/
|
||||
|
||||
if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
||||
|
||||
} else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
|
||||
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
|
||||
r_status_flags = value;
|
||||
|
||||
}
|
||||
|
||||
if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) {
|
||||
|
||||
/* update failsafe values, now that the mixer is set to ok */
|
||||
mixer_set_failsafe();
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
|
@ -542,32 +341,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
|
||||
/* disable the conflicting options with SBUS 1 */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_ADC_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
}
|
||||
|
||||
/* disable the conflicting options with SBUS 2 */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_ADC_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* disable the conflicting options with ADC RSSI */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
}
|
||||
|
||||
/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
|
||||
if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
|
||||
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
value &= ~(PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
|
||||
}
|
||||
|
||||
/* apply changes */
|
||||
|
@ -579,18 +365,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
|
||||
value &= PX4IO_P_SETUP_ARMING_VALID;
|
||||
|
||||
/*
|
||||
* Update arming state - disarm if no longer OK.
|
||||
* This builds on the requirement that the FMU driver
|
||||
* asks about the FMU arming state on initialization,
|
||||
* so that an in-air reset of FMU can not lead to a
|
||||
* lockup of the IO arming state.
|
||||
*/
|
||||
|
||||
if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
|
||||
}
|
||||
|
||||
/*
|
||||
* If the failsafe termination flag is set, do not allow the autopilot to unset it
|
||||
*/
|
||||
|
@ -689,32 +463,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_RC_THR_FAILSAFE_US:
|
||||
if (value > 650 && value < 2350) {
|
||||
r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_SBUS_RATE:
|
||||
r_page_setup[offset] = value;
|
||||
sbus1_set_output_rate_hz(value);
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_THR_MDL_FAC:
|
||||
update_mc_thrust_param = true;
|
||||
r_page_setup[offset] = value;
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_PWM_REVERSE:
|
||||
case PX4IO_P_SETUP_TRIM_ROLL:
|
||||
case PX4IO_P_SETUP_TRIM_PITCH:
|
||||
case PX4IO_P_SETUP_TRIM_YAW:
|
||||
case PX4IO_P_SETUP_SCALE_ROLL:
|
||||
case PX4IO_P_SETUP_SCALE_PITCH:
|
||||
case PX4IO_P_SETUP_SCALE_YAW:
|
||||
case PX4IO_P_SETUP_MOTOR_SLEW_MAX:
|
||||
case PX4IO_P_SETUP_AIRMODE:
|
||||
case PX4IO_P_SETUP_THERMAL:
|
||||
case PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION:
|
||||
r_page_setup[offset] = value;
|
||||
|
@ -747,11 +500,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
|
||||
switch (index) {
|
||||
|
||||
case PX4IO_P_RC_CONFIG_MIN:
|
||||
case PX4IO_P_RC_CONFIG_CENTER:
|
||||
case PX4IO_P_RC_CONFIG_MAX:
|
||||
case PX4IO_P_RC_CONFIG_DEADZONE:
|
||||
case PX4IO_P_RC_CONFIG_ASSIGNMENT:
|
||||
conf[index] = value;
|
||||
break;
|
||||
|
@ -760,9 +508,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
|
||||
|
||||
/* clear any existing RC disabled flag */
|
||||
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
|
||||
|
||||
/* set all options except the enabled option */
|
||||
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
|
||||
|
@ -772,33 +517,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
uint8_t count = 0;
|
||||
bool disabled = false;
|
||||
|
||||
/* assert min..center..max ordering */
|
||||
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
|
||||
count++;
|
||||
}
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
|
||||
count++;
|
||||
}
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
|
||||
count++;
|
||||
}
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
|
||||
count++;
|
||||
}
|
||||
|
||||
/* assert deadzone is sane */
|
||||
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
|
||||
count++;
|
||||
}
|
||||
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
|
||||
disabled = true;
|
||||
|
||||
} else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) &&
|
||||
(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) {
|
||||
(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_KILLSWITCH)) {
|
||||
count++;
|
||||
}
|
||||
|
||||
|
@ -863,49 +586,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
|
||||
/* PX4IO_P_STATUS_ALARMS maintained externally */
|
||||
|
||||
#ifdef ADC_VBATT
|
||||
/* PX4IO_P_STATUS_VBATT */
|
||||
{
|
||||
/*
|
||||
* Coefficients here derived by measurement of the 5-16V
|
||||
* range on one unit, validated on sample points of another unit
|
||||
*
|
||||
* Data in Tools/tests-host/data folder.
|
||||
*
|
||||
* measured slope = 0.004585267878277 (int: 4585)
|
||||
* nominal theoretic slope: 0.00459340659 (int: 4593)
|
||||
* intercept = 0.016646394188076 (int: 16646)
|
||||
* nominal theoretic intercept: 0.00 (int: 0)
|
||||
*
|
||||
*/
|
||||
unsigned counts = adc_measure(ADC_VBATT);
|
||||
|
||||
if (counts != 0xffff) {
|
||||
unsigned mV = (166460 + (counts * 45934)) / 10000;
|
||||
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
|
||||
|
||||
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#ifdef ADC_IBATT
|
||||
/* PX4IO_P_STATUS_IBATT */
|
||||
{
|
||||
/*
|
||||
note that we have no idea what sort of
|
||||
current sensor is attached, so we just
|
||||
return the raw 12 bit ADC value and let the
|
||||
FMU sort it out, with user selectable
|
||||
configuration for their sensor
|
||||
*/
|
||||
unsigned counts = adc_measure(ADC_IBATT);
|
||||
|
||||
if (counts != 0xffff) {
|
||||
r_page_status[PX4IO_P_STATUS_IBATT] = counts;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef ADC_VSERVO
|
||||
/* PX4IO_P_STATUS_VSERVO */
|
||||
{
|
||||
|
@ -917,6 +597,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#ifdef ADC_RSSI
|
||||
/* PX4IO_P_STATUS_VRSSI */
|
||||
|
@ -937,12 +618,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
|
||||
case PX4IO_PAGE_RAW_ADC_INPUT:
|
||||
memset(r_page_scratch, 0, sizeof(r_page_scratch));
|
||||
#ifdef ADC_VBATT
|
||||
r_page_scratch[0] = adc_measure(ADC_VBATT);
|
||||
#endif
|
||||
#ifdef ADC_IBATT
|
||||
r_page_scratch[1] = adc_measure(ADC_IBATT);
|
||||
#endif
|
||||
|
||||
#ifdef ADC_VSERVO
|
||||
r_page_scratch[0] = adc_measure(ADC_VSERVO);
|
||||
|
@ -972,10 +647,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
SELECT_PAGE(r_page_config);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_ACTUATORS:
|
||||
SELECT_PAGE(r_page_actuators);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_SERVOS:
|
||||
SELECT_PAGE(r_page_servos);
|
||||
break;
|
||||
|
@ -984,19 +655,11 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
SELECT_PAGE(r_page_raw_rc_input);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_RC_INPUT:
|
||||
SELECT_PAGE(r_page_rc_input);
|
||||
break;
|
||||
|
||||
/* readback of input pages */
|
||||
case PX4IO_PAGE_SETUP:
|
||||
SELECT_PAGE(r_page_setup);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROLS:
|
||||
SELECT_PAGE(r_page_controls);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_RC_CONFIG:
|
||||
SELECT_PAGE(r_page_rc_input_config);
|
||||
break;
|
||||
|
@ -1009,18 +672,6 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
SELECT_PAGE(r_page_servo_failsafe);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_MIN_PWM:
|
||||
SELECT_PAGE(r_page_servo_control_min);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_MAX_PWM:
|
||||
SELECT_PAGE(r_page_servo_control_max);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_CONTROL_TRIM_PWM:
|
||||
SELECT_PAGE(r_page_servo_control_trim);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_DISARMED_PWM:
|
||||
SELECT_PAGE(r_page_servo_disarmed);
|
||||
break;
|
||||
|
|
|
@ -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.
|
||||
*
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
@ -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 ×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;
|
||||
|
@ -533,17 +568,6 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample)
|
|||
if (switches.mode_slot > num_slots) {
|
||||
switches.mode_slot = num_slots;
|
||||
}
|
||||
|
||||
} else if (_param_rc_map_mode_sw.get() > 0) {
|
||||
switches.mode_switch = get_rc_sw3pos_position(rc_channels_s::FUNCTION_MODE,
|
||||
_param_rc_auto_th.get(), _param_rc_assist_th.get());
|
||||
|
||||
// only used with legacy mode switch
|
||||
switches.man_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_MAN, _param_rc_man_th.get());
|
||||
switches.acro_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ACRO, _param_rc_acro_th.get());
|
||||
switches.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RATTITUDE, _param_rc_ratt_th.get());
|
||||
switches.stab_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_STAB, _param_rc_stab_th.get());
|
||||
switches.posctl_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_POSCTL, _param_rc_posctl_th.get());
|
||||
}
|
||||
|
||||
switches.return_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RETURN, _param_rc_return_th.get());
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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>
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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"
|
||||
|
||||
|
|
|
@ -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},
|
||||
|
|
|
@ -48,7 +48,6 @@ extern int test_atomic_bitset(int argc, char *argv[]);
|
|||
extern int test_bezierQuad(int argc, char *argv[]);
|
||||
extern int test_bitset(int argc, char *argv[]);
|
||||
extern int test_bson(int argc, char *argv[]);
|
||||
extern int test_conv(int argc, char *argv[]);
|
||||
extern int test_dataman(int argc, char *argv[]);
|
||||
extern int test_file(int argc, char *argv[]);
|
||||
extern int test_file2(int argc, char *argv[]);
|
||||
|
|
Loading…
Reference in New Issue