Browse Source

add parameter for arm/disarm "hysteresis"

sbg
Mark Whitehorn 9 years ago committed by Lorenz Meier
parent
commit
84761a9b8e
  1. 16
      src/modules/commander/commander.cpp
  2. 11
      src/modules/commander/commander_params.c

16
src/modules/commander/commander.cpp

@ -138,8 +138,6 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage @@ -138,8 +138,6 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage
#define MAVLINK_OPEN_INTERVAL 50000
#define STICK_ON_OFF_LIMIT 0.9f
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define POSITION_TIMEOUT (1 * 1000 * 1000) /**< consider the local or global position estimate invalid after 1000ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
@ -1183,6 +1181,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -1183,6 +1181,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_autostart_id = param_find("SYS_AUTOSTART");
param_t _param_autosave_params = param_find("COM_AUTOS_PAR");
param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
param_t _param_rc_arm_hyst = param_find("COM_RC_ARM_HYST");
param_t _param_eph = param_find("COM_HOME_H_T");
param_t _param_epv = param_find("COM_HOME_V_T");
param_t _param_geofence_action = param_find("GF_ACTION");
@ -1515,13 +1514,18 @@ int commander_thread_main(int argc, char *argv[]) @@ -1515,13 +1514,18 @@ int commander_thread_main(int argc, char *argv[])
status_flags.condition_system_sensors_initialized = false;
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} else {
// sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet
// sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet
status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true,
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
!status_flags.circuit_breaker_engaged_gpsfailure_check, false);
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
}
// user adjustable duration required to assert arm/disarm via throttle/rudder stick
int32_t rc_arm_hyst = 100;
param_get(_param_rc_arm_hyst, &rc_arm_hyst);
rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
commander_boot_timestamp = hrt_absolute_time();
transition_result_t arming_ret;
@ -1616,6 +1620,8 @@ int commander_thread_main(int argc, char *argv[]) @@ -1616,6 +1620,8 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_rc_loss_timeout, &rc_loss_timeout);
param_get(_param_rc_in_off, &rc_in_off);
status.rc_input_mode = rc_in_off;
param_get(_param_rc_arm_hyst, &rc_arm_hyst);
rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
param_get(_param_datalink_regain_timeout, &datalink_regain_timeout);
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
@ -2344,7 +2350,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -2344,7 +2350,7 @@ int commander_thread_main(int argc, char *argv[])
land_detector.landed) &&
sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (stick_off_counter > rc_arm_hyst) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
@ -2374,7 +2380,7 @@ int commander_thread_main(int argc, char *argv[]) @@ -2374,7 +2380,7 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF ) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (stick_on_counter > rc_arm_hyst) {
/* we check outside of the transition function here because the requirement
* for being in manual mode only applies to manual arming actions.

11
src/modules/commander/commander_params.c

@ -235,6 +235,17 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1); @@ -235,6 +235,17 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1);
*/
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
/**
* RC input arm/disarm command duration
*
* The default value of 1000 requires the stick to be held in the arm or disarm position for 1 second.
*
* @group Commander
* @min 100
* @max 1500
*/
PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000);
/**
* Time-out for auto disarm after landing
*

Loading…
Cancel
Save