From b965857229d10890936fb6873c5eed7a6ae85269 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 3 Dec 2015 15:50:29 +0900 Subject: [PATCH] AP_MotorsMatrix: remove output_armed_not_stabilizing --- libraries/AP_Motors/AP_MotorsMatrix.cpp | 53 ------------------------- libraries/AP_Motors/AP_MotorsMatrix.h | 1 - 2 files changed, 54 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index a9cd071dbb..9f259cab19 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -111,59 +111,6 @@ uint16_t AP_MotorsMatrix::get_motor_mask() return rc_map_mask(mask); } -void AP_MotorsMatrix::output_armed_not_stabilizing() -{ - uint8_t i; - int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 - int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent to the motors - int16_t out_min_pwm = _throttle_radio_min + _min_throttle; // minimum pwm value we can send to the motors - int16_t out_max_pwm = _throttle_radio_max; // maximum pwm value we can send to the motors - - // initialize limits flags - limit.roll_pitch = true; - limit.yaw = true; - limit.throttle_lower = false; - limit.throttle_upper = false; - - int16_t thr_in_min = rel_pwm_to_thr_range(_spin_when_armed_ramped); - if (_throttle_control_input <= thr_in_min) { - _throttle_control_input = thr_in_min; - limit.throttle_lower = true; - } - - if (_throttle_control_input >= _hover_out) { - _throttle_control_input = _hover_out; - limit.throttle_upper = true; - } - - throttle_radio_output = calc_throttle_radio_output(); - - // set output throttle - for (i=0; i= out_min_pwm) { - // apply thrust curve and voltage scaling - for (i=0; icork(); - for( i=0; ipush(); -} - // output_armed - sends commands to the motors // includes new scaling stability patch void AP_MotorsMatrix::output_armed_stabilizing() diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index 832a56e70f..c45dfe8344 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -66,7 +66,6 @@ public: protected: // output - sends commands to the motors void output_armed_stabilizing(); - void output_armed_not_stabilizing(); void output_disarmed(); // add_motor using raw roll, pitch, throttle and yaw factors