forked from Archive/PX4-Autopilot
Commander: cleanup COM_POSCTL_NAVL parameter
- move to px4::params - use enum
This commit is contained in:
parent
907b6ccf46
commit
4c9288d993
|
@ -1243,9 +1243,6 @@ Commander::run()
|
|||
param_t _param_airmode = param_find("MC_AIRMODE");
|
||||
param_t _param_rc_map_arm_switch = param_find("RC_MAP_ARM_SW");
|
||||
|
||||
/* failsafe response to loss of navigation accuracy */
|
||||
param_t _param_posctl_nav_loss_act = param_find("COM_POSCTL_NAVL");
|
||||
|
||||
status_flags.avoidance_system_required = _param_com_obs_avoid.get();
|
||||
|
||||
/* pthread for slow low prio thread */
|
||||
|
@ -1374,7 +1371,6 @@ Commander::run()
|
|||
param_get(_param_rc_arm_hyst, &rc_arm_hyst);
|
||||
rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
|
||||
|
||||
int32_t posctl_nav_loss_act = 0;
|
||||
int32_t geofence_action = 0;
|
||||
int32_t flight_uuid = 0;
|
||||
int32_t airmode = 0;
|
||||
|
@ -1508,9 +1504,6 @@ Commander::run()
|
|||
param_get(_param_fmode_5, &_flight_mode_slots[4]);
|
||||
param_get(_param_fmode_6, &_flight_mode_slots[5]);
|
||||
|
||||
/* failsafe response to loss of navigation accuracy */
|
||||
param_get(_param_posctl_nav_loss_act, &posctl_nav_loss_act);
|
||||
|
||||
param_get(_param_takeoff_finished_action, &takeoff_complete_act);
|
||||
|
||||
/* check for unsafe Airmode settings: yaw airmode requires the use of an arming switch */
|
||||
|
@ -2317,7 +2310,7 @@ Commander::run()
|
|||
(link_loss_actions_t)_param_nav_rcl_act.get(),
|
||||
_param_com_obl_act.get(),
|
||||
_param_com_obl_rc_act.get(),
|
||||
posctl_nav_loss_act);
|
||||
(position_nav_loss_actions_t)_param_com_posctl_navl.get());
|
||||
|
||||
if (status.failsafe != failsafe_old) {
|
||||
status_changed = true;
|
||||
|
|
|
@ -116,6 +116,7 @@ private:
|
|||
(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
|
||||
(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv,
|
||||
(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
|
||||
(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */
|
||||
|
||||
(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
|
||||
(ParamInt<px4::params::COM_POS_FS_PROB>) _param_com_pos_fs_prob,
|
||||
|
|
|
@ -649,8 +649,8 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);
|
|||
* This sets the flight mode that will be used if navigation accuracy is no longer adequate 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 Altitude mode if a height estimate is available, else switch to MANUAL.
|
||||
* @value 1 Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
|
||||
* @value 0 Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
|
||||
* @value 1 Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
|
||||
*
|
||||
* @group Commander
|
||||
*/
|
||||
|
|
|
@ -393,7 +393,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
|
|||
orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
|
||||
const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
|
||||
const link_loss_actions_t rc_loss_act, const int offb_loss_act, const int offb_loss_rc_act,
|
||||
const int posctl_nav_loss_act)
|
||||
const position_nav_loss_actions_t posctl_nav_loss_act)
|
||||
{
|
||||
navigation_state_t nav_state_old = status->nav_state;
|
||||
|
||||
|
@ -468,7 +468,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
|
|||
* For fixedwing, a global position is needed. */
|
||||
|
||||
} else if (is_armed
|
||||
&& check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1),
|
||||
&& check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags,
|
||||
!(posctl_nav_loss_act == position_nav_loss_actions_t::LAND_TERMINATE),
|
||||
status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
|
||||
// nothing to do - everything done in check_invalid_pos_nav_state
|
||||
|
||||
|
|
|
@ -68,6 +68,11 @@ enum class link_loss_actions_t {
|
|||
LOCKDOWN = 6, // Kill the motors, same result as kill switch
|
||||
};
|
||||
|
||||
enum class position_nav_loss_actions_t {
|
||||
ALTITUDE_MANUAL = 0, // Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
|
||||
LAND_TERMINATE = 1, // Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
|
||||
};
|
||||
|
||||
typedef enum {
|
||||
ARM_REQ_NONE = 0,
|
||||
ARM_REQ_MISSION_BIT = (1 << 0),
|
||||
|
@ -95,7 +100,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
|
|||
orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished,
|
||||
const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed,
|
||||
const link_loss_actions_t rc_loss_act, const int offb_loss_act, const int offb_loss_rc_act,
|
||||
const int posctl_nav_loss_act);
|
||||
const position_nav_loss_actions_t posctl_nav_loss_act);
|
||||
|
||||
/*
|
||||
* Checks the validty of position data aaainst the requirements of the current navigation
|
||||
|
|
Loading…
Reference in New Issue