Browse Source

Commander: cleanup COM_POSCTL_NAVL parameter

- move to px4::params
- use enum
sbg
Julien Lecoeur 6 years ago committed by Julian Oes
parent
commit
4c9288d993
  1. 9
      src/modules/commander/Commander.cpp
  2. 1
      src/modules/commander/Commander.hpp
  3. 4
      src/modules/commander/commander_params.c
  4. 5
      src/modules/commander/state_machine_helper.cpp
  5. 7
      src/modules/commander/state_machine_helper.h

9
src/modules/commander/Commander.cpp

@ -1243,9 +1243,6 @@ Commander::run() @@ -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() @@ -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() @@ -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() @@ -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;

1
src/modules/commander/Commander.hpp

@ -116,6 +116,7 @@ private: @@ -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,

4
src/modules/commander/commander_params.c

@ -649,8 +649,8 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0); @@ -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
*/

5
src/modules/commander/state_machine_helper.cpp

@ -393,7 +393,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, 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_ @@ -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

7
src/modules/commander/state_machine_helper.h

@ -68,6 +68,11 @@ enum class link_loss_actions_t { @@ -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_ @@ -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…
Cancel
Save