Skip to content

Commit

Permalink
commander: move manual_control and switches out
Browse files Browse the repository at this point in the history 
This moves the remaining handling of the manual control stuff out
of commander. All communication between manual control now goes through
vehicle commands, and the landing gear topic.
  • Loading branch information
@julianoes
julianoes committed May 11, 2021
1 parent 24fb580 commit d37bbf1
Show file tree
Hide file tree
Showing 8 changed files with 61 additions and 390 deletions.
1 change: 0 additions & 1 deletion src/modules/commander/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,6 @@ px4_add_module(
level_calibration.cpp
lm_fit.cpp
mag_calibration.cpp
ManualControl.cpp
rc_calibration.cpp
state_machine_helper.cpp
worker_thread.cpp
Expand Down
150 changes: 43 additions & 107 deletions src/modules/commander/Commander.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -508,14 +508,16 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_

if (run_preflight_checks) {
if (_vehicle_control_mode.flag_control_manual_enabled) {
if (_vehicle_control_mode.flag_control_climb_rate_enabled && _manual_control.isThrottleAboveCenter()) {
if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
!_status.rc_signal_lost && _is_throttle_above_center) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center");
tune_negative(true);
return TRANSITION_DENIED;

}

if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_manual_control.isThrottleLow()) {
if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
!_status.rc_signal_lost && !_is_throttle_low) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle");
tune_negative(true);
return TRANSITION_DENIED;
Expand Down Expand Up @@ -2188,18 +2190,16 @@ 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_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON;

if (!in_loiter_mode || manual_loiter_switch_on) {
if (!in_loiter_mode) {
_geofence_loiter_on = false;
}


// 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_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON;

if (!in_rtl_mode || manual_return_switch_on) {
if (!in_rtl_mode) {
_geofence_rtl_on = false;
}

Expand Down Expand Up @@ -2239,130 +2239,63 @@ Commander::run()
}
}

// Manual control input handling
_manual_control.setRCAllowed(!_status_flags.rc_input_blocked);
manual_control_setpoint_s manual_control_setpoint;

if (_manual_control.update()) {
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
if (manual_control_setpoint.valid) {
if (!_status_flags.rc_signal_found_once) {
_status_flags.rc_signal_found_once = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true,
_status_flags.rc_calibration_valid, _status);
_status_changed = true;

/* handle the case where RC signal was regained */
if (!_status_flags.rc_signal_found_once) {
_status_flags.rc_signal_found_once = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _status_flags.rc_calibration_valid, _status);
_status_changed = true;
} else {
if (_status.rc_signal_lost) {
if (_last_valid_manual_control_setpoint > 0) {
mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs",
hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6);
}

} else {
if (_status.rc_signal_lost) {
if (_rc_signal_lost_timestamp > 0) {
mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs",
hrt_elapsed_time(&_rc_signal_lost_timestamp) * 1e-6);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true,
_status_flags.rc_calibration_valid, _status);
_status_changed = true;
}
}

set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _status_flags.rc_calibration_valid, _status);
_status_changed = true;
_status.rc_signal_lost = false;
_is_throttle_above_center = manual_control_setpoint.z > 0.6f;
_is_throttle_low = manual_control_setpoint.z < 0.1f;
_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;

} else {
if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) {
if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) {
mavlink_log_critical(&_mavlink_log_pub, "Manual control lost");
_status.rc_signal_lost = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true,
false, _status);
_status_changed = true;
}
}
}

_status.rc_signal_lost = false;

// Abort autonomous mode and switch to position mode if sticks are moved significantly
// but only if actually in air.
if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& !in_low_battery_failsafe && !_geofence_warning_action_on
&& _armed.armed
&& _manual_control.wantsOverride(_vehicle_control_mode)) {
&& !_status_flags.rc_input_blocked
&& manual_control_setpoint.user_override) {
if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
_internal_state) == TRANSITION_CHANGED) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
_status_changed = true;
}
}

if (!_armed.armed && _manual_control.isMavlink() && (_internal_state.main_state_changes == 0)) {
// If there's never been a mode change try to use position control as initial state.
_internal_state.desired_main_state = commander_state_s::MAIN_STATE_POSCTL;
}

if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) {

// handle landing gear switch if configured and in a manual mode
if ((_vehicle_control_mode.flag_control_manual_enabled) &&
(_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)) {
// 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
tune_positive(_armed.armed);
_status_changed = true;
}
}

/* check throttle kill switch */
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";

if (_land_detector.landed) {
mavlink_log_info(&_mavlink_log_pub, kill_switch_string);

} else {
mavlink_log_critical(&_mavlink_log_pub, kill_switch_string);
}

_status_changed = true;
_armed.manual_lockdown = true;
}

} 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;
_armed.manual_lockdown = false;
}
}

/* no else case: do not change lockdown flag in unconfigured case */
}

if (!_manual_control.isRCAvailable()) {
// set RC lost
if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) {
// ignore RC lost during calibration
if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) {
mavlink_log_critical(&_mavlink_log_pub, "Manual control lost");
_status.rc_signal_lost = true;
_rc_signal_lost_timestamp = _manual_control.getLastRcTimestamp();
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _status);
_status_changed = true;
}
}
}

// data link checks which update the status
data_link_check();
Expand Down Expand Up @@ -2550,6 +2483,9 @@ Commander::run()
if (desired_ret == TRANSITION_CHANGED) {
// Reset it for next time.
_internal_state.desired_main_state = commander_state_s::MAIN_STATE_MAX;

tune_positive(_armed.armed);
_status_changed = true;
}

} else if (_armed.armed) {
Expand Down
15 changes: 6 additions & 9 deletions src/modules/commander/Commander.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@
/* Helper classes */
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
#include "failure_detector/FailureDetector.hpp"
#include "ManualControl.hpp"
#include "state_machine_helper.h"
#include "worker_thread.hpp"

Expand All @@ -51,7 +50,6 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/test_motor.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_control_mode.h>
Expand All @@ -71,7 +69,7 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/offboard_control_mode.h>
Expand Down Expand Up @@ -343,10 +341,10 @@ class Commander : public ModuleBase<Commander>, public ModuleParams

unsigned int _leds_counter{0};

manual_control_switches_s _manual_control_switches{};
manual_control_switches_s _last_manual_control_switches{};
ManualControl _manual_control{this};
hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost
hrt_abstime _last_valid_manual_control_setpoint{0};

bool _is_throttle_above_center{false};
bool _is_throttle_low{false};

hrt_abstime _boot_timestamp{0};
hrt_abstime _last_disarmed_timestamp{0};
Expand Down Expand Up @@ -390,11 +388,11 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};

uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

Expand Down Expand Up @@ -422,7 +420,6 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
uORB::Publication<vehicle_status_s> _status_pub{ORB_ID(vehicle_status)};
uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};

uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};

Expand Down
Loading

0 comments on commit d37bbf1

Please sign in to comment.