diff --git a/libraries/APM_OBC/APM_OBC.cpp b/libraries/APM_OBC/APM_OBC.cpp index 56fb78ca70..6c0ad0aa83 100644 --- a/libraries/APM_OBC/APM_OBC.cpp +++ b/libraries/APM_OBC/APM_OBC.cpp @@ -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); diff --git a/libraries/APM_OBC/APM_OBC.h b/libraries/APM_OBC/APM_OBC.h index 1054ba9956..b08ee8ce2e 100644 --- a/libraries/APM_OBC/APM_OBC.h +++ b/libraries/APM_OBC/APM_OBC.h @@ -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