forked from Archive/PX4-Autopilot
commander: Add parameter to control postal fallback after loss of navigation accuracy
Also remove else if branch that cannot be accessed.
This commit is contained in:
parent
b85c8fa135
commit
00a42abc69
|
@ -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)
|
||||
{
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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,
|
||||
|
|
Loading…
Reference in New Issue