From ade6281923ac29ae28348273c7af6cc191ecbc4c Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 8 Jan 2019 17:05:40 -0800 Subject: [PATCH] Plane: use #if ADVANCED_FAILSAFE == ENABLED just like Rover and Copter --- ArduPlane/ArduPlane.cpp | 4 ++++ ArduPlane/GCS_Mavlink.cpp | 4 ++++ ArduPlane/Parameters.cpp | 2 ++ ArduPlane/Plane.h | 6 ++++++ ArduPlane/afs_plane.cpp | 2 ++ ArduPlane/afs_plane.h | 3 +++ ArduPlane/config.h | 9 +++++++++ ArduPlane/events.cpp | 4 ++++ ArduPlane/failsafe.cpp | 4 ++++ ArduPlane/quadplane.cpp | 6 ++++++ ArduPlane/servos.cpp | 4 ++++ 11 files changed, 48 insertions(+) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 8dd0168ad8..f1c046b069 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -48,7 +48,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(read_airspeed, 10, 100), SCHED_TASK(update_alt, 10, 200), SCHED_TASK(adjust_altitude_target, 10, 200), +#if ADVANCED_FAILSAFE == ENABLED SCHED_TASK(afs_fs_check, 10, 100), +#endif SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500), SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 500), SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150), @@ -244,11 +246,13 @@ void Plane::update_logging2(void) /* check for AFS failsafe check */ +#if ADVANCED_FAILSAFE == ENABLED void Plane::afs_fs_check(void) { // perform AFS failsafe checks afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms); } +#endif #if HAL_WITH_IO_MCU #include diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 52366b020a..8d333a1057 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1407,7 +1407,11 @@ void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, mavlink_ AP_AdvancedFailsafe *GCS_MAVLINK_Plane::get_advanced_failsafe() const { +#if ADVANCED_FAILSAFE == ENABLED return &plane.afs; +#else + return nullptr; +#endif } /* diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index d79c6ce5a2..5af7c81bf8 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1034,9 +1034,11 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(sitl, "SIM_", SITL::SITL), #endif +#if ADVANCED_FAILSAFE == ENABLED // @Group: AFS_ // @Path: ../libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp GOBJECT(afs, "AFS_", AP_AdvancedFailsafe), +#endif #if OPTFLOW == ENABLED // @Group: FLOW diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 2be5a6ba80..bed8d246fb 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -104,7 +104,9 @@ // Configuration #include "config.h" +#if ADVANCED_FAILSAFE == ENABLED #include "afs_plane.h" +#endif // Local modules #include "defines.h" @@ -671,7 +673,9 @@ private: AP_Avoidance_Plane avoidance_adsb{ahrs, adsb}; // Outback Challenge Failsafe Support +#if ADVANCED_FAILSAFE == ENABLED AP_AdvancedFailsafe_Plane afs {mission, gps}; +#endif /* meta data to support counting the number of circles in a loiter @@ -960,7 +964,9 @@ private: void update_GPS_10Hz(void); void update_compass(void); void update_alt(void); +#if ADVANCED_FAILSAFE == ENABLED void afs_fs_check(void); +#endif void compass_cal_update(); void update_optical_flow(void); void one_second_loop(void); diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 9e1378d5bb..9d48b8dc82 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -4,6 +4,7 @@ #include "Plane.h" +#if ADVANCED_FAILSAFE == ENABLED // Constructor AP_AdvancedFailsafe_Plane::AP_AdvancedFailsafe_Plane(AP_Mission &_mission, const AP_GPS &_gps) : AP_AdvancedFailsafe(_mission, _gps) @@ -91,3 +92,4 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void) } return AP_AdvancedFailsafe::AFS_STABILIZED; } +#endif // ADVANCED_FAILSAFE diff --git a/ArduPlane/afs_plane.h b/ArduPlane/afs_plane.h index fb917c3a7f..6920ddd5fa 100644 --- a/ArduPlane/afs_plane.h +++ b/ArduPlane/afs_plane.h @@ -18,6 +18,7 @@ advanced failsafe support for plane */ +#if ADVANCED_FAILSAFE == ENABLED #include /* @@ -39,3 +40,5 @@ protected: enum control_mode afs_mode(void); }; +#endif // ADVANCED_FAILSAFE + diff --git a/ArduPlane/config.h b/ArduPlane/config.h index e156e895cd..889e7dcff6 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -58,6 +58,15 @@ #define FRSKY_TELEM_ENABLED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// Advanced Failsafe support +// + +#ifndef ADVANCED_FAILSAFE + # define ADVANCED_FAILSAFE ENABLED +#endif + + ////////////////////////////////////////////////////////////////////////////// // Optical flow sensor support // diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 6ad6e49efe..1528900b33 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -169,9 +169,13 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) break; case Failsafe_Action_Terminate: +#if ADVANCED_FAILSAFE == ENABLED char battery_type_str[17]; snprintf(battery_type_str, 17, "%s battery", type_str); afs.gcs_terminate(true, battery_type_str); +#else + disarm_motors(); +#endif break; case Failsafe_Action_None: diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 6294f1c42b..302d640641 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -41,11 +41,13 @@ void Plane::failsafe_check(void) if (in_failsafe && tnow - last_timestamp > 20000) { last_timestamp = tnow; +#if ADVANCED_FAILSAFE == ENABLED if (in_calibration) { // tell the failsafe system that we are calibrating // sensors, so don't trigger failsafe afs.heartbeat(); } +#endif if (RC_Channels::get_valid_channel_count() < 5) { // we don't have any RC input to pass through @@ -75,10 +77,12 @@ void Plane::failsafe_check(void) // this is to allow the failsafe module to deliberately crash // the plane. Only used in extreme circumstances to meet the // OBC rules +#if ADVANCED_FAILSAFE == ENABLED if (afs.should_crash_vehicle()) { afs.terminate_vehicle(); return; } +#endif // setup secondary output channels that do have // corresponding input channels diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index ab02b516db..7491adf1fb 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1586,11 +1586,13 @@ void QuadPlane::update(void) ahrs_view->set_pitch_trim(_last_ahrs_trim_pitch); } +#if ADVANCED_FAILSAFE == ENABLED if (plane.afs.should_crash_vehicle()) { motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); motors->output(); return; } +#endif if (motor_test.running) { motor_test_output(); @@ -1771,7 +1773,11 @@ void QuadPlane::motors_output(bool run_rate_controller) attitude_control->rate_controller_run(); } +#if ADVANCED_FAILSAFE == ENABLED if (!hal.util->get_soft_armed() || plane.afs.should_crash_vehicle() || SRV_Channels::get_emergency_stop()) { +#else + if (!hal.util->get_soft_armed() || SRV_Channels::get_emergency_stop()) { +#endif motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); motors->output(); return; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index cf7c07b7e8..ddfa186619 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -609,10 +609,12 @@ void Plane::servos_twin_engine_mix(void) float rud_gain = float(plane.g2.rudd_dt_gain) / 100; rudder_dt = rud_gain * SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / float(SERVO_MAX); +#if ADVANCED_FAILSAFE == ENABLED if (afs.should_crash_vehicle()) { // when in AFS failsafe force rudder input for differential thrust to zero rudder_dt = 0; } +#endif float throttle_left, throttle_right; @@ -666,10 +668,12 @@ void Plane::set_servos(void) // this is to allow the failsafe module to deliberately crash // the plane. Only used in extreme circumstances to meet the // OBC rules +#if ADVANCED_FAILSAFE == ENABLED if (afs.should_crash_vehicle()) { afs.terminate_vehicle(); return; } +#endif // do any transition updates for quadplane quadplane.update();