commander: Add parameter to control postal fallback after loss of navigation accuracy

Also remove else if branch that cannot be accessed.

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)
 		{

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);

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)) {
+			} 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 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 {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;

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,