Browse Source

APM_OBC: changed param RC_FAIL_MS to RC_FAIL_TIME in float seconds

mission-4.1.18
Tom Pittenger 9 years ago
parent
commit
bcc2838a37
  1. 33
      libraries/APM_OBC/APM_OBC.cpp
  2. 2
      libraries/APM_OBC/APM_OBC.h

33
libraries/APM_OBC/APM_OBC.cpp

@ -99,11 +99,7 @@ const AP_Param::GroupInfo APM_OBC::var_info[] = { @@ -99,11 +99,7 @@ const AP_Param::GroupInfo APM_OBC::var_info[] = {
// @User: Advanced
AP_GROUPINFO("ENABLE", 11, APM_OBC, _enable, 0),
// @Param: RC_FAIL_MS
// @DisplayName: RC failure time
// @Description: This is the time in milliseconds in manual mode that failsafe termination will activate if RC input is lost. For the OBC rules this should be 1500. Use 0 to disable.
// @User: Advanced
AP_GROUPINFO("RC_FAIL_MS", 12, APM_OBC, _rc_fail_time, 0),
// *NOTE* index 12 of AP_Int16 RC_FAIL_MS was depreciated. Replaced by RC_FAIL_TIME.
// @Param: MAX_GPS_LOSS
// @DisplayName: Maximum number of GPS loss events
@ -141,6 +137,13 @@ const AP_Param::GroupInfo APM_OBC::var_info[] = { @@ -141,6 +137,13 @@ const AP_Param::GroupInfo APM_OBC::var_info[] = {
// @User: Advanced
AP_GROUPINFO("DUAL_LOSS", 18, APM_OBC, _enable_dual_loss, 1),
// @Param: RC_FAIL_TIME
// @DisplayName: RC failure time
// @Description: This is the time in seconds in manual mode that failsafe termination will activate if RC input is lost. For the OBC rules this should be (1.5). Use 0 to disable.
// @User: Advanced
// @Units: seconds
AP_GROUPINFO("RC_FAIL_TIME", 19, APM_OBC, _rc_fail_time_seconds, 0),
AP_GROUPEND
};
@ -163,14 +166,14 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof @@ -163,14 +166,14 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
}
// check if RC termination is enabled
// check for RC failure in manual mode or RC failure when AFS_RC_MANUAL is 0
// check for RC failure in manual mode or RC failure when AFS_RC_MANUAL is 0
if (_state != STATE_PREFLIGHT && !_terminate && _enable_RC_fs &&
(mode == OBC_MANUAL || mode == OBC_FBW || !_rc_term_manual_only) &&
_rc_fail_time != 0 &&
(AP_HAL::millis() - last_valid_rc_ms) > (unsigned)_rc_fail_time.get()) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "RC failure terminate");
_terminate.set(1);
}
(mode == OBC_MANUAL || mode == OBC_FBW || !_rc_term_manual_only) &&
_rc_fail_time_seconds > 0 &&
(AP_HAL::millis() - last_valid_rc_ms) > (_rc_fail_time_seconds * 1000.0f)) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "RC failure terminate");
_terminate.set(1);
}
// tell the failsafe board if we are in manual control
// mode. This tells it to pass through controls from the
@ -254,9 +257,9 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof @@ -254,9 +257,9 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
// losing GCS link when GPS lock lost
// leads to termination if AFS_DUAL_LOSS is 1
if (!_terminate && _enable_dual_loss) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Dual loss TERMINATE");
_terminate.set(1);
}
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Dual loss TERMINATE");
_terminate.set(1);
}
} else if (gps_lock_ok) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS OK");
_state = STATE_AUTO;

2
libraries/APM_OBC/APM_OBC.h

@ -99,7 +99,7 @@ private: @@ -99,7 +99,7 @@ private:
AP_Float _qnh_pressure;
AP_Int32 _amsl_limit;
AP_Int32 _amsl_margin_gps;
AP_Int16 _rc_fail_time;
AP_Float _rc_fail_time_seconds;
AP_Int8 _max_gps_loss;
AP_Int8 _max_comms_loss;
AP_Int8 _enable_geofence_fs;

Loading…
Cancel
Save