fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 04:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: remove compile time dependencies on non-commander parameters

Browse Source 
- 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
4 changed files with 182 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+99 -120
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,