yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

new manual_control_switches msg (split out of manual_control_setpoint) (#16270)

Browse Source 
- split out switches from manual_control_setpoint into new message manual_control_switches
 - manual_control_switches published at minimal rate (~ 1 Hz) or immediately on change
 - simple switch debounce in rc_update (2 consecutive identical decodes required)
 - manual_control_switches logged at full rate rather than sampled at (5-10% of messages logged)
 - manual_control_setpoint publish at minimal rate unless changing
 - commander handle landing gear switch for manual modes
 - processing of mode_slot and mode_switch is now split so we only do one or the other (not both)
     - a future step will be to finally drop mode_switch and accompanying switches entirely

Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Daniel Agar
2020-12-11 12:11:35 -05:00
committed by GitHub
parent 25ef76b3b8
commit ef6209ba03
25 changed files with 670 additions and 587 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@@ -78,6 +78,7 @@ set(msg_files
log_message.msg
logger_status.msg
manual_control_setpoint.msg
manual_control_switches.msg
mavlink_log.msg
mission.msg
mission_result.msg

36
msg/manual_control_setpoint.msg
View File

@@ -1,23 +1,15 @@
uint64 timestamp # time since system start (microseconds)
uint8 SWITCH_POS_NONE = 0 # switch is not mapped
uint8 SWITCH_POS_ON = 1 # switch activated (value = 1)
uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0)
uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1)
uint8 MODE_SLOT_NONE = 0 # no mode slot assigned
uint8 MODE_SLOT_1 = 1 # mode slot 1 selected
uint8 MODE_SLOT_2 = 2 # mode slot 2 selected
uint8 MODE_SLOT_3 = 3 # mode slot 3 selected
uint8 MODE_SLOT_4 = 4 # mode slot 4 selected
uint8 MODE_SLOT_5 = 5 # mode slot 5 selected
uint8 MODE_SLOT_6 = 6 # mode slot 6 selected
uint8 MODE_SLOT_NUM = 6 # number of slots
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
uint8 SOURCE_RC = 1 # radio control
uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0
uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1
uint8 SOURCE_MAVLINK_2 = 4 # mavlink instance 2
uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 4
uint8 data_source # where this input is coming from
# Any of the channels may not be available and be set to NaN
# to indicate that it does not contain valid data.
# The variable names follow the definition of the
@@ -30,19 +22,24 @@ float32 x # stick position in x direction -1..1
# in general corresponds to forward/back motion or pitch of vehicle,
# in general a positive value means forward or negative pitch and
# a negative value means backward or positive pitch
float32 y # stick position in y direction -1..1
# in general corresponds to right/left motion or roll of vehicle,
# in general a positive value means right or positive roll and
# a negative value means left or negative roll
float32 z # throttle stick position 0..1
# in general corresponds to up/down motion or thrust of vehicle,
# in general the value corresponds to the demanded throttle by the user,
# if the input is used for setting the setpoint of a vertical position
# controller any value > 0.5 means up and any value < 0.5 means down
float32 r # yaw stick/twist position, -1..1
# in general corresponds to the righthand rotation around the vertical
# (downwards) axis of the vehicle
float32 flaps # flap position
float32 aux1 # default function: camera yaw / azimuth
float32 aux2 # default function: camera pitch / tilt
float32 aux3 # default function: camera trigger
@@ -50,18 +47,3 @@ float32 aux4 # default function: camera roll
float32 aux5 # default function: payload drop
float32 aux6
uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL
uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE
uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL
uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER
uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO
uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
uint8 kill_switch # throttle kill: _NORMAL_, KILL
uint8 arm_switch # arm/disarm switch: _DISARMED_, ARMED
uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT
uint8 gear_switch # landing gear switch: _DOWN_, UP
uint8 mode_slot # the slot a specific model selector is in
uint8 data_source # where this input is coming from
uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED
uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL

37
msg/manual_control_switches.msg Normal file
View File

@@ -0,0 +1,37 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # the timestamp of the raw data (microseconds)
uint8 SWITCH_POS_NONE = 0 # switch is not mapped
uint8 SWITCH_POS_ON = 1 # switch activated (value = 1)
uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0)
uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1)
uint8 MODE_SLOT_NONE = 0 # no mode slot assigned
uint8 MODE_SLOT_1 = 1 # mode slot 1 selected
uint8 MODE_SLOT_2 = 2 # mode slot 2 selected
uint8 MODE_SLOT_3 = 3 # mode slot 3 selected
uint8 MODE_SLOT_4 = 4 # mode slot 4 selected
uint8 MODE_SLOT_5 = 5 # mode slot 5 selected
uint8 MODE_SLOT_6 = 6 # mode slot 6 selected
uint8 MODE_SLOT_NUM = 6 # number of slots
uint8 mode_slot # the slot a specific model selector is in
uint8 arm_switch # arm/disarm switch: _DISARMED_, ARMED
uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL
uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER
uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
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

54
msg/rc_channels.msg
View File

@@ -1,32 +1,32 @@
uint64 timestamp # time since system start (microseconds)
uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
uint8 RC_CHANNELS_FUNCTION_ROLL=1
uint8 RC_CHANNELS_FUNCTION_PITCH=2
uint8 RC_CHANNELS_FUNCTION_YAW=3
uint8 RC_CHANNELS_FUNCTION_MODE=4
uint8 RC_CHANNELS_FUNCTION_RETURN=5
uint8 RC_CHANNELS_FUNCTION_POSCTL=6
uint8 RC_CHANNELS_FUNCTION_LOITER=7
uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8
uint8 RC_CHANNELS_FUNCTION_ACRO=9
uint8 RC_CHANNELS_FUNCTION_FLAPS=10
uint8 RC_CHANNELS_FUNCTION_AUX_1=11
uint8 RC_CHANNELS_FUNCTION_AUX_2=12
uint8 RC_CHANNELS_FUNCTION_AUX_3=13
uint8 RC_CHANNELS_FUNCTION_AUX_4=14
uint8 RC_CHANNELS_FUNCTION_AUX_5=15
uint8 RC_CHANNELS_FUNCTION_PARAM_1=16
uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
uint8 RC_CHANNELS_FUNCTION_TRANSITION=21
uint8 RC_CHANNELS_FUNCTION_GEAR=22
uint8 RC_CHANNELS_FUNCTION_ARMSWITCH=23
uint8 RC_CHANNELS_FUNCTION_STAB=24
uint8 RC_CHANNELS_FUNCTION_AUX_6=25
uint8 RC_CHANNELS_FUNCTION_MAN=26
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
uint64 timestamp_last_valid # Timestamp of last valid RC signal
float32[18] channels # Scaled to -1..1 (throttle: 0..1)

2
msg/tools/uorb_rtps_message_ids.yaml
View File

@@ -289,6 +289,8 @@ rtps:
id: 138
- msg: estimator_selector_status
id: 139
- msg: manual_control_switches
id: 140
########## multi topics: begin ##########
- msg: actuator_controls_0
id: 150

2
src/drivers/linux_pwm_out/linux_pwm_out.cpp
View File

@@ -322,7 +322,7 @@ void task_main(int argc, char *argv[])
_controls[0].control[0] = 0.f;
_controls[0].control[1] = 0.f;
_controls[0].control[2] = 0.f;
int channel = rc_channels.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE];
int channel = rc_channels.function[rc_channels_s::FUNCTION_THROTTLE];
if (ret == 0 && channel >= 0 && channel < (int)(sizeof(rc_channels.channels) / sizeof(rc_channels.channels[0]))) {
_controls[0].control[3] = rc_channels.channels[channel];

4
src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
View File

@@ -42,7 +42,7 @@ using namespace matrix;
FlightTaskAutoMapper::FlightTaskAutoMapper() :
_sticks(this)
{};
{}
bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint)
{
@@ -187,7 +187,7 @@ float FlightTaskAutoMapper::_getLandSpeed()
// user input assisted land speed
if (_param_mpc_land_rc_help.get()
&& (_dist_to_ground < _param_mpc_land_alt1.get())
&& _sticks.checkAndSetStickInputs(_time_stamp_current)) {
&& _sticks.checkAndSetStickInputs()) {
// stick full up -1 -> stop, stick full down 1 -> double the speed
land_speed *= (1 + _sticks.getPositionExpo()(2));
}

5
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
View File

@@ -44,14 +44,13 @@ using namespace matrix;
FlightTaskManualAltitude::FlightTaskManualAltitude() :
_sticks(this)
{};
{}
bool FlightTaskManualAltitude::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
_sticks.checkAndSetStickInputs(_time_stamp_current);
_sticks.setGearAccordingToSwitch(_gear);
_sticks.checkAndSetStickInputs();
if (_sticks_data_required) {
ret = ret && _sticks.isAvailable();

3
src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
View File

@@ -42,6 +42,7 @@
#include "FlightTask.hpp"
#include "Sticks.hpp"
#include <lib/ecl/AlphaFilter/AlphaFilter.hpp>
#include <uORB/Subscription.hpp>
class FlightTaskManualAltitude : public FlightTask
{
@@ -129,6 +130,8 @@ private:
*/
void _respectGroundSlowdown();
void setGearAccordingToSwitch();
float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */
uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
float _max_speed_up = 10.0f;

63
src/lib/flight_tasks/tasks/Utility/Sticks.cpp
View File

@@ -44,36 +44,40 @@ Sticks::Sticks(ModuleParams *parent) :
ModuleParams(parent)
{};
bool Sticks::checkAndSetStickInputs(hrt_abstime now)
bool Sticks::checkAndSetStickInputs()
{
_sub_manual_control_setpoint.update();
hrt_abstime rc_timeout = (_param_com_rc_loss_t.get() * 1.5f) * 1_s;
// Sticks are rescaled linearly and exponentially to [-1,1]
if ((now - _sub_manual_control_setpoint.get().timestamp) < rc_timeout) {
manual_control_setpoint_s manual_control_setpoint;
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// Linear scale
_positions(0) = _sub_manual_control_setpoint.get().x; // NED x, pitch [-1,1]
_positions(1) = _sub_manual_control_setpoint.get().y; // NED y, roll [-1,1]
_positions(2) = -(_sub_manual_control_setpoint.get().z - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
_positions(3) = _sub_manual_control_setpoint.get().r; // yaw [-1,1]
_positions(0) = manual_control_setpoint.x; // NED x, pitch [-1,1]
_positions(1) = manual_control_setpoint.y; // NED y, roll [-1,1]
_positions(2) = -(manual_control_setpoint.z - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1]
_positions(3) = manual_control_setpoint.r; // yaw [-1,1]
// Exponential scale
_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(1) = math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
// valid stick inputs are required
const bool valid_sticks = PX4_ISFINITE(_positions(0))
&& PX4_ISFINITE(_positions(1))
&& PX4_ISFINITE(_positions(2))
&& PX4_ISFINITE(_positions(3));
const bool valid_sticks = PX4_ISFINITE(_positions(0))
&& PX4_ISFINITE(_positions(1))
&& PX4_ISFINITE(_positions(2))
&& PX4_ISFINITE(_positions(3));
_input_available = valid_sticks;
} else {
_input_available = false;
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
if (vehicle_status.rc_signal_lost) {
_input_available = false;
}
}
}
if (!_input_available) {
@@ -85,31 +89,6 @@ bool Sticks::checkAndSetStickInputs(hrt_abstime now)
return _input_available;
}
void Sticks::setGearAccordingToSwitch(landing_gear_s &gear)
{
// Only switch the landing gear up if the user switched from gear down to gear up.
// If the user had the switch in the gear up position and took off ignore it
// until he toggles the switch to avoid retracting the gear immediately on takeoff.
if (!isAvailable()) {
gear.landing_gear = landing_gear_s::GEAR_KEEP;
} else {
const int8_t gear_switch = _sub_manual_control_setpoint.get().gear_switch;
if (_gear_switch_old != gear_switch) {
if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
if (gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
gear.landing_gear = landing_gear_s::GEAR_UP;
}
}
_gear_switch_old = gear_switch;
}
}
void Sticks::limitStickUnitLengthXY(Vector2f &v)
{
const float vl = v.length();

14
src/lib/flight_tasks/tasks/Utility/Sticks.hpp
View File

@@ -44,8 +44,8 @@
#include <px4_platform_common/module_params.h>
#include <matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
class Sticks : public ModuleParams
{
@@ -53,8 +53,7 @@ public:
Sticks(ModuleParams *parent);
~Sticks() = default;
bool checkAndSetStickInputs(hrt_abstime now);
void setGearAccordingToSwitch(landing_gear_s &gear);
bool checkAndSetStickInputs();
bool isAvailable() { return _input_available; };
const matrix::Vector<float, 4> &getPosition() { return _positions; };
const matrix::Vector<float, 4> &getPositionExpo() { return _positions_expo; };
@@ -75,18 +74,17 @@ public:
static void rotateIntoHeadingFrameXY(matrix::Vector2f &v, const float yaw, const float yaw_setpoint);
private:
bool _input_available = false;
bool _input_available{false};
matrix::Vector<float, 4> _positions; ///< unmodified manual stick inputs
matrix::Vector<float, 4> _positions_expo; ///< modified manual sticks using expo function
int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; ///< old switch state
uORB::SubscriptionData<manual_control_setpoint_s> _sub_manual_control_setpoint{ORB_ID(manual_control_setpoint)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz,
(ParamFloat<px4::params::MPC_XY_MAN_EXPO>) _param_mpc_xy_man_expo,
(ParamFloat<px4::params::MPC_Z_MAN_EXPO>) _param_mpc_z_man_expo,
(ParamFloat<px4::params::MPC_YAW_EXPO>) _param_mpc_yaw_expo,
(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t
(ParamFloat<px4::params::MPC_YAW_EXPO>) _param_mpc_yaw_expo
)
};

17
src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp
View File

@@ -35,7 +35,7 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_switches.h>
using namespace time_literals;
@@ -43,14 +43,13 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
{
bool success = true;
uORB::SubscriptionData<manual_control_setpoint_s> manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
manual_control_setpoint_sub.update();
const manual_control_setpoint_s &manual_control_setpoint = manual_control_setpoint_sub.get();
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
const manual_control_switches_s &manual_control_switches = manual_control_switches_sub.get();
if (manual_control_setpoint.timestamp != 0) {
if (manual_control_switches.timestamp != 0) {
//check action switches
if (manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
// check action switches
if (manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
@@ -58,7 +57,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
}
}
if (manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
@@ -66,7 +65,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
}
}
if (manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {

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

@@ -1965,8 +1965,7 @@ Commander::run()
}
// update manual_control_setpoint before geofence (which might check sticks or switches)
const bool manual_control_setpoint_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint);
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
/* start geofence result check */
_geofence_result_sub.update(&_geofence_result);
@@ -2035,7 +2034,7 @@ Commander::run()
// reset if no longer in LOITER or if manually switched to LOITER
const bool in_loiter_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER;
const bool manual_loiter_switch_on = _manual_control_setpoint.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON;
const bool manual_loiter_switch_on = _manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON;
if (!in_loiter_mode || manual_loiter_switch_on) {
_geofence_loiter_on = false;
@@ -2044,7 +2043,7 @@ Commander::run()
// reset if no longer in RTL or if manually switched to RTL
const bool in_rtl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL;
const bool manual_return_switch_on = _manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON;
const bool manual_return_switch_on = _manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON;
if (!in_rtl_mode || manual_return_switch_on) {
_geofence_rtl_on = false;
@@ -2092,7 +2091,7 @@ Commander::run()
const float minimum_stick_deflection = 0.01f * _param_com_rc_stick_ov.get();
// transition to previous state if sticks are touched
if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < 1_s && // don't use uninitialized or old messages
if (!_status.rc_signal_lost &&
((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) ||
(fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) {
// revert to position control in any case
@@ -2144,9 +2143,9 @@ Commander::run()
const bool in_armed_state = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool arm_switch_or_button_mapped =
_manual_control_setpoint.arm_switch != manual_control_setpoint_s::SWITCH_POS_NONE;
_manual_control_switches.arm_switch != manual_control_switches_s::SWITCH_POS_NONE;
const bool arm_button_pressed = _param_arm_switch_is_button.get()
&& (_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON);
&& (_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
/* DISARM
* check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm
@@ -2157,8 +2156,8 @@ Commander::run()
&& (_manual_control_setpoint.z < 0.1f)
&& !arm_switch_or_button_mapped;
const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get() &&
(_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
(_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
(_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_ON) &&
(_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF);
if (in_armed_state &&
(_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
@@ -2182,7 +2181,7 @@ Commander::run()
_stick_off_counter++;
} else if (!(_param_arm_switch_is_button.get()
&& _manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
&& _manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)) {
/* do not reset the counter when holding the arm button longer than needed */
_stick_off_counter = 0;
}
@@ -2199,8 +2198,8 @@ Commander::run()
&& (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s);
const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() &&
(_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) &&
(_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
(_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_OFF) &&
(_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) &&
(_manual_control_setpoint.z < 0.1f || in_rearming_grace_period);
if (!in_armed_state &&
@@ -2243,12 +2242,12 @@ Commander::run()
_stick_on_counter++;
} else if (!(_param_arm_switch_is_button.get()
&& _manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
&& _manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)) {
/* do not reset the counter when holding the arm button longer than needed */
_stick_on_counter = 0;
}
_last_manual_control_setpoint_arm_switch = _manual_control_setpoint.arm_switch;
_last_manual_control_switches_arm_switch = _manual_control_switches.arm_switch;
if (arming_ret == TRANSITION_DENIED) {
/*
@@ -2260,7 +2259,47 @@ Commander::run()
tune_negative(true);
}
if (manual_control_setpoint_updated || safety_updated) {
if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) {
// handle landing gear switch if configured and in a manual mode
if ((_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) &&
(_last_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) &&
(_manual_control_switches.gear_switch != _last_manual_control_switches.gear_switch)) {
// TODO: replace with vehicle_control_mode manual
if (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
// Only switch the landing gear up if the user switched from gear down to gear up.
int8_t gear = landing_gear_s::GEAR_KEEP;
if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) {
gear = landing_gear_s::GEAR_DOWN;
} else if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
// gear up ignored unless flying
if (!_land_detector.landed && !_land_detector.maybe_landed) {
gear = landing_gear_s::GEAR_UP;
} else {
mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear")
}
}
if (gear != landing_gear_s::GEAR_KEEP) {
landing_gear_s landing_gear{};
landing_gear.landing_gear = gear;
landing_gear.timestamp = hrt_absolute_time();
_landing_gear_pub.publish(landing_gear);
}
}
}
// evaluate the main state machine according to mode switches
if (set_main_state(&_status_changed) == TRANSITION_CHANGED) {
// play tune on mode change only if armed, blink LED always
@@ -2270,7 +2309,7 @@ Commander::run()
}
/* check throttle kill switch */
if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
/* set lockdown flag */
if (!_armed.manual_lockdown) {
const char kill_switch_string[] = "Kill-switch engaged";
@@ -2286,7 +2325,7 @@ Commander::run()
_armed.manual_lockdown = true;
}
} else if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
} else if (_manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) {
if (_armed.manual_lockdown) {
mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged");
_status_changed = true;
@@ -2890,8 +2929,8 @@ Commander::set_main_state_override_on(bool *changed)
transition_result_t
Commander::set_main_state_rc(bool *changed)
{
if ((_manual_control_setpoint.timestamp == 0)
|| (_manual_control_setpoint.timestamp == _last_manual_control_setpoint.timestamp)) {
if ((_manual_control_switches.timestamp == 0)
|| (_manual_control_switches.timestamp == _last_manual_control_switches.timestamp)) {
// no manual control or no update -> nothing changed
return TRANSITION_NOT_CHANGED;
@@ -2903,23 +2942,23 @@ Commander::set_main_state_rc(bool *changed)
// only switch mode based on RC switch if necessary to also allow mode switching via MAVLink
bool should_evaluate_rc_mode_switch =
(_last_manual_control_setpoint.offboard_switch != _manual_control_setpoint.offboard_switch)
|| (_last_manual_control_setpoint.return_switch != _manual_control_setpoint.return_switch)
|| (_last_manual_control_setpoint.mode_switch != _manual_control_setpoint.mode_switch)
|| (_last_manual_control_setpoint.acro_switch != _manual_control_setpoint.acro_switch)
|| (_last_manual_control_setpoint.rattitude_switch != _manual_control_setpoint.rattitude_switch)
|| (_last_manual_control_setpoint.posctl_switch != _manual_control_setpoint.posctl_switch)
|| (_last_manual_control_setpoint.loiter_switch != _manual_control_setpoint.loiter_switch)
|| (_last_manual_control_setpoint.mode_slot != _manual_control_setpoint.mode_slot)
|| (_last_manual_control_setpoint.stab_switch != _manual_control_setpoint.stab_switch)
|| (_last_manual_control_setpoint.man_switch != _manual_control_setpoint.man_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);
if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// if already armed don't evaluate first time RC
if (_last_manual_control_setpoint.timestamp == 0) {
if (_last_manual_control_switches.timestamp == 0) {
should_evaluate_rc_mode_switch = false;
_last_manual_control_setpoint = _manual_control_setpoint;
_last_manual_control_switches = _manual_control_switches;
}
} else {
@@ -2937,41 +2976,21 @@ Commander::set_main_state_rc(bool *changed)
}
if (!should_evaluate_rc_mode_switch) {
// store the last manual control setpoint set by the pilot in a manual state
// if the system now later enters an autonomous state the pilot can move
// the sticks to break out of the autonomous state
if (!_status.rc_signal_lost && !_geofence_warning_action_on
&& (_internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL ||
_internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL ||
_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL ||
_internal_state.main_state == commander_state_s::MAIN_STATE_ACRO ||
_internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
_internal_state.main_state == commander_state_s::MAIN_STATE_STAB)) {
_last_manual_control_setpoint.timestamp = _manual_control_setpoint.timestamp;
_last_manual_control_setpoint.x = _manual_control_setpoint.x;
_last_manual_control_setpoint.y = _manual_control_setpoint.y;
_last_manual_control_setpoint.z = _manual_control_setpoint.z;
_last_manual_control_setpoint.r = _manual_control_setpoint.r;
}
/* no timestamp change or no switch change -> nothing changed */
return TRANSITION_NOT_CHANGED;
}
_last_manual_control_setpoint = _manual_control_setpoint;
_last_manual_control_switches = _manual_control_switches;
// reset the position and velocity validity calculation to give the best change of being able to select
// the desired mode
reset_posvel_validity(changed);
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
transition_result_t res = TRANSITION_NOT_CHANGED;
/* offboard switch overrides main switch */
if (_manual_control_setpoint.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, &_internal_state);
if (res == TRANSITION_DENIED) {
@@ -2985,7 +3004,7 @@ Commander::set_main_state_rc(bool *changed)
}
/* RTL switch overrides main switch */
if (_manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, &_internal_state);
if (res == TRANSITION_DENIED) {
@@ -3004,7 +3023,7 @@ Commander::set_main_state_rc(bool *changed)
}
/* Loiter switch overrides main switch */
if (_manual_control_setpoint.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
if (_manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
if (res == TRANSITION_DENIED) {
@@ -3016,14 +3035,14 @@ Commander::set_main_state_rc(bool *changed)
}
/* we know something has changed - check if we are in mode slot operation */
if (_manual_control_setpoint.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
if (_manual_control_switches.mode_slot != manual_control_switches_s::MODE_SLOT_NONE) {
if (_manual_control_setpoint.mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) {
if (_manual_control_switches.mode_slot > manual_control_switches_s::MODE_SLOT_NUM) {
PX4_WARN("m slot overflow");
return TRANSITION_DENIED;
}
int new_mode = _flight_mode_slots[_manual_control_setpoint.mode_slot - 1];
int new_mode = _flight_mode_slots[_manual_control_switches.mode_slot - 1];
if (new_mode < 0) {
/* slot is unused */
@@ -3152,143 +3171,145 @@ Commander::set_main_state_rc(bool *changed)
}
return res;
}
/* offboard and RTL switches off or denied, check main mode switch */
switch (_manual_control_setpoint.mode_switch) {
case manual_control_setpoint_s::SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
break;
} 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_setpoint_s::SWITCH_POS_OFF: // MANUAL
if (_manual_control_setpoint.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE &&
_manual_control_setpoint.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
/*
* Legacy mode:
* Acro switch being used as stabilized switch in FW.
*/
if (_manual_control_setpoint.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* manual mode is stabilized already for multirotors, so switch to acro
* for any non-manual mode
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 (_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);
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 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 if (_manual_control_setpoint.rattitude_switch == manual_control_setpoint_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) {
} 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);
}
} 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_setpoint.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
// TRANSITION_DENIED is not possible here
break;
} else if (_manual_control_setpoint.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, &_internal_state);
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);
} else if (_manual_control_setpoint.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_RATTITUDE, _status_flags, &_internal_state);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else if (_manual_control_setpoint.stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
} else if (_manual_control_setpoint.man_switch == manual_control_setpoint_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);
print_reject_mode("POSITION CONTROL");
}
}
// TRANSITION_DENIED is not possible here
break;
// fallback to ALTCTL
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, &_internal_state);
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
if (_manual_control_setpoint.posctl_switch == manual_control_setpoint_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 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("POSITION CONTROL");
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;
}
// 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_setpoint.posctl_switch != manual_control_setpoint_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_setpoint_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;

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

@@ -48,6 +48,7 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/test_motor.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -68,6 +69,7 @@
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/offboard_control_mode.h>
@@ -356,11 +358,12 @@ private:
unsigned int _leds_counter{0};
manual_control_setpoint_s _manual_control_setpoint{}; ///< the current manual control setpoint
manual_control_setpoint_s _last_manual_control_setpoint{}; ///< the manual control setpoint valid at the last mode switch
manual_control_setpoint_s _manual_control_setpoint{}; ///< the current manual control setpoint
manual_control_switches_s _manual_control_switches{};
manual_control_switches_s _last_manual_control_switches{};
hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost
int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM] {};
uint8_t _last_manual_control_setpoint_arm_switch{0};
int32_t _flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {};
uint8_t _last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE};
uint32_t _stick_off_counter{0};
uint32_t _stick_on_counter{0};
@@ -406,6 +409,7 @@ private:
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
@@ -433,6 +437,7 @@ private:
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
uORB::Publication<vehicle_status_s> _status_pub{ORB_ID(vehicle_status)};
uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};

1
src/modules/commander/commander_params.c
View File

@@ -250,6 +250,7 @@ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
* @group Commander
* @min 100
* @max 1500
* @unit ms
*/
PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000);

1
src/modules/logger/logged_topics.cpp
View File

@@ -62,6 +62,7 @@ void LoggedTopics::add_default_topics()
add_topic("hover_thrust_estimate", 100);
add_topic("input_rc", 500);
add_topic("manual_control_setpoint", 200);
add_topic("manual_control_switches");
add_topic("mission");
add_topic("mission_result");
add_topic("navigator_mission_item");

25
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -72,6 +72,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/sensor_baro.h>
@@ -3602,6 +3603,7 @@ public:
private:
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
/* do not allow top copying this class */
MavlinkStreamManualControl(MavlinkStreamManualControl &) = delete;
@@ -3623,15 +3625,20 @@ protected:
msg.y = manual_control_setpoint.y * 1000;
msg.z = manual_control_setpoint.z * 1000;
msg.r = manual_control_setpoint.r * 1000;
unsigned shift = 2;
msg.buttons = 0;
msg.buttons |= (manual_control_setpoint.mode_switch << (shift * 0));
msg.buttons |= (manual_control_setpoint.return_switch << (shift * 1));
msg.buttons |= (manual_control_setpoint.posctl_switch << (shift * 2));
msg.buttons |= (manual_control_setpoint.loiter_switch << (shift * 3));
msg.buttons |= (manual_control_setpoint.acro_switch << (shift * 4));
msg.buttons |= (manual_control_setpoint.offboard_switch << (shift * 5));
msg.buttons |= (manual_control_setpoint.kill_switch << (shift * 6));
manual_control_switches_s manual_control_switches{};
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));
}
mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg);

16
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@@ -336,8 +336,6 @@ void MulticopterPositionControl::Run()
// publish trajectory setpoint
_traj_sp_pub.publish(setpoint);
landing_gear_s gear = _flight_tasks.getGear();
// check if all local states are valid and map accordingly
set_vehicle_states(setpoint.vz);
@@ -445,14 +443,16 @@ void MulticopterPositionControl::Run()
_wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z();
// if there's any change in landing gear setpoint publish it
if (gear.landing_gear != _old_landing_gear_position
&& gear.landing_gear != landing_gear_s::GEAR_KEEP) {
_landing_gear.timestamp = time_stamp_now;
_landing_gear.landing_gear = gear.landing_gear;
_landing_gear_pub.publish(_landing_gear);
landing_gear_s landing_gear = _flight_tasks.getGear();
if (landing_gear.landing_gear != _old_landing_gear_position
&& landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) {
landing_gear.timestamp = hrt_absolute_time();
_landing_gear_pub.publish(landing_gear);
}
_old_landing_gear_position = gear.landing_gear;
_old_landing_gear_position = landing_gear.landing_gear;
} else {
// reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation

2
src/modules/mc_pos_control/MulticopterPositionControl.hpp
View File

@@ -135,7 +135,7 @@ private:
vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */
vehicle_local_position_s _local_pos{}; /**< vehicle local position */
home_position_s _home_pos{}; /**< home position */
landing_gear_s _landing_gear{};
int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
DEFINE_PARAMETERS(

47
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@@ -96,30 +96,6 @@ MulticopterRateControl::parameters_updated()
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY);
}
float
MulticopterRateControl::get_landing_gear_state()
{
// Only switch the landing gear up if we are not landed and if
// the user switched from gear down to gear up.
// If the user had the switch in the gear up position and took off ignore it
// until he toggles the switch to avoid retracting the gear immediately on takeoff.
if (_landed) {
_gear_state_initialized = false;
}
float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
if (_manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
landing_gear = landing_gear_s::GEAR_UP;
} else if (_manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
// Switching the gear off does put it into a safe defined state
_gear_state_initialized = true;
}
return landing_gear;
}
void
MulticopterRateControl::Run()
{
@@ -173,6 +149,16 @@ MulticopterRateControl::Run()
_vehicle_status_sub.update(&_vehicle_status);
if (_landing_gear_sub.updated()) {
landing_gear_s landing_gear;
if (_landing_gear_sub.copy(&landing_gear)) {
if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) {
_landing_gear = landing_gear.landing_gear;
}
}
}
const bool manual_control_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint);
// generate the rate setpoint from sticks?
@@ -183,14 +169,6 @@ MulticopterRateControl::Run()
!_v_control_mode.flag_control_velocity_enabled &&
!_v_control_mode.flag_control_position_enabled) {
// landing gear controlled from stick inputs if we are in Manual/Stabilized mode
// limit landing gear update rate to 10 Hz
if ((now - _landing_gear.timestamp) > 100_ms) {
_landing_gear.landing_gear = get_landing_gear_state();
_landing_gear.timestamp = hrt_absolute_time();
_landing_gear_pub.publish(_landing_gear);
}
if (!_v_control_mode.flag_control_attitude_enabled) {
manual_rate_sp = true;
}
@@ -202,9 +180,6 @@ MulticopterRateControl::Run()
(fabsf(_manual_control_setpoint.y) > _param_mc_ratt_th.get()) ||
(fabsf(_manual_control_setpoint.x) > _param_mc_ratt_th.get());
}
} else {
_landing_gear_sub.update(&_landing_gear);
}
if (manual_rate_sp) {
@@ -279,7 +254,7 @@ MulticopterRateControl::Run()
actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear;
actuators.timestamp_sample = angular_velocity.timestamp_sample;
// scale effort by battery status if enabled

11
src/modules/mc_rate_control/MulticopterRateControl.hpp
View File

@@ -85,12 +85,6 @@ private:
*/
void parameters_updated();
/**
* Get the landing gear state based on the manual control switch position
* @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN
*/
float get_landing_gear_state();
RateControl _rate_control; ///< class for rate control calculations
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
@@ -111,7 +105,6 @@ private:
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<vehicle_rates_setpoint_s> _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */
landing_gear_s _landing_gear{};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_control_mode_s _v_control_mode{};
vehicle_status_s _vehicle_status{};
@@ -128,10 +121,10 @@ private:
float _thrust_sp{0.0f}; /**< thrust setpoint */
bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */
hrt_abstime _last_run{0};
int8_t _landing_gear{landing_gear_s::GEAR_DOWN};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,

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

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -44,10 +44,31 @@ using namespace time_literals;
namespace RCUpdate
{
// TODO: find a better home for this
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);
}
static bool operator !=(const manual_control_switches_s &a, const manual_control_switches_s &b) { return !(a == b); }
RCUpdate::RCUpdate() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME))
WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
// initialize parameter handles
for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) {
@@ -90,10 +111,11 @@ RCUpdate::RCUpdate() :
RCUpdate::~RCUpdate()
{
perf_free(_loop_perf);
perf_free(_loop_interval_perf);
perf_free(_valid_data_interval_perf);
}
bool
RCUpdate::init()
bool RCUpdate::init()
{
if (!_input_rc_sub.registerCallback()) {
PX4_ERR("input_rc callback registration failed!");
@@ -103,29 +125,28 @@ RCUpdate::init()
return true;
}
void
RCUpdate::parameters_updated()
void RCUpdate::parameters_updated()
{
// rc values
for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) {
float min = 0.0f;
float min = 0.f;
param_get(_parameter_handles.min[i], &min);
_parameters.min[i] = min;
float trim = 0.0f;
float trim = 0.f;
param_get(_parameter_handles.trim[i], &trim);
_parameters.trim[i] = trim;
float max = 0.0f;
float max = 0.f;
param_get(_parameter_handles.max[i], &max);
_parameters.max[i] = max;
float rev = 0.0f;
float rev = 0.f;
param_get(_parameter_handles.rev[i], &rev);
_parameters.rev[i] = rev < 0.0f;
_parameters.rev[i] = (rev < 0.f);
float dz = 0.0f;
float dz = 0.f;
param_get(_parameter_handles.dz[i], &dz);
_parameters.dz[i] = dz;
}
@@ -137,52 +158,50 @@ RCUpdate::parameters_updated()
update_rc_functions();
}
void
RCUpdate::update_rc_functions()
void RCUpdate::update_rc_functions()
{
/* update RC function mappings */
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _param_rc_map_throttle.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _param_rc_map_roll.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _param_rc_map_pitch.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _param_rc_map_yaw.get() - 1;
_rc.function[rc_channels_s::FUNCTION_THROTTLE] = _param_rc_map_throttle.get() - 1;
_rc.function[rc_channels_s::FUNCTION_ROLL] = _param_rc_map_roll.get() - 1;
_rc.function[rc_channels_s::FUNCTION_PITCH] = _param_rc_map_pitch.get() - 1;
_rc.function[rc_channels_s::FUNCTION_YAW] = _param_rc_map_yaw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _param_rc_map_mode_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _param_rc_map_return_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _param_rc_map_ratt_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _param_rc_map_posctl_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _param_rc_map_loiter_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _param_rc_map_acro_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _param_rc_map_offb_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _param_rc_map_kill_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH] = _param_rc_map_arm_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION] = _param_rc_map_trans_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_GEAR] = _param_rc_map_gear_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_STAB] = _param_rc_map_stab_sw.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MAN] = _param_rc_map_man_sw.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::RC_CHANNELS_FUNCTION_FLAPS] = _param_rc_map_flaps.get() - 1;
_rc.function[rc_channels_s::FUNCTION_FLAPS] = _param_rc_map_flaps.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _param_rc_map_aux1.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _param_rc_map_aux2.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _param_rc_map_aux3.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _param_rc_map_aux4.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _param_rc_map_aux5.get() - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6] = _param_rc_map_aux6.get() - 1;
_rc.function[rc_channels_s::FUNCTION_AUX_1] = _param_rc_map_aux1.get() - 1;
_rc.function[rc_channels_s::FUNCTION_AUX_2] = _param_rc_map_aux2.get() - 1;
_rc.function[rc_channels_s::FUNCTION_AUX_3] = _param_rc_map_aux3.get() - 1;
_rc.function[rc_channels_s::FUNCTION_AUX_4] = _param_rc_map_aux4.get() - 1;
_rc.function[rc_channels_s::FUNCTION_AUX_5] = _param_rc_map_aux5.get() - 1;
_rc.function[rc_channels_s::FUNCTION_AUX_6] = _param_rc_map_aux6.get() - 1;
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
_rc.function[rc_channels_s::FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
}
}
void
RCUpdate::rc_parameter_map_poll(bool forced)
void RCUpdate::rc_parameter_map_poll(bool forced)
{
if (_rc_parameter_map_sub.updated() || forced) {
_rc_parameter_map_sub.copy(&_rc_parameter_map);
/* update parameter handles to which the RC channels are mapped */
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
if (_rc.function[rc_channels_s::FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
*/
@@ -196,7 +215,6 @@ RCUpdate::rc_parameter_map_poll(bool forced)
} else {
_parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]);
}
}
PX4_DEBUG("rc to parameter map updated");
@@ -214,69 +232,26 @@ RCUpdate::rc_parameter_map_poll(bool forced)
}
}
float
RCUpdate::get_rc_value(uint8_t func, float min_value, float max_value)
float RCUpdate::get_rc_value(uint8_t func, float min_value, float max_value) const
{
if (_rc.function[func] >= 0) {
float value = _rc.channels[_rc.function[func]];
return math::constrain(value, min_value, max_value);
} else {
return 0.0f;
return math::constrain(_rc.channels[_rc.function[func]], min_value, max_value);
}
return 0.f;
}
switch_pos_t
RCUpdate::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return manual_control_setpoint_s::SWITCH_POS_ON;
} else if (mid_inv ? value < mid_th : value > mid_th) {
return manual_control_setpoint_s::SWITCH_POS_MIDDLE;
} else {
return manual_control_setpoint_s::SWITCH_POS_OFF;
}
} else {
return manual_control_setpoint_s::SWITCH_POS_NONE;
}
}
switch_pos_t
RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return manual_control_setpoint_s::SWITCH_POS_ON;
} else {
return manual_control_setpoint_s::SWITCH_POS_OFF;
}
} else {
return manual_control_setpoint_s::SWITCH_POS_NONE;
}
}
void
RCUpdate::set_params_from_rc()
void RCUpdate::set_params_from_rc()
{
for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
if (_rc.function[rc_channels_s::FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
*/
continue;
}
float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
float rc_val = get_rc_value((rc_channels_s::FUNCTION_PARAM_1 + i), -1.f, 1.f);
/* Check if the value has changed,
* maybe we need to introduce a more aggressive limit here */
@@ -291,8 +266,7 @@ RCUpdate::set_params_from_rc()
}
}
void
RCUpdate::Run()
void RCUpdate::Run()
{
if (should_exit()) {
_input_rc_sub.unregisterCallback();
@@ -301,6 +275,7 @@ RCUpdate::Run()
}
perf_begin(_loop_perf);
perf_count(_loop_interval_perf);
// check for parameter updates
if (_parameter_update_sub.updated()) {
@@ -339,6 +314,12 @@ RCUpdate::Run()
_input_source_previous = input_rc.input_source;
_channel_count_previous = input_rc.channel_count;
const uint8_t channel_count_limited = math::min(input_rc.channel_count, RC_MAX_CHAN_COUNT);
if (channel_count_limited > _channel_count_max) {
_channel_count_max = channel_count_limited;
}
/* detect RC signal loss */
bool signal_lost = true;
@@ -347,7 +328,6 @@ RCUpdate::Run()
/* signal is lost or no enough channels */
signal_lost = true;
} else if ((input_rc.input_source == input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM ||
input_rc.input_source == input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM)
&& input_rc.channel_count == 16) {
@@ -355,7 +335,6 @@ RCUpdate::Run()
// This is a specific RC lost check for RFD 868+/900 Modems on PPM.
// The observation was that when RC is lost, 16 channels are active and the first 12 are 1000
// and the remaining ones are 0.
for (unsigned int i = 0; i < 16; i++) {
if (i < 12 && input_rc.values[i] > 999 && input_rc.values[i] < 1005) {
signal_lost = true;
@@ -390,14 +369,8 @@ RCUpdate::Run()
}
}
unsigned channel_limit = input_rc.channel_count;
if (channel_limit > RC_MAX_CHAN_COUNT) {
channel_limit = RC_MAX_CHAN_COUNT;
}
/* read out and scale values from raw message even if signal is invalid */
for (unsigned int i = 0; i < channel_limit; i++) {
for (unsigned int i = 0; i < channel_count_limited; i++) {
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
@@ -430,7 +403,7 @@ RCUpdate::Run()
} else {
/* in the configured dead zone, output zero */
_rc.channels[i] = 0.0f;
_rc.channels[i] = 0.f;
}
if (_parameters.rev[i]) {
@@ -440,7 +413,7 @@ RCUpdate::Run()
/* handle any parameter-induced blowups */
if (!PX4_ISFINITE(_rc.channels[i])) {
_rc.channels[i] = 0.0f;
_rc.channels[i] = 0.f;
}
}
@@ -451,108 +424,31 @@ RCUpdate::Run()
_rc.frame_drop_count = input_rc.rc_lost_frame_count;
/* publish rc_channels topic even if signal is invalid, for debug */
_rc_pub.publish(_rc);
_rc_channels_pub.publish(_rc);
/* only publish manual control if the signal is still present and was present once */
if (input_source_stable && channel_count_stable && !signal_lost && (input_rc.timestamp_last_signal > 0)) {
/* only publish manual control if the signal is present */
if (input_source_stable && channel_count_stable && !signal_lost
&& (input_rc.timestamp_last_signal > _last_timestamp_signal)) {
/* initialize manual setpoint */
manual_control_setpoint_s manual_control_setpoint{};
/* set mode slot to unassigned */
manual_control_setpoint.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE;
/* set the timestamp to the last signal time */
manual_control_setpoint.timestamp = input_rc.timestamp_last_signal;
manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_RC;
_last_timestamp_signal = input_rc.timestamp_last_signal;
perf_count(_valid_data_interval_perf);
/* limit controls */
manual_control_setpoint.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
manual_control_setpoint.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
manual_control_setpoint.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
manual_control_setpoint.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
manual_control_setpoint.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
manual_control_setpoint.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
manual_control_setpoint.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
manual_control_setpoint.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
manual_control_setpoint.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
manual_control_setpoint.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
manual_control_setpoint.aux6 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_6, -1.0, 1.0);
// check if channels actually updated
bool rc_updated = false;
if (_param_rc_map_fltmode.get() > 0) {
/* number of valid slots */
const int num_slots = manual_control_setpoint_s::MODE_SLOT_NUM;
/* the half width of the range of a slot is the total range
* divided by the number of slots, again divided by two
*/
const float slot_width_half = 2.0f / num_slots / 2.0f;
/* min is -1, max is +1, range is 2. We offset below min and max */
const float slot_min = -1.0f - 0.05f;
const float slot_max = 1.0f + 0.05f;
/* the slot gets mapped by first normalizing into a 0..1 interval using min
* and max. Then the right slot is obtained by multiplying with the number of
* slots. And finally we add half a slot width to ensure that integer rounding
* will take us to the correct final index.
*/
manual_control_setpoint.mode_slot = (((((_rc.channels[_param_rc_map_fltmode.get() - 1] - slot_min) * num_slots) +
slot_width_half)
/ (slot_max - slot_min)) + (1.0f / num_slots)) + 1;
if (manual_control_setpoint.mode_slot > num_slots) {
manual_control_setpoint.mode_slot = num_slots;
for (unsigned i = 0; i < channel_count_limited; i++) {
if (_rc_values_previous[i] != input_rc.values[i]) {
rc_updated = true;
break;
}
}
/* mode switches */
manual_control_setpoint.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE,
_param_rc_auto_th.get(), _param_rc_auto_th.get() < 0.f,
_param_rc_assist_th.get(), _param_rc_assist_th.get() < 0.f);
// limit processing if there's no update
if (rc_updated || (hrt_elapsed_time(&_last_manual_control_setpoint_publish) > 300_ms)) {
UpdateManualSetpoint(input_rc.timestamp_last_signal);
}
manual_control_setpoint.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,
_param_rc_ratt_th.get(), _param_rc_ratt_th.get() < 0.f);
manual_control_setpoint.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL,
_param_rc_posctl_th.get(), _param_rc_posctl_th.get() < 0.f);
manual_control_setpoint.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN,
_param_rc_return_th.get(), _param_rc_return_th.get() < 0.f);
manual_control_setpoint.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER,
_param_rc_loiter_th.get(), _param_rc_loiter_th.get() < 0.f);
manual_control_setpoint.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO,
_param_rc_acro_th.get(), _param_rc_acro_th.get() < 0.f);
manual_control_setpoint.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
_param_rc_offb_th.get(), _param_rc_offb_th.get() < 0.f);
manual_control_setpoint.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
_param_rc_killswitch_th.get(), _param_rc_killswitch_th.get() < 0.f);
manual_control_setpoint.arm_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ARMSWITCH,
_param_rc_armswitch_th.get(), _param_rc_armswitch_th.get() < 0.f);
manual_control_setpoint.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
_param_rc_trans_th.get(), _param_rc_trans_th.get() < 0.f);
manual_control_setpoint.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR,
_param_rc_gear_th.get(), _param_rc_gear_th.get() < 0.f);
manual_control_setpoint.stab_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_STAB,
_param_rc_stab_th.get(), _param_rc_stab_th.get() < 0.f);
manual_control_setpoint.man_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MAN,
_param_rc_man_th.get(), _param_rc_man_th.get() < 0.f);
/* publish manual_control_setpoint topic */
_manual_control_setpoint_pub.publish(manual_control_setpoint);
/* copy from mapped manual_control_setpoint control to control group 3 */
actuator_controls_s actuator_group_3{};
actuator_group_3.timestamp = input_rc.timestamp_last_signal;
actuator_group_3.control[0] = manual_control_setpoint.y;
actuator_group_3.control[1] = manual_control_setpoint.x;
actuator_group_3.control[2] = manual_control_setpoint.r;
actuator_group_3.control[3] = manual_control_setpoint.z;
actuator_group_3.control[4] = manual_control_setpoint.flaps;
actuator_group_3.control[5] = manual_control_setpoint.aux1;
actuator_group_3.control[6] = manual_control_setpoint.aux2;
actuator_group_3.control[7] = manual_control_setpoint.aux3;
/* publish actuator_controls_3 topic */
_actuator_group_3_pub.publish(actuator_group_3);
UpdateManualSwitches(input_rc.timestamp_last_signal);
/* Update parameters from RC Channels (tuning with RC) if activated */
if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1_s) {
@@ -560,13 +456,168 @@ RCUpdate::Run()
_last_rc_to_param_map_time = hrt_absolute_time();
}
}
memcpy(_rc_values_previous, input_rc.values, sizeof(input_rc.values[0]) * channel_count_limited);
static_assert(sizeof(_rc_values_previous) == sizeof(input_rc.values), "check sizeof(_rc_values_previous)");
}
perf_end(_loop_perf);
}
int
RCUpdate::task_spawn(int argc, char *argv[])
switch_pos_t RCUpdate::get_rc_sw3pos_position(uint8_t func, float on_th, float mid_th) const
{
if (_rc.function[func] >= 0) {
const bool on_inv = (on_th < 0.f);
const bool mid_inv = (mid_th < 0.f);
const float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return manual_control_switches_s::SWITCH_POS_ON;
} else if (mid_inv ? value < mid_th : value > mid_th) {
return manual_control_switches_s::SWITCH_POS_MIDDLE;
} else {
return manual_control_switches_s::SWITCH_POS_OFF;
}
}
return manual_control_switches_s::SWITCH_POS_NONE;
}
switch_pos_t RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th) const
{
if (_rc.function[func] >= 0) {
const bool on_inv = (on_th < 0.f);
const float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return manual_control_switches_s::SWITCH_POS_ON;
} else {
return manual_control_switches_s::SWITCH_POS_OFF;
}
}
return manual_control_switches_s::SWITCH_POS_NONE;
}
void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
{
manual_control_switches_s switches{};
switches.timestamp_sample = timestamp_sample;
// check mode slot (RC_MAP_FLTMODE) or legacy mode switch (RC_MAP_MODE_SW), but not both
if (_param_rc_map_fltmode.get() > 0) {
// number of valid slots
static constexpr int num_slots = manual_control_switches_s::MODE_SLOT_NUM;
// the half width of the range of a slot is the total range
// divided by the number of slots, again divided by two
static constexpr float slot_width_half = 1.f / num_slots;
// min is -1, max is +1, range is 2. We offset below min and max
static constexpr float slot_min = -1.f - 0.05f;
static constexpr float slot_max = 1.f + 0.05f;
// the slot gets mapped by first normalizing into a 0..1 interval using min
// and max. Then the right slot is obtained by multiplying with the number of
// slots. And finally we add half a slot width to ensure that integer rounding
// will take us to the correct final index.
const float value = _rc.channels[_param_rc_map_fltmode.get() - 1];
switches.mode_slot = (((((value - slot_min) * num_slots) + slot_width_half) / (slot_max - slot_min)) +
slot_width_half) + 1;
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());
switches.loiter_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_LOITER, _param_rc_loiter_th.get());
switches.offboard_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_OFFBOARD, _param_rc_offb_th.get());
switches.kill_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_KILLSWITCH, _param_rc_killswitch_th.get());
switches.arm_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ARMSWITCH, _param_rc_armswitch_th.get());
switches.transition_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_TRANSITION, _param_rc_trans_th.get());
switches.gear_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_GEAR, _param_rc_gear_th.get());
// last 2 switch updates identical (simple protection from bad RC data)
if (switches == _manual_switches_previous) {
const bool switches_changed = (switches != _manual_switches_last_publish);
// publish immediately on change or at ~1 Hz
if (switches_changed || (hrt_elapsed_time(&_manual_switches_last_publish.timestamp) >= 1_s)) {
uint32_t switch_changes = _manual_switches_last_publish.switch_changes;
if (switches_changed) {
switch_changes++;
}
_manual_switches_last_publish = switches;
_manual_switches_last_publish.switch_changes = switch_changes;
_manual_switches_last_publish.timestamp_sample = _manual_switches_previous.timestamp_sample;
_manual_switches_last_publish.timestamp = hrt_absolute_time();
_manual_control_switches_pub.publish(_manual_switches_last_publish);
}
}
_manual_switches_previous = switches;
}
void RCUpdate::UpdateManualSetpoint(const hrt_abstime &timestamp_sample)
{
manual_control_setpoint_s manual_control_setpoint{};
manual_control_setpoint.timestamp_sample = timestamp_sample;
manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_RC;
// limit controls
manual_control_setpoint.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f);
manual_control_setpoint.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f);
manual_control_setpoint.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f);
manual_control_setpoint.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, 0.f, 1.f);
manual_control_setpoint.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f);
manual_control_setpoint.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f);
manual_control_setpoint.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f);
manual_control_setpoint.aux3 = get_rc_value(rc_channels_s::FUNCTION_AUX_3, -1.f, 1.f);
manual_control_setpoint.aux4 = get_rc_value(rc_channels_s::FUNCTION_AUX_4, -1.f, 1.f);
manual_control_setpoint.aux5 = get_rc_value(rc_channels_s::FUNCTION_AUX_5, -1.f, 1.f);
manual_control_setpoint.aux6 = get_rc_value(rc_channels_s::FUNCTION_AUX_6, -1.f, 1.f);
// publish manual_control_setpoint topic
manual_control_setpoint.timestamp = hrt_absolute_time();
_manual_control_setpoint_pub.publish(manual_control_setpoint);
_last_manual_control_setpoint_publish = manual_control_setpoint.timestamp;
// populate and publish actuator_controls_3 copied from mapped manual_control_setpoint
actuator_controls_s actuator_group_3{};
actuator_group_3.control[0] = manual_control_setpoint.y;
actuator_group_3.control[1] = manual_control_setpoint.x;
actuator_group_3.control[2] = manual_control_setpoint.r;
actuator_group_3.control[3] = manual_control_setpoint.z;
actuator_group_3.control[4] = manual_control_setpoint.flaps;
actuator_group_3.control[5] = manual_control_setpoint.aux1;
actuator_group_3.control[6] = manual_control_setpoint.aux2;
actuator_group_3.control[7] = manual_control_setpoint.aux3;
actuator_group_3.timestamp = hrt_absolute_time();
_actuator_group_3_pub.publish(actuator_group_3);
}
int RCUpdate::task_spawn(int argc, char *argv[])
{
RCUpdate *instance = new RCUpdate();
@@ -589,14 +640,32 @@ RCUpdate::task_spawn(int argc, char *argv[])
return PX4_ERROR;
}
int
RCUpdate::custom_command(int argc, char *argv[])
int RCUpdate::print_status()
{
PX4_INFO_RAW("Running\n");
if (_channel_count_max > 0) {
PX4_INFO_RAW(" # MIN MAX TRIM DZ REV\n");
for (int i = 0; i < _channel_count_max; i++) {
PX4_INFO_RAW("%2d %4d %4d %4d %3d %3d\n", i, _parameters.min[i], _parameters.max[i], _parameters.trim[i],
_parameters.dz[i], _parameters.rev[i]);
}
}
perf_print_counter(_loop_perf);
perf_print_counter(_loop_interval_perf);
perf_print_counter(_valid_data_interval_perf);
return 0;
}
int RCUpdate::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int
RCUpdate::print_usage(const char *reason)
int RCUpdate::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
@@ -606,8 +675,8 @@ RCUpdate::print_usage(const char *reason)
R"DESCR_STR(
### Description
The rc_update module handles RC channel mapping: read the raw input channels (`input_rc`),
then apply the calibration, map the RC channels to the configured channels & mode switches,
low-pass filter, and then publish as `rc_channels` and `manual_control_setpoint`.
then apply the calibration, map the RC channels to the configured channels & mode switches
and then publish as `rc_channels` and `manual_control_setpoint`.
### Implementation
To reduce control latency, the module is scheduled on input_rc publications.

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

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -47,7 +47,6 @@
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
@@ -55,6 +54,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/rc_parameter_map.h>
@@ -85,6 +85,8 @@ public:
bool init();
int print_status() override;
private:
void Run() override;
@@ -99,6 +101,9 @@ private:
*/
void update_rc_functions();
void UpdateManualSetpoint(const hrt_abstime &timestamp_sample);
void UpdateManualSwitches(const hrt_abstime &timestamp_sample);
/**
* Update our local parameter cache.
*/
@@ -107,13 +112,13 @@ private:
/**
* Get and limit value for specified RC function. Returns NAN if not mapped.
*/
float get_rc_value(uint8_t func, float min_value, float max_value);
float get_rc_value(uint8_t func, float min_value, float max_value) const;
/**
* Get switch position for specified function.
*/
switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, float mid_th) const;
switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th) const;
/**
* Update parameters from RC channels if the functionality is activated and the
@@ -123,7 +128,7 @@ private:
*/
void set_params_from_rc();
static constexpr unsigned RC_MAX_CHAN_COUNT{input_rc_s::RC_INPUT_MAX_CHANNELS}; /**< maximum number of r/c channels we handle */
static constexpr uint8_t RC_MAX_CHAN_COUNT{input_rc_s::RC_INPUT_MAX_CHANNELS}; /**< maximum number of r/c channels we handle */
struct Parameters {
uint16_t min[RC_MAX_CHAN_COUNT];
@@ -151,25 +156,35 @@ private:
uORB::SubscriptionCallbackWorkItem _input_rc_sub{this, ORB_ID(input_rc)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)}; /**< rc parameter map subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)};
uORB::Publication<rc_channels_s> _rc_pub{ORB_ID(rc_channels)}; /**< raw r/c control topic */
uORB::Publication<actuator_controls_s> _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; /**< manual control as actuator topic */
uORB::Publication<rc_channels_s> _rc_channels_pub{ORB_ID(rc_channels)};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
uORB::Publication<manual_control_switches_s> _manual_control_switches_pub{ORB_ID(manual_control_switches)};
uORB::Publication<actuator_controls_s> _actuator_group_3_pub{ORB_ID(actuator_controls_3)};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; /**< manual control signal topic */
rc_channels_s _rc {}; /**< r/c channel data */
manual_control_switches_s _manual_switches_previous{};
manual_control_switches_s _manual_switches_last_publish{};
rc_channels_s _rc{};
rc_parameter_map_s _rc_parameter_map {};
float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {}; /**< parameter values for RC control */
hrt_abstime _last_rc_to_param_map_time = 0;
hrt_abstime _last_manual_control_setpoint_publish{0};
hrt_abstime _last_rc_to_param_map_time{0};
hrt_abstime _last_timestamp_signal{0};
uint16_t _rc_values_previous[RC_MAX_CHAN_COUNT] {};
uint8_t _channel_count_previous{0};
uint8_t _input_source_previous{input_rc_s::RC_INPUT_SOURCE_UNKNOWN};
perf_counter_t _loop_perf; /**< loop performance counter */
uint8_t _channel_count_max{0};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
perf_counter_t _valid_data_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": valid data interval")};
DEFINE_PARAMETERS(
@@ -223,9 +238,5 @@ private:
(ParamInt<px4::params::RC_CHAN_CNT>) _param_rc_chan_cnt
)
};
} /* namespace RCUpdate */

6
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@@ -198,9 +198,9 @@ VtolAttitudeControl::is_fixed_wing_requested()
{
bool to_fw = false;
if (_manual_control_setpoint.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE &&
if (_manual_control_switches.transition_switch != manual_control_switches_s::SWITCH_POS_NONE &&
_v_control_mode.flag_control_manual_enabled) {
to_fw = (_manual_control_setpoint.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON);
to_fw = (_manual_control_switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON);
} else {
// listen to transition commands if not in manual or mode switch is not mapped
@@ -403,7 +403,7 @@ VtolAttitudeControl::Run()
}
_v_control_mode_sub.update(&_v_control_mode);
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_manual_control_switches_sub.update(&_manual_control_switches);
_v_att_sub.update(&_v_att);
_local_pos_sub.update(&_local_pos);
_local_pos_sp_sub.update(&_local_pos_sp);

6
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@@ -66,7 +66,7 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/tecs_status.h>
@@ -138,7 +138,7 @@ private:
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; // setpoint subscription
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; // sensor subscription
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; //manual control setpoint subscription
uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; //manual control setpoint subscription
uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; // local position setpoint subscription
@@ -165,7 +165,7 @@ private:
actuator_controls_s _actuators_out_1{}; //actuator controls going to the fw mixer (used for elevons)
airspeed_validated_s _airspeed_validated{}; // airspeed
manual_control_setpoint_s _manual_control_setpoint{}; //manual control setpoint
manual_control_switches_s _manual_control_switches{}; //manual control setpoint
position_setpoint_triplet_s _pos_sp_triplet{};
tecs_status_s _tecs_status{};
vehicle_attitude_s _v_att{}; //vehicle attitude