From c99cac773b1793f738dd09c34e8ed630623fba3e Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Mon, 4 Jan 2016 22:12:25 -0800 Subject: [PATCH] Copter: add brake_timeout_to_loiter_ms --- ArduCopter/Copter.h | 5 +++++ ArduCopter/control_brake.cpp | 15 +++++++++++++++ 2 files changed, 20 insertions(+) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5bd92bd374..46c0c58e30 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -375,6 +375,10 @@ private: uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds) uint32_t loiter_time; // How long have we been loitering - The start time in millis + // Brake + uint32_t brake_timeout_start; + uint32_t brake_timeout_ms; + // Flip Vector3f flip_orig_attitude; // original copter attitude before flip @@ -761,6 +765,7 @@ private: void adsb_handle_vehicle_threats(void); bool brake_init(bool ignore_checks); void brake_run(); + void brake_timeout_to_loiter_ms(uint32_t timeout_ms); bool circle_init(bool ignore_checks); void circle_run(); bool drift_init(bool ignore_checks); diff --git a/ArduCopter/control_brake.cpp b/ArduCopter/control_brake.cpp index 39b3b3ce08..02ef385049 100644 --- a/ArduCopter/control_brake.cpp +++ b/ArduCopter/control_brake.cpp @@ -25,6 +25,8 @@ bool Copter::brake_init(bool ignore_checks) pos_control.set_alt_target(inertial_nav.get_altitude()); pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); + brake_timeout_ms = 0; + return true; }else{ return false; @@ -75,4 +77,17 @@ void Copter::brake_run() // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); pos_control.update_z_controller(); + + if (brake_timeout_ms != 0 && millis()-brake_timeout_start >= brake_timeout_ms) + { + if(!set_mode(LOITER)) { + set_mode(ALT_HOLD); + } + } +} + +void Copter::brake_timeout_to_loiter_ms(uint32_t timeout_ms) +{ + brake_timeout_start = millis(); + brake_timeout_ms = timeout_ms; }