From 2eaa4a844510b0e759be604d8e5e9ac3e1de71d8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 21 Feb 2015 17:33:13 +0900 Subject: [PATCH] Motors: remove old throttle curve --- libraries/AP_Motors/AP_Motors_Class.cpp | 70 +------------------------ libraries/AP_Motors/AP_Motors_Class.h | 11 +--- 2 files changed, 2 insertions(+), 79 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 613a0a1b18..28d4f1b2c2 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -36,32 +36,8 @@ extern const AP_HAL::HAL& hal; // parameters for the motor class const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = { // 0 was used by TB_RATIO + // 1,2,3 were used by throttle curve - // @Param: TCRV_ENABLE - // @DisplayName: Thrust Curve Enable - // @Description: Controls whether a curve is used to linearize the thrust produced by the motors - // @User: Advanced - // @Values: 0:Disabled,1:Enable - AP_GROUPINFO("TCRV_ENABLE", 1, AP_Motors, _throttle_curve_enabled, THROTTLE_CURVE_ENABLED), - - // @Param: TCRV_MIDPCT - // @DisplayName: Thrust Curve mid-point percentage - // @Description: Set the pwm position that produces half the maximum thrust of the motors - // @User: Advanced - // @Units: percent - // @Range: 20 80 - // @Increment: 1 - AP_GROUPINFO("TCRV_MIDPCT", 2, AP_Motors, _throttle_curve_mid, THROTTLE_CURVE_MID_THRUST), - - // @Param: TCRV_MAXPCT - // @DisplayName: Thrust Curve max thrust percentage - // @Description: Set to the lowest pwm position that produces the maximum thrust of the motors. Most motors produce maximum thrust below the maximum pwm value that they accept. - // @User: Advanced - // @Units: percent - // @Range: 50 100 - // @Increment: 1 - AP_GROUPINFO("TCRV_MAXPCT", 3, AP_Motors, _throttle_curve_max, THROTTLE_CURVE_MAX_THRUST), - // @Param: SPIN_ARMED // @DisplayName: Motors always spin when armed // @Description: Controls whether motors always spin when armed (must be below THR_MIN) @@ -153,13 +129,6 @@ AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_t _flags.slow_start_low_end = true; }; -// init -void AP_Motors::Init() -{ - // set-up throttle curve - motors classes will decide whether to use it based on _throttle_curve_enabled parameter - setup_throttle_curve(); -}; - void AP_Motors::armed(bool arm) { _flags.armed = arm; @@ -210,43 +179,6 @@ void AP_Motors::output() } }; -// setup_throttle_curve - used to linearise thrust output by motors -// returns true if set up successfully -bool AP_Motors::setup_throttle_curve() -{ - int16_t min_pwm = _rc_throttle.radio_min; - int16_t max_pwm = _rc_throttle.radio_max; - int16_t mid_throttle_pwm = (max_pwm + min_pwm) / 2; - int16_t mid_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_mid/100.0f); - int16_t max_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_max/100.0f); - bool retval = true; - - // some basic checks that the curve is valid - if( mid_thrust_pwm >= (min_pwm+_min_throttle) && mid_thrust_pwm <= max_pwm && max_thrust_pwm >= mid_thrust_pwm && max_thrust_pwm <= max_pwm ) { - // clear curve - _throttle_curve.clear(); - - // curve initialisation - retval &= _throttle_curve.add_point(min_pwm, min_pwm); - retval &= _throttle_curve.add_point(min_pwm+_min_throttle, min_pwm+_min_throttle); - retval &= _throttle_curve.add_point(mid_throttle_pwm, mid_thrust_pwm); - retval &= _throttle_curve.add_point(max_pwm, max_thrust_pwm); - - // return success - return retval; - }else{ - retval = false; - } - - // disable throttle curve if not set-up correctly - if( !retval ) { - _throttle_curve_enabled = false; - hal.console->println_P(PSTR("AP_Motors: failed to create throttle curve")); - } - - return retval; -} - // slow_start - set to true to slew motors from current speed to maximum // Note: this must be set immediately before a step up in throttle void AP_Motors::slow_start(bool true_false) diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 6de0a93596..5e19c16d0f 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -7,7 +7,6 @@ #include #include // ArduPilot Mega Vector/Matrix math Library #include // Notify library -#include // Curve used to linearlise throttle pwm to thrust #include // RC Channel Library // offsets for motors in motor_out, _motor_filtered and _motor_to_channel_map arrays @@ -88,7 +87,7 @@ public: AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); // init - virtual void Init(); + virtual void Init() {} // set update rate to motors - a value in hertz virtual void set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; }; @@ -145,10 +144,6 @@ public: // set_current - set current to be used for output scaling virtual void set_current(float current){ _batt_current = current; } - // setup_throttle_curve - used to linearlise thrust output by motors - // returns true if curve is created successfully - bool setup_throttle_curve(); - // 1 if motor is enabled, 0 otherwise bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; @@ -195,10 +190,6 @@ protected: static const uint8_t _motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS] PROGMEM; // parameters - AP_CurveInt16_Size4 _throttle_curve; // curve used to linearize the pwm->thrust - AP_Int8 _throttle_curve_enabled; // enable throttle curve - AP_Int8 _throttle_curve_mid; // throttle which produces 1/2 the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range - AP_Int8 _throttle_curve_max; // throttle which produces the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range AP_Int16 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min AP_Int16 _yaw_headroom; // yaw control is given at least this pwm range