diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 33714d9176..e7a7a43d54 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1378,6 +1378,9 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_max_imu_acc_diff = param_find("COM_ARM_IMU_ACC"); param_t _param_max_imu_gyr_diff = param_find("COM_ARM_IMU_GYR"); + /* failsafe response to loss of navigation accuracy */ + param_t _param_posctl_nav_loss_act = param_find("COM_POSCTL_NAVL"); + // These are too verbose, but we will retain them a little longer // until we are sure we really don't need them. @@ -1757,6 +1760,7 @@ int commander_thread_main(int argc, char *argv[]) float offboard_loss_timeout = 0.0f; int32_t offboard_loss_act = 0; int32_t offboard_loss_rc_act = 0; + int32_t posctl_nav_loss_act = 0; int32_t geofence_action = 0; @@ -1906,6 +1910,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_max_imu_acc_diff, &max_imu_acc_diff); param_get(_param_max_imu_gyr_diff, &max_imu_gyr_diff); + /* failsafe response to loss of navigation accuracy */ + param_get(_param_posctl_nav_loss_act, &posctl_nav_loss_act); + param_init_forced = false; } @@ -3065,7 +3072,8 @@ int commander_thread_main(int argc, char *argv[]) land_detector.landed, (link_loss_actions_t)rc_loss_act, offboard_loss_act, - offboard_loss_rc_act); + offboard_loss_rc_act, + posctl_nav_loss_act); if (status.failsafe != failsafe_old) { diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index bba7453385..dc11eba845 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -601,3 +601,16 @@ PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 0); * @boolean */ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0); + +/** + * Position control navigation loss response. + * + * This sets the flight mode that will be used if navigation accuracy is no longer adequte for position control. + * Navigation accuracy checks can be disabled using the CBRK_VELPOSERR parameter, but doing so will remove protection for all flight modes. + * + * @value 0 Assume use of remote control after fallback. Switch to ALTCTL if a height estimate is available, else switch to MANUAL. + * @value 1 Assume no use of remote control after fallback. Switch to DESCEND if a height estimate is available, else switch to TERMINATION. + * + * @group Mission + */ +PARAM_DEFINE_INT32(COM_POSCTL_NAVL, 0); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d5bcdbe897..1a9c05f30f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -549,7 +549,8 @@ bool set_nav_state(struct vehicle_status_s *status, bool landed, const link_loss_actions_t rc_loss_act, const int offb_loss_act, - const int offb_loss_rc_act) + const int offb_loss_rc_act, + const int posctl_nav_loss_act) { navigation_state_t nav_state_old = status->nav_state; @@ -622,9 +623,7 @@ bool set_nav_state(struct vehicle_status_s *status, * this enables POSCTL using e.g. flow. * For fixedwing, a global position is needed. */ - } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, true, !status->is_rotary_wing)) { - // nothing to do - everything done in check_invalid_pos_nav_state - } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, true, status->is_rotary_wing)) { + } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1), !status->is_rotary_wing)) { // nothing to do - everything done in check_invalid_pos_nav_state } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index d62abcf5b6..a113c07286 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -142,7 +142,8 @@ bool set_nav_state(struct vehicle_status_s *status, bool landed, const link_loss_actions_t rc_loss_act, const int offb_loss_act, - const int offb_loss_rc_act); + const int offb_loss_rc_act, + const int posctl_nav_loss_act); void set_rc_loss_nav_state(struct vehicle_status_s *status, struct actuator_armed_s *armed,