From ef6209ba03d3627259ced8d5898bdfc4db0b094b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 11 Dec 2020 12:11:35 -0500 Subject: [PATCH] new manual_control_switches msg (split out of manual_control_setpoint) (#16270) - 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 --- msg/CMakeLists.txt | 1 + msg/manual_control_setpoint.msg | 36 +- msg/manual_control_switches.msg | 37 ++ msg/rc_channels.msg | 54 +- msg/tools/uorb_rtps_message_ids.yaml | 2 + src/drivers/linux_pwm_out/linux_pwm_out.cpp | 2 +- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 4 +- .../FlightTaskManualAltitude.cpp | 5 +- .../FlightTaskManualAltitude.hpp | 3 + src/lib/flight_tasks/tasks/Utility/Sticks.cpp | 63 +-- src/lib/flight_tasks/tasks/Utility/Sticks.hpp | 14 +- .../checks/manualControlCheck.cpp | 17 +- src/modules/commander/Commander.cpp | 355 +++++++------ src/modules/commander/Commander.hpp | 13 +- src/modules/commander/commander_params.c | 1 + src/modules/logger/logged_topics.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 25 +- .../MulticopterPositionControl.cpp | 16 +- .../MulticopterPositionControl.hpp | 2 +- .../MulticopterRateControl.cpp | 47 +- .../MulticopterRateControl.hpp | 11 +- src/modules/rc_update/rc_update.cpp | 487 ++++++++++-------- src/modules/rc_update/rc_update.h | 49 +- .../vtol_att_control_main.cpp | 6 +- .../vtol_att_control/vtol_att_control_main.h | 6 +- 25 files changed, 670 insertions(+), 587 deletions(-) create mode 100644 msg/manual_control_switches.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 55cc6c5b90..c5c44dc09a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -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 diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index febe5edc4e..16abfedb26 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -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 diff --git a/msg/manual_control_switches.msg b/msg/manual_control_switches.msg new file mode 100644 index 0000000000..fb68c1d44d --- /dev/null +++ b/msg/manual_control_switches.msg @@ -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 diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 8986ffb5a5..18d8d97f58 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -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) diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 059d1f033c..4effe7d83d 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -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 diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index c2ec391d43..884e53cb9f 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -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]; diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index ae74752f4d..0699af4d37 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -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)); } diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 7d784b8f72..215fe0cf5d 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -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(); diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 60378b2a2a..4980ae8d1c 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -42,6 +42,7 @@ #include "FlightTask.hpp" #include "Sticks.hpp" #include +#include 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; diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.cpp b/src/lib/flight_tasks/tasks/Utility/Sticks.cpp index e489332590..a271db5f18 100644 --- a/src/lib/flight_tasks/tasks/Utility/Sticks.cpp +++ b/src/lib/flight_tasks/tasks/Utility/Sticks.cpp @@ -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(); diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.hpp b/src/lib/flight_tasks/tasks/Utility/Sticks.hpp index 94e9334a44..dae247f97c 100644 --- a/src/lib/flight_tasks/tasks/Utility/Sticks.hpp +++ b/src/lib/flight_tasks/tasks/Utility/Sticks.hpp @@ -44,8 +44,8 @@ #include #include #include -#include #include +#include 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 &getPosition() { return _positions; }; const matrix::Vector &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 _positions; ///< unmodified manual stick inputs matrix::Vector _positions_expo; ///< modified manual sticks using expo function - int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; ///< old switch state - uORB::SubscriptionData _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) _param_mpc_hold_dz, (ParamFloat) _param_mpc_xy_man_expo, (ParamFloat) _param_mpc_z_man_expo, - (ParamFloat) _param_mpc_yaw_expo, - (ParamFloat) _param_com_rc_loss_t + (ParamFloat) _param_mpc_yaw_expo ) }; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp index babbfde815..661d8fbaba 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp @@ -35,7 +35,7 @@ #include #include -#include +#include 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_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_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) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 8bbb791cd1..bac4c2e45e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 4858b75eec..669cfc0680 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -68,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -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_pub{ORB_ID(vehicle_status_flags)}; uORB::Publication _status_pub{ORB_ID(vehicle_status)}; uORB::Publication _mission_pub{ORB_ID(mission)}; + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::PublicationData _home_pub{ORB_ID(home_position)}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 7c7a42eb81..a29f8c4ec0 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -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); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index aa5572ac2a..8118a6ad7c 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -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"); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2107518fa3..b840e97ccd 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -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); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index a7590ba557..472f97fa73 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -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 diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 632edd473a..46f331eef7 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -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( diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 5974a86962..dc97d17dbb 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -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 diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index a3e5ca2d62..f21982963a 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -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_pub{ORB_ID(landing_gear)}; uORB::Publication _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) _param_mc_rollrate_p, (ParamFloat) _param_mc_rollrate_i, diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 3407b1db27..e2a167c659 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -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 ×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 + 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 ×tamp_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. diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index ae37c5340c..d0734ffb81 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -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 #include #include -#include #include #include #include @@ -55,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -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 ×tamp_sample); + void UpdateManualSwitches(const hrt_abstime ×tamp_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_pub{ORB_ID(rc_channels)}; /**< raw r/c control topic */ - uORB::Publication _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; /**< manual control as actuator topic */ + uORB::Publication _rc_channels_pub{ORB_ID(rc_channels)}; + uORB::PublicationMulti _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; + uORB::Publication _manual_control_switches_pub{ORB_ID(manual_control_switches)}; + uORB::Publication _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; - uORB::PublicationMulti _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) _param_rc_chan_cnt ) - }; - - - } /* namespace RCUpdate */ diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 0a745f211d..6aee2b721e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 25259d54d2..adc873b34c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -66,7 +66,7 @@ #include #include #include -#include +#include #include #include #include @@ -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