dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: remove compile time dependencies on non-commander parameters 

- this allows builds with mavlink fully disabled
 - move commander MAN_ARM_GESTURE, RC_MAP_ARM_SW, MC_AIRMODE checks to manual_control
This commit is contained in:
Daniel Agar 2022-05-11 10:14:23 -04:00 committed by GitHub
parent 8d36ba6727
commit c772e5230f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 182 additions and 140 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -721,7 +721,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
return TRANSITION_DENIED;
}
if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)
if ((_geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL)
&& !_status_flags.home_position_valid) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t");
events::send(events::ID("commander_arm_denied_geofence_rtl"),
@ -803,6 +803,12 @@ Commander::Commander() :
{
_vehicle_land_detected.landed = true;
_status.system_id = 1;
_status.component_id = 1;
_status.system_type = 0;
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
// XXX for now just set sensors as initialized
_status_flags.system_sensors_initialized = true;
@ -825,6 +831,13 @@ Commander::Commander() :
_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
_param_mav_comp_id = param_find("MAV_COMP_ID");
_param_mav_sys_id = param_find("MAV_SYS_ID");
_param_mav_type = param_find("MAV_TYPE");
_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
updateParameters();
}
Commander::~Commander()
@ -2035,15 +2048,84 @@ Commander::updateHomePositionYaw(float yaw)
}
}
void Commander::updateParameters()
{
// update parameters from storage
updateParams();
get_circuit_breaker_params();
int32_t value_int32 = 0;
// MAV_SYS_ID => vehicle_status.system_id
if ((_param_mav_sys_id != PARAM_INVALID) && (param_get(_param_mav_sys_id, &value_int32) == PX4_OK)) {
_status.system_id = value_int32;
}
// MAV_COMP_ID => vehicle_status.component_id
if ((_param_mav_comp_id != PARAM_INVALID) && (param_get(_param_mav_comp_id, &value_int32) == PX4_OK)) {
_status.component_id = value_int32;
}
// MAV_TYPE -> vehicle_status.system_type
if ((_param_mav_type != PARAM_INVALID) && (param_get(_param_mav_type, &value_int32) == PX4_OK)) {
_status.system_type = value_int32;
}
_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
_arm_requirements.arm_authorization = _param_arm_auth_required.get();
_arm_requirements.esc_check = _param_escs_checks_required.get();
_arm_requirements.global_position = !_param_arm_without_gps.get();
_arm_requirements.mission = _param_arm_mission_required.get();
_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status) && _vtol_status.vtol_in_rw_mode);
const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status) && !_vtol_status.vtol_in_rw_mode);
const bool is_ground = is_ground_rover(_status);
/* disable manual override for all systems that rely on electronic stabilization */
if (is_rotary) {
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
} else if (is_fixed) {
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
} else if (is_ground) {
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
}
_status.is_vtol = is_vtol(_status);
_status.is_vtol_tailsitter = is_vtol_tailsitter(_status);
// CP_DIST: collision preventation enabled if CP_DIST > 0
if (is_rotary_wing(_status) || is_vtol(_status)) {
if (_param_cp_dist == PARAM_INVALID) {
_param_cp_dist = param_find("CP_DIST");
}
float cp_dist = 0;
if (_param_cp_dist != PARAM_INVALID && (param_get(_param_cp_dist, &cp_dist) == PX4_OK)) {
_collision_prevention_enabled = cp_dist > 0.f;
}
}
// _mode_switch_mapped = (RC_MAP_FLTMODE > 0)
if (_param_rc_map_fltmode != PARAM_INVALID && (param_get(_param_rc_map_fltmode, &value_int32) == PX4_OK)) {
_mode_switch_mapped = (value_int32 > 0);
}
}
void
Commander::run()
{
bool sensor_fail_tune_played = false;
const param_t param_airmode = param_find("MC_AIRMODE");
const param_t param_man_arm_gesture = param_find("MAN_ARM_GESTURE");
const param_t param_rc_map_arm_sw = param_find("RC_MAP_ARM_SW");
/* initialize */
led_init();
buzzer_init();
@ -2070,29 +2152,6 @@ Commander::run()
#endif // BOARD_HAS_POWER_CONTROL
get_circuit_breaker_params();
bool param_init_forced = true;
/* update vehicle status to find out vehicle type (required for preflight checks) */
_status.system_type = _param_mav_type.get();
if (is_rotary_wing(_status) || is_vtol(_status)) {
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
} else if (is_fixed_wing(_status)) {
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
} else if (is_ground_rover(_status)) {
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
} else {
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
}
_status.is_vtol = is_vtol(_status);
_status.is_vtol_tailsitter = is_vtol_tailsitter(_status);
_boot_timestamp = hrt_absolute_time();
// initially set to failed
@ -2100,7 +2159,6 @@ Commander::run()
_last_gpos_fail_time_us = _boot_timestamp;
_last_lvel_fail_time_us = _boot_timestamp;
_status.system_id = _param_mav_sys_id.get();
arm_auth_init(&_mavlink_log_pub, &_status.system_id);
// run preflight immediately to find all relevant parameters, but don't report
@ -2114,94 +2172,17 @@ Commander::run()
/* update parameters */
const bool params_updated = _parameter_update_sub.updated();
if (params_updated || param_init_forced) {
if (params_updated) {
// clear update
parameter_update_s update;
_parameter_update_sub.copy(&update);
// update parameters from storage
updateParams();
/* update parameters */
if (!_armed.armed) {
_status.system_type = _param_mav_type.get();
const bool is_rotary = is_rotary_wing(_status) || (is_vtol(_status) && _vtol_status.vtol_in_rw_mode);
const bool is_fixed = is_fixed_wing(_status) || (is_vtol(_status) && !_vtol_status.vtol_in_rw_mode);
const bool is_ground = is_ground_rover(_status);
/* disable manual override for all systems that rely on electronic stabilization */
if (is_rotary) {
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
} else if (is_fixed) {
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
} else if (is_ground) {
_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
}
/* set vehicle_status.is_vtol flag */
_status.is_vtol = is_vtol(_status);
_status.is_vtol_tailsitter = is_vtol_tailsitter(_status);
/* check and update system / component ID */
_status.system_id = _param_mav_sys_id.get();
_status.component_id = _param_mav_comp_id.get();
get_circuit_breaker_params();
updateParameters();
_status_changed = true;
}
_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
_arm_requirements.arm_authorization = _param_arm_auth_required.get();
_arm_requirements.esc_check = _param_escs_checks_required.get();
_arm_requirements.global_position = !_param_arm_without_gps.get();
_arm_requirements.mission = _param_arm_mission_required.get();
_arm_requirements.geofence = _param_geofence_action.get() > geofence_result_s::GF_ACTION_NONE;
_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
// disable arm gesture if an arm switch is configured
if (param_man_arm_gesture != PARAM_INVALID && param_rc_map_arm_sw != PARAM_INVALID) {
int32_t man_arm_gesture = 0, rc_map_arm_sw = 0;
param_get(param_man_arm_gesture, &man_arm_gesture);
param_get(param_rc_map_arm_sw, &rc_map_arm_sw);
if (rc_map_arm_sw > 0 && man_arm_gesture == 1) {
man_arm_gesture = 0; // disable arm gesture
param_set(param_man_arm_gesture, &man_arm_gesture);
mavlink_log_critical(&_mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t")
/* EVENT
* @description <param>MAN_ARM_GESTURE</param> is now set to disable arm/disarm stick gesture.
*/
events::send(events::ID("rc_update_arm_stick_gesture_disabled_with_switch"), {events::Log::Info, events::LogInternal::Disabled},
"Arm stick gesture disabled if arm switch in use");
}
}
// check for unsafe Airmode settings: yaw airmode requires disabling the stick arm gesture
if (param_airmode != PARAM_INVALID && param_man_arm_gesture != PARAM_INVALID) {
int32_t airmode = 0, man_arm_gesture = 0;
param_get(param_airmode, &airmode);
param_get(param_man_arm_gesture, &man_arm_gesture);
if (airmode == 2 && man_arm_gesture == 1) {
airmode = 1; // change to roll/pitch airmode
param_set(param_airmode, &airmode);
mavlink_log_critical(&_mavlink_log_pub, "Yaw Airmode requires disabling the stick arm gesture\t")
/* EVENT
* @description <param>MC_AIRMODE</param> is now set to roll/pitch airmode.
*/
events::send(events::ID("commander_airmode_requires_no_arm_gesture"), {events::Log::Error, events::LogInternal::Disabled},
"Yaw Airmode requires disabling the stick arm gesture");
}
}
param_init_forced = false;
}
/* Update OA parameter */
@ -2525,16 +2506,16 @@ Commander::run()
}
/* start geofence result check */
_geofence_result_sub.update(&_geofence_result);
_status.geofence_violated = _geofence_result.geofence_violated;
if (_geofence_result_sub.update(&_geofence_result)) {
_arm_requirements.geofence = (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE);
_status.geofence_violated = _geofence_result.geofence_violated;
}
const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0;
// Geofence actions
const bool geofence_action_enabled = _geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE;
if (_armed.armed
&& geofence_action_enabled
&& (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)
&& !in_low_battery_failsafe_delay) {
// check for geofence violation transition
@ -2681,10 +2662,9 @@ Commander::run()
}
}
const bool mode_switch_mapped = (_param_rc_map_fltmode.get() > 0) || (_param_rc_map_mode_sw.get() > 0);
const bool is_mavlink = manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC;
if (!_armed.armed && (is_mavlink || !mode_switch_mapped) && (_internal_state.main_state_changes == 0)) {
if (!_armed.armed && (is_mavlink || !_mode_switch_mapped) && (_internal_state.main_state_changes == 0)) {
// if there's never been a mode change force position control as initial state
_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
_internal_state.main_state_changes++;
@ -3170,7 +3150,7 @@ Commander::run()
_was_armed = _armed.armed;
arm_auth_update(now, params_updated || param_init_forced);
arm_auth_update(now, params_updated);
px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed);
@ -3822,18 +3802,17 @@ void Commander::avoidance_check()
}
}
const bool cp_enabled = _param_cp_dist.get() > 0.f;
const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms;
const bool cp_healthy = _status_flags.avoidance_system_valid || distance_sensor_valid;
const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || cp_enabled;
const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || _collision_prevention_enabled;
const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled;
const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled
&& _vehicle_control_mode.flag_control_position_enabled);
const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode && cp_enabled));
const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode
&& _collision_prevention_enabled));
const bool sensor_oa_healthy = ((auto_mode && _status_flags.avoidance_system_valid) || (pos_ctl_mode && cp_healthy));
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled,

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

@ -184,6 +184,8 @@ private:
void checkWindSpeedThresholds();
void updateParameters();
DEFINE_PARAMETERS(
(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
@ -222,9 +224,6 @@ private:
(ParamFloat<px4::params::COM_WIND_WARN>) _param_com_wind_warn,
(ParamInt<px4::params::RC_MAP_FLTMODE>) _param_rc_map_fltmode,
(ParamInt<px4::params::RC_MAP_MODE_SW>) _param_rc_map_mode_sw,
// Quadchute
(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
@ -262,22 +261,17 @@ private:
(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr,
(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
// Geofence
(ParamInt<px4::params::GF_ACTION>) _param_geofence_action,
// Mavlink
(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
(ParamFloat<px4::params::CP_DIST>) _param_cp_dist,
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max
)
// optional parameters
param_t _param_cp_dist{PARAM_INVALID};
param_t _param_mav_comp_id{PARAM_INVALID};
param_t _param_mav_sys_id{PARAM_INVALID};
param_t _param_mav_type{PARAM_INVALID};
param_t _param_rc_map_fltmode{PARAM_INVALID};
enum class PrearmedMode {
DISABLED = 0,
SAFETY_BUTTON = 1,
@ -320,6 +314,8 @@ private:
bool _geofence_warning_action_on{false};
bool _geofence_violated_prev{false};
bool _collision_prevention_enabled{false};
bool _rtl_time_actions_done{false};
FailureDetector _failure_detector;
@ -357,6 +353,7 @@ private:
Hysteresis _offboard_available{false};
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
bool _mode_switch_mapped{false};
bool _last_local_altitude_valid{false};
bool _last_local_position_valid{false};

66
src/modules/manual_control/ManualControl.cpp
View File

@ -32,6 +32,9 @@
****************************************************************************/
#include "ManualControl.hpp"
#include <px4_platform_common/events.h>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/topics/commander_state.h>
#include <uORB/topics/vehicle_command.h>
@ -64,6 +67,16 @@ void ManualControl::Run()
perf_begin(_loop_perf);
perf_count(_loop_interval_perf);
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.copy(&vehicle_status)) {
_system_id = vehicle_status.system_id;
_rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
_vtol = vehicle_status.is_vtol;
}
}
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
// clear update
@ -78,6 +91,53 @@ void ManualControl::Run()
_selector.setRcInMode(_param_com_rc_in_mode.get());
_selector.setTimeout(_param_com_rc_loss_t.get() * 1_s);
// MAN_ARM_GESTURE
if (_param_man_arm_gesture.get() == 1) {
// RC_MAP_ARM_SW & MAN_ARM_GESTURE: disable arm gesture if an arm switch is configured
param_t param_rc_map_arm_sw = param_find("RC_MAP_ARM_SW");
if (param_rc_map_arm_sw != PARAM_INVALID) {
int32_t rc_map_arm_sw = 0;
param_get(param_rc_map_arm_sw, &rc_map_arm_sw);
if (rc_map_arm_sw > 0) {
_param_man_arm_gesture.set(0); // disable arm gesture
_param_man_arm_gesture.commit();
orb_advert_t mavlink_log_pub = nullptr;
mavlink_log_critical(&mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t")
/* EVENT
* @description <param>MAN_ARM_GESTURE</param> is now set to disable arm/disarm stick gesture.
*/
events::send(events::ID("rc_update_arm_stick_gesture_disabled_with_switch"), {events::Log::Info, events::LogInternal::Disabled},
"Arm stick gesture disabled if arm switch in use");
}
}
// MC_AIRMODE & MAN_ARM_GESTURE: check for unsafe Airmode settings: yaw airmode requires disabling the stick arm gesture
if ((_param_man_arm_gesture.get() == 1) && (_rotary_wing || _vtol)) {
param_t param_mc_airmode = param_find("MC_AIRMODE");
if (param_mc_airmode != PARAM_INVALID) {
int32_t airmode = 0;
param_get(param_mc_airmode, &airmode);
if (airmode == 2) {
airmode = 1; // change to roll/pitch airmode
param_set(param_mc_airmode, &airmode);
orb_advert_t mavlink_log_pub = nullptr;
mavlink_log_critical(&mavlink_log_pub, "Yaw Airmode requires disabling the stick arm gesture\t")
/* EVENT
* @description <param>MC_AIRMODE</param> is now set to roll/pitch airmode.
*/
events::send(events::ID("commander_airmode_requires_no_arm_gesture"), {events::Log::Error, events::LogInternal::Disabled},
"Yaw Airmode requires disabling the stick arm gesture");
}
}
}
}
}
const hrt_abstime now = hrt_absolute_time();
@ -361,7 +421,7 @@ void ManualControl::send_camera_mode_command(CameraMode camera_mode)
vehicle_command_s command{};
command.command = vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE;
command.param2 = static_cast<float>(camera_mode);
command.target_system = _param_mav_sys_id.get();
command.target_system = _system_id;
command.target_component = 100; // any camera
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
@ -375,7 +435,7 @@ void ManualControl::send_photo_command()
command.command = vehicle_command_s::VEHICLE_CMD_IMAGE_START_CAPTURE;
command.param3 = 1; // one picture
command.param4 = _image_sequence++;
command.target_system = _param_mav_sys_id.get();
command.target_system = _system_id;
command.target_component = 100; // any camera
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};
@ -395,7 +455,7 @@ void ManualControl::send_video_command()
command.command = vehicle_command_s::VEHICLE_CMD_VIDEO_START_CAPTURE;
}
command.target_system = _param_mav_sys_id.get();
command.target_system = _system_id;
command.target_component = 100; // any camera
uORB::Publication<vehicle_command_s> command_pub{ORB_ID(vehicle_command)};

10
src/modules/manual_control/ManualControl.hpp
View File

@ -46,6 +46,7 @@
#include <uORB/topics/manual_control_switches.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/SubscriptionCallback.hpp>
@ -97,6 +98,8 @@ private:
uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
int _previous_manual_control_input_instance{-1};
uORB::SubscriptionCallbackWorkItem _manual_control_setpoint_subs[MAX_MANUAL_INPUT_COUNT] {
{this, ORB_ID(manual_control_input), 0},
@ -137,10 +140,13 @@ private:
(ParamInt<px4::params::COM_FLTMODE3>) _param_fltmode_3,
(ParamInt<px4::params::COM_FLTMODE4>) _param_fltmode_4,
(ParamInt<px4::params::COM_FLTMODE5>) _param_fltmode_5,
(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6,
(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id
(ParamInt<px4::params::COM_FLTMODE6>) _param_fltmode_6
)
unsigned _image_sequence {0};
bool _video_recording {false}; // TODO: hopefully there is a command soon to toggle without keeping state
uint8_t _system_id{1};
bool _rotary_wing{false};
bool _vtol{false};
};