aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM_OBC: added AFS_RC_FAIL_MS, AFS_MAX_GPS_LOSS and AFS_MAX_COM_LOSS

Browse Source
This commit is contained in:
Andrew Tridgell 2014-08-17 18:06:42 +10:00
parent 9fff67ec89
commit d8fe4f366d
2 changed files with 73 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
libraries/APM_OBC/APM_OBC.cpp
View File

@ -97,7 +97,25 @@ const AP_Param::GroupInfo APM_OBC::var_info[] PROGMEM = {
// @DisplayName: Enable Advanced Failsafe
// @Description: This enables the advanced failsafe system. If this is set to zero (disable) then all the other AFS options have no effect
// @User: Advanced
AP_GROUPINFO("ENABLE", 11, APM_OBC, _enable, 0),
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),
// @Param: MAX_GPS_LOSS
// @DisplayName: Maximum number of GPS loss events
// @Description: Maximum number of GPS loss events before the aircraft stops returning to mission on GPS recovery. Use zero to allow for any number of GPS loss events.
// @User: Advanced
AP_GROUPINFO("MAX_GPS_LOSS", 13, APM_OBC, _max_gps_loss, 0),
// @Param: MAX_COM_LOSS
// @DisplayName: Maximum number of comms loss events
// @Description: Maximum number of comms loss events before the aircraft stops returning to mission on comms recovery. Use zero to allow for any number of comms loss events.
// @User: Advanced
AP_GROUPINFO("MAX_COM_LOSS", 14, APM_OBC, _max_comms_loss, 0),
AP_GROUPEND
};
@ -105,7 +123,7 @@ const AP_Param::GroupInfo APM_OBC::var_info[] PROGMEM = {
// check for Failsafe conditions. This is called at 10Hz by the main
// ArduPlane code
void
APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geofence_breached)
APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms)
{
if (!_enable) {
return;
@ -117,6 +135,17 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
_terminate.set(1);
}
}
// check for RC failure in manual mode
if (_state != STATE_PREFLIGHT &&
(mode == OBC_MANUAL || mode == OBC_FBW) &&
_rc_fail_time != 0 &&
(hal.scheduler->millis() - last_valid_rc_ms) > (unsigned)_rc_fail_time.get()) {
if (!_terminate) {
GCS_MAVLINK::send_statustext_all(PSTR("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
@ -149,6 +178,11 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
_saved_wp = mission.get_current_nav_cmd().index;
mission.set_current_cmd(_wp_comms_hold);
}
// if two events happen within 30s we consider it to be part of the same event
if (now - _last_comms_loss_ms > 30*1000UL) {
_comms_loss_count++;
_last_comms_loss_ms = now;
}
break;
}
if (!gps_lock_ok) {
@ -158,6 +192,11 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
_saved_wp = mission.get_current_nav_cmd().index;
mission.set_current_cmd(_wp_gps_loss);
}
// if two events happen within 30s we consider it to be part of the same event
if (now - _last_gps_loss_ms > 30*1000UL) {
_gps_loss_count++;
_last_gps_loss_ms = now;
}
break;
}
break;
@ -173,7 +212,10 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
} else if (gcs_link_ok) {
_state = STATE_AUTO;
GCS_MAVLINK::send_statustext_all(PSTR("GCS OK"));
if (_saved_wp != 0) {
// we only return to the mission if we have not exceeded AFS_MAX_COM_LOSS
if (_saved_wp != 0 &&
(_max_comms_loss <= 0 ||
_comms_loss_count <= _max_comms_loss)) {
mission.set_current_cmd(_saved_wp);
_saved_wp = 0;
}
@ -191,7 +233,10 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
} else if (gps_lock_ok) {
GCS_MAVLINK::send_statustext_all(PSTR("GPS OK"));
_state = STATE_AUTO;
if (_saved_wp != 0) {
// we only return to the mission if we have not exceeded AFS_MAX_GPS_LOSS
if (_saved_wp != 0 &&
(_max_gps_loss <= 0 ||
_gps_loss_count <= _max_gps_loss)) {
mission.set_current_cmd(_saved_wp);
_saved_wp = 0;
}
@ -263,7 +308,7 @@ void APM_OBC::setup_failsafe(void)
const RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
// setup primary channel output values
hal.rcout->set_failsafe_pwm(1U<<(rcmap.roll()-1), ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.roll()-1), ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.pitch()-1), ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.yaw()-1), ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX));
hal.rcout->set_failsafe_pwm(1U<<(rcmap.throttle()-1), ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN));
@ -271,7 +316,7 @@ void APM_OBC::setup_failsafe(void)
// and all aux channels
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);
@ -307,7 +352,7 @@ void APM_OBC::check_crash_plane(void)
RC_Channel *ch_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
RC_Channel *ch_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
ch_roll->radio_out = ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX);
ch_roll->radio_out = ch_roll->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN);
ch_pitch->radio_out = ch_pitch->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX);
ch_yaw->radio_out = ch_yaw->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MAX);
ch_throttle->radio_out = ch_throttle->get_limit_pwm(RC_Channel::RC_CHANNEL_LIMIT_MIN);
@ -315,7 +360,7 @@ void APM_OBC::check_crash_plane(void)
// and all aux channels
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap_auto, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_flap, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_aileron, RC_Channel::RC_CHANNEL_LIMIT_MIN);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_rudder, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator, RC_Channel::RC_CHANNEL_LIMIT_MAX);
RC_Channel_aux::set_servo_limit(RC_Channel_aux::k_elevator_with_input, RC_Channel::RC_CHANNEL_LIMIT_MAX);

23
libraries/APM_OBC/APM_OBC.h
View File

@ -52,7 +52,9 @@ public:
mission(_mission),
baro(_baro),
gps(_gps),
rcmap(_rcmap)
rcmap(_rcmap),
_gps_loss_count(0),
_comms_loss_count(0)
{
AP_Param::setup_object_defaults(this, var_info);
@ -62,7 +64,7 @@ public:
_saved_wp = 0;
}
void check(enum control_mode control_mode, uint32_t last_heartbeat_ms, bool geofence_breached);
void check(enum control_mode control_mode, uint32_t last_heartbeat_ms, bool geofence_breached, uint32_t last_valid_rc_ms);
// called in servo output code to set servos to crash position if needed
void check_crash_plane(void);
@ -93,11 +95,26 @@ private:
AP_Float _qnh_pressure;
AP_Int32 _amsl_limit;
AP_Int32 _amsl_margin_gps;
AP_Int16 _rc_fail_time;
AP_Int8 _max_gps_loss;
AP_Int8 _max_comms_loss;
bool _heartbeat_pin_value;
// saved waypoint for resuming mission
uint8_t _saved_wp;
// number of times we've lost GPS
uint8_t _gps_loss_count;
// number of times we've lost data link
uint8_t _comms_loss_count;
// last comms loss time
uint32_t _last_comms_loss_ms;
// last GPS loss time
uint32_t _last_gps_loss_ms;
// have the failsafe values been setup?
bool _failsafe_setup:1;
@ -109,6 +126,6 @@ private:
};
// map from ArduPlane control_mode to APM_OBC::control_mode
#define OBC_MODE(control_mode) ((control_mode==AUTO?APM_OBC::OBC_AUTO:control_mode==MANUAL?APM_OBC::OBC_MANUAL:APM_OBC::OBC_FBW))
#define OBC_MODE(control_mode) (auto_throttle_mode?APM_OBC::OBC_AUTO:(control_mode==MANUAL?APM_OBC::OBC_MANUAL:APM_OBC::OBC_FBW))
#endif // APM_OBC_H