From 1f847132d71c992c4042229086045cf1a08d4c64 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 5 May 2017 07:33:30 +1000 Subject: [PATCH] AP_Motors: setup limits flags for tailsitters --- libraries/AP_Motors/AP_MotorsTailsitter.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.cpp b/libraries/AP_Motors/AP_MotorsTailsitter.cpp index c7bb121a2a..567db250d9 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.cpp +++ b/libraries/AP_Motors/AP_MotorsTailsitter.cpp @@ -55,16 +55,31 @@ void AP_MotorsTailsitter::output_to_motors() switch (_spool_mode) { case SHUT_DOWN: _throttle = 0; + // set limits flags + limit.roll_pitch = true; + limit.yaw = true; + limit.throttle_lower = true; + limit.throttle_upper = true; break; case SPIN_WHEN_ARMED: // sends output to motors when armed but not flying _throttle = constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min; + // set limits flags + limit.roll_pitch = true; + limit.yaw = true; + limit.throttle_lower = true; + limit.throttle_upper = true; break; case SPOOL_UP: case THROTTLE_UNLIMITED: case SPOOL_DOWN: throttle_left = constrain_float(_throttle + _rudder*0.5, 0, 1); throttle_right = constrain_float(_throttle - _rudder*0.5, 0, 1); + // initialize limits flags + limit.roll_pitch = false; + limit.yaw = false; + limit.throttle_lower = false; + limit.throttle_upper = false; break; } // outputs are setup here, and written to the HAL by the plane servos loop