From 50a098c35987386b54a6ec54cf988a80c9f2089d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 28 Sep 2019 23:07:57 +0930 Subject: [PATCH] Copter: Standby functions --- ArduCopter/Copter.cpp | 1 + ArduCopter/Copter.h | 5 +++++ ArduCopter/RC_Channel.cpp | 17 +++++++++++++++++ ArduCopter/crash_check.cpp | 16 ++++++++++++++++ ArduCopter/land_detector.cpp | 3 +++ ArduCopter/standby.cpp | 22 ++++++++++++++++++++++ 6 files changed, 64 insertions(+) create mode 100644 ArduCopter/standby.cpp diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 638cc8b159..d79874eaa4 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -144,6 +144,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(ekf_check, 10, 75), SCHED_TASK(gpsglitch_check, 10, 50), SCHED_TASK(landinggear_update, 10, 75), + SCHED_TASK(standby_update, 100, 75), SCHED_TASK(lost_vehicle_check, 10, 50), SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180), SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 341d9521d2..dcffffc20d 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -582,6 +582,8 @@ private: float takeoff_alt_cm; } gndeffect_state; + bool standby_active; + // set when we are upgrading parameters from 3.4 bool upgrading_frame_params; @@ -744,6 +746,9 @@ private: // landing_gear.cpp void landinggear_update(); + // standby.cpp + void standby_update(); + // Log.cpp void Log_Write_Control_Tuning(); void Log_Write_Performance(); diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 49b2421a5b..d0a2a17f61 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -77,6 +77,7 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_ case AUX_FUNC::PRECISION_LOITER: case AUX_FUNC::INVERTED: case AUX_FUNC::WINCH_ENABLE: + case AUX_FUNC::STAND_BY: case AUX_FUNC::SURFACE_TRACKING: do_aux_function(ch_option, ch_flag); break; @@ -542,6 +543,22 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw #endif break; + case AUX_FUNC::STAND_BY: { + switch (ch_flag) { + case HIGH: + copter.standby_active = true; + copter.Log_Write_Event(DATA_STAND_BY_ENABLE); + gcs().send_text(MAV_SEVERITY_INFO, "Stand By Enabled"); + break; + default: + copter.standby_active = false; + copter.Log_Write_Event(DATA_STAND_BY_DISABLE); + gcs().send_text(MAV_SEVERITY_INFO, "Stand By Disabled"); + break; + } + break; + } + case AUX_FUNC::SURFACE_TRACKING: switch (ch_flag) { case LOW: diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 96861d0242..99bd79a4a1 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -23,6 +23,12 @@ void Copter::crash_check() return; } + // exit immediately if in standby + if (standby_active) { + crash_counter = 0; + return; + } + // return immediately if we are not in an angle stabilize flight mode or we are flipping if (control_mode == Mode::Number::ACRO || control_mode == Mode::Number::FLIP) { crash_counter = 0; @@ -73,6 +79,11 @@ void Copter::thrust_loss_check() return; } + // exit immediately if in standby + if (standby_active) { + return; + } + // check for desired angle over 15 degrees // todo: add thrust angle to AC_AttitudeControl const Vector3f angle_target = attitude_control->get_att_target_euler_cd(); @@ -142,6 +153,11 @@ void Copter::parachute_check() return; } + // exit immediately if in standby + if (standby_active) { + return; + } + // call update to give parachute a chance to move servo or relay back to off position parachute.update(); diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index a3dbe0f282..da4d21308e 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -54,6 +54,9 @@ void Copter::update_land_detector() #endif set_land_complete(false); } + } else if (standby_active) { + // land detector will not run in standby mode + land_detector_count = 0; } else { #if FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopter/standby.cpp b/ArduCopter/standby.cpp new file mode 100644 index 0000000000..3439017a4c --- /dev/null +++ b/ArduCopter/standby.cpp @@ -0,0 +1,22 @@ +#include "Copter.h" + +// Run standby functions at approximately 100 Hz to limit maximum variable build up +// +// When standby is active: +// all I terms are continually reset +// heading error is reset to zero +// position errors are reset to zero +// crash_check is disabled +// thrust_loss_check is disabled +// parachute_check is disabled +// and landing detection is disabled. +void Copter::standby_update() +{ + if (!standby_active) { + return; + } + + attitude_control->reset_rate_controller_I_terms(); + attitude_control->set_yaw_target_to_current_heading(); + pos_control->standby_xyz_reset(); +}