|
|
|
@ -721,7 +721,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
@@ -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() :
@@ -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() :
@@ -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)
@@ -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()
@@ -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()
@@ -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()
@@ -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()
@@ -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()
@@ -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()
@@ -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()
@@ -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, |
|
|
|
|