diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cd750783c0..a153a1d3b4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -194,6 +194,7 @@ static float max_ekf_dang_bias = 3.5e-4f; /* pre-flight IMU consistency checks */ static float max_imu_acc_diff = 0.7f; static float max_imu_gyr_diff = 0.09f; +static float min_stick_change = 0.25f; static struct vehicle_status_s status = {}; static struct vehicle_roi_s _roi = {}; @@ -229,6 +230,7 @@ static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was static float avionics_power_rail_voltage; // voltage of the avionics power rail static bool can_arm_without_gps = false; +static bool position_lock_gained = false; /** @@ -1083,7 +1085,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { /* ok, home set, use it to take off */ if (TRANSITION_CHANGED == main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, main_state_prev, &status_flags, &internal_state)) { - mavlink_and_console_log_info(&mavlink_log_pub, "Taking off"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1298,6 +1302,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_autosave_params = param_find("COM_AUTOS_PAR"); param_t _param_rc_in_off = param_find("COM_RC_IN_MODE"); param_t _param_rc_arm_hyst = param_find("COM_RC_ARM_HYST"); + param_t _param_min_stick_change = param_find("COM_RC_STICK_OV"); param_t _param_eph = param_find("COM_HOME_H_T"); param_t _param_epv = param_find("COM_HOME_V_T"); param_t _param_geofence_action = param_find("GF_ACTION"); @@ -1773,6 +1778,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_rc_in_off, &rc_in_off); status.rc_input_mode = rc_in_off; param_get(_param_rc_arm_hyst, &rc_arm_hyst); + param_get(_param_min_stick_change, &min_stick_change); + // percentage (* 0.01) needs to be doubled because RC total interval is 2, not 1 + min_stick_change *= 0.02f; rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC; param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_ef_throttle_thres, &ef_throttle_thres); @@ -2498,15 +2506,34 @@ int commander_thread_main(int argc, char *argv[]) main_state_before_rtl == commander_state_s::MAIN_STATE_RATTITUDE || main_state_before_rtl == commander_state_s::MAIN_STATE_STAB)) { - // transition to previous state if sticks are increased - const float min_stick_change = 0.2f; + // transition to previous state if sticks are touched if ((_last_sp_man.timestamp != sp_man.timestamp) && ((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) || (fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) || (fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) || (fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) { - main_state_transition(&status, main_state_before_rtl, main_state_prev, &status_flags, &internal_state); + // revert to position control in any case + main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); + mavlink_log_critical(&mavlink_log_pub, "Aborted RTL, returned control to pilot"); + } + } + + // abort landing or auto or loiter if sticks are moved significantly + if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL) { + // transition to previous state if sticks are touched + if ((_last_sp_man.timestamp != sp_man.timestamp) && + ((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) || + (fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) || + (fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) || + (fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) { + + // revert to position control in any case + main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state); + mavlink_log_critical(&mavlink_log_pub, "Autopilot off, returned control to pilot"); } } @@ -2850,6 +2877,19 @@ int commander_thread_main(int argc, char *argv[]) } } + /* check if we are disarmed and there is a better mode to wait in */ + if (!armed.armed) { + + /* if there is no radio control but GPS lock the user might want to fly using + * just a tablet. Since the RC will force its mode switch setting on connecting + * we can as well just wait in a hold mode which enables tablet control. + */ + if (status.rc_signal_lost && (internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL) + && status_flags.condition_home_position_valid) { + (void)main_state_transition(&status, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state); + } + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -2924,12 +2964,17 @@ int commander_thread_main(int argc, char *argv[]) /* First time home position update - but only if disarmed */ if (!status_flags.condition_home_position_valid && !armed.armed) { commander_set_home_position(home_pub, _home, local_position, global_position, attitude); + position_lock_gained = true; } /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ else if (((!was_armed && armed.armed) || (was_landed && !land_detector.landed)) && (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { commander_set_home_position(home_pub, _home, local_position, global_position, attitude); + position_lock_gained = false; + + } else { + position_lock_gained = false; } was_armed = armed.armed; @@ -3267,7 +3312,7 @@ set_main_state_rc(struct vehicle_status_s *status_local) // feature, just in case offboard control goes crazy. /* manual setpoint has not updated, do not re-evaluate it */ - if (((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) || + if ((((_last_sp_man.timestamp != 0) && (_last_sp_man.timestamp == sp_man.timestamp)) || ((_last_sp_man.offboard_switch == sp_man.offboard_switch) && (_last_sp_man.return_switch == sp_man.return_switch) && (_last_sp_man.mode_switch == sp_man.mode_switch) && @@ -3275,7 +3320,8 @@ set_main_state_rc(struct vehicle_status_s *status_local) (_last_sp_man.rattitude_switch == sp_man.rattitude_switch) && (_last_sp_man.posctl_switch == sp_man.posctl_switch) && (_last_sp_man.loiter_switch == sp_man.loiter_switch) && - (_last_sp_man.mode_slot == sp_man.mode_slot))) { + (_last_sp_man.mode_slot == sp_man.mode_slot))) + && !position_lock_gained) { // update these fields for the geofence system diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 509e3a8000..2de6c6288f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -178,6 +178,21 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); */ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); +/** + * RC stick override threshold + * + * If an RC stick is moved more than by this amount the system will interpret this as + * override request by the pilot. + * + * @group Commander + # @unit % + * @min 5 + * @max 40 + * @decimal 0 + * @increment 0.05 + */ +PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 12.0f); + /** * Home set horizontal threshold *