From a2fdcfaf3fc92224bfabc455e5655b2513db4245 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 20 Jan 2016 10:32:36 +0900 Subject: [PATCH] AP_MotorsTri: remove output_armed_not_stabilizing --- libraries/AP_Motors/AP_MotorsTri.cpp | 51 ---------------------------- libraries/AP_Motors/AP_MotorsTri.h | 1 - 2 files changed, 52 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 729ab0e5e6..3cab6084b0 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -136,58 +136,7 @@ uint16_t AP_MotorsTri::get_motor_mask() (1U << AP_MOTORS_CH_TRI_YAW); } -void AP_MotorsTri::output_armed_not_stabilizing() -{ - int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 - int16_t out_min = _throttle_radio_min + _min_throttle; - int16_t out_max = _throttle_radio_max; - int16_t motor_out[AP_MOTORS_MOT_4+1]; - - // 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(); - - motor_out[AP_MOTORS_MOT_1] = throttle_radio_output; - motor_out[AP_MOTORS_MOT_2] = throttle_radio_output; - motor_out[AP_MOTORS_MOT_4] = throttle_radio_output; - - if(throttle_radio_output >= out_min) { - // adjust for thrust curve and voltage scaling - motor_out[AP_MOTORS_MOT_1] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_1], out_min, out_max); - motor_out[AP_MOTORS_MOT_2] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_2], out_min, out_max); - motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, out_max); - } - - hal.rcout->cork(); - - // send output to each motor - rc_write(AP_MOTORS_MOT_1, motor_out[AP_MOTORS_MOT_1]); - rc_write(AP_MOTORS_MOT_2, motor_out[AP_MOTORS_MOT_2]); - rc_write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]); - - // send centering signal to yaw servo - rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim); - - hal.rcout->push(); -} - // sends commands to the motors -// TODO pull code that is common to output_armed_not_stabilizing into helper functions void AP_MotorsTri::output_armed_stabilizing() { int16_t roll_pwm; // roll pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400 diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index efc90017f5..58ce6e4583 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -50,7 +50,6 @@ public: protected: // output - sends commands to the motors void output_armed_stabilizing(); - void output_armed_not_stabilizing(); void output_disarmed(); // calc_yaw_radio_output - calculate final radio output for yaw channel