diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 3e34b4dd4b..96fa2705bd 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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 MAN_ARM_GESTURE 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 MC_AIRMODE 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, diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 983cc60ad2..6a7a1ca6d1 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -184,6 +184,8 @@ private: void checkWindSpeedThresholds(); + void updateParameters(); + DEFINE_PARAMETERS( (ParamInt) _param_nav_dll_act, @@ -222,9 +224,6 @@ private: (ParamFloat) _param_com_wind_warn, - (ParamInt) _param_rc_map_fltmode, - (ParamInt) _param_rc_map_mode_sw, - // Quadchute (ParamInt) _param_com_qc_act, @@ -262,22 +261,17 @@ private: (ParamInt) _param_cbrk_velposerr, (ParamInt) _param_cbrk_vtolarming, - // Geofence - (ParamInt) _param_geofence_action, - - // Mavlink - (ParamInt) _param_mav_comp_id, - (ParamInt) _param_mav_sys_id, - (ParamInt) _param_mav_type, - - (ParamFloat) _param_cp_dist, - - (ParamFloat) _param_bat_low_thr, - (ParamFloat) _param_bat_crit_thr, (ParamInt) _param_com_flt_time_max, (ParamFloat) _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}; diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index a73073a8c4..056f6cbe5e 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -32,6 +32,9 @@ ****************************************************************************/ #include "ManualControl.hpp" + +#include +#include #include #include @@ -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 MAN_ARM_GESTURE 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 MC_AIRMODE 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(camera_mode); - command.target_system = _param_mav_sys_id.get(); + command.target_system = _system_id; command.target_component = 100; // any camera uORB::Publication 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 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 command_pub{ORB_ID(vehicle_command)}; diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index c450ac9518..1ddf25c46e 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -97,6 +98,8 @@ private: uORB::Publication _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) _param_fltmode_3, (ParamInt) _param_fltmode_4, (ParamInt) _param_fltmode_5, - (ParamInt) _param_fltmode_6, - (ParamInt) _param_mav_sys_id + (ParamInt) _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}; };