From 93ae29a3e9d47a37cba2fa7c9b89f085f258a3ee Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Tue, 18 Sep 2012 23:05:08 +0900 Subject: [PATCH] ArduCopter: added throttle curve (although disabled by default) for all multicopters --- ArduCopter/ArduCopter.pde | 1 + libraries/AP_Common/AP_Curve.h | 144 ++++++++++++++++++++++++ libraries/AP_Motors/AP_Motors.cpp | 53 ++++++++- libraries/AP_Motors/AP_Motors.h | 16 ++- libraries/AP_Motors/AP_MotorsMatrix.cpp | 12 ++ libraries/AP_Motors/AP_MotorsTri.cpp | 10 ++ 6 files changed, 233 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_Common/AP_Curve.h diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 20c5523f39..980946d455 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -73,6 +73,7 @@ #include #include // ArduPilot Mega Magnetometer Library #include // ArduPilot Mega Vector/Matrix math Library +#include // Curve used to linearlise throttle pwm to thrust #include // ArduPilot Mega Inertial Sensor (accel & gyro) Library #include // ArduPilot Mega IMU Library #include // Parent header of Timer diff --git a/libraries/AP_Common/AP_Curve.h b/libraries/AP_Common/AP_Curve.h new file mode 100644 index 0000000000..52c0aa10c9 --- /dev/null +++ b/libraries/AP_Common/AP_Curve.h @@ -0,0 +1,144 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_Curve.h +/// @brief used to transforms a pwm value to account for the non-linear pwm->thrust values of normal ESC+motors + +#ifndef AP_CURVE +#define AP_CURVE + +#include +#include +#include // ArduPilot Mega Vector/Matrix math Library + +/// @class AP_Curve +template +class AP_Curve { +public: + + // Constructor + AP_Curve(); + + // clear - removes all points from the curve + virtual void clear(); + + // add_point - adds a point to the curve. returns TRUE if successfully added + virtual bool add_point( T x, T y ); + + // get_y - returns the point on the curve at the given pwm_value (i.e. the new modified pwm_value) + virtual T get_y( T x ); + + // displays the contents of the curve (for debugging) + virtual void dump_curve(); + +protected: + uint8_t _num_points; // number of points in the cruve + T _x[SIZE]; // x values of each point on the curve + T _y[SIZE]; // y values of each point on the curve + float _slope[SIZE]; // slope between any two points. i.e. slope[0] is the slope between points 0 and 1 + bool _constrained; // if true, first and last points added will constrain the y values returned by get_y function +}; + +// Typedef for convenience +typedef AP_Curve AP_CurveInt16_Size3; +typedef AP_Curve AP_CurveInt16_Size4; +typedef AP_Curve AP_CurveInt16_Size5; + +typedef AP_Curve AP_CurveUInt16_Size3; +typedef AP_Curve AP_CurveUInt16_Size4; +typedef AP_Curve AP_CurveUInt16_Size5; + +// Constructor +template +AP_Curve::AP_Curve() : + _num_points(0) +{ + // clear the curve + clear(); +}; + +// clear the curve +template +void AP_Curve::clear() { + // clear the curve + for( uint8_t i=0; i +bool AP_Curve::add_point( T x, T y ) +{ + if( _num_points < SIZE ) { + _x[_num_points] = x; + _y[_num_points] = y; + + // increment the number of points + _num_points++; + + // if we have at least two points calculate the slope + if( _num_points > 1 ) { + _slope[_num_points-2] = (float)(_y[_num_points-1] - _y[_num_points-2]) / (float)(_x[_num_points-1] - _x[_num_points-2]); + _slope[_num_points-1] = _slope[_num_points-2]; // the final slope is for interpolation beyond the end of the curve + } + return true; + }else{ + // we do not have room for the new point + return false; + } +} + +// get_y - returns the y value on the curve for a given x value +template +T AP_Curve::get_y( T x ) +{ + uint8_t i; + T result; + + // deal with case where ther is no curve + if( _num_points == 0 ) { + return x; + } + + // when x value is lower than the first point's x value, return minimum y value + if( x <= _x[0] ) { + return _y[0]; + } + + // when x value is higher than the last point's x value, return maximum y value + if( x >= _x[_num_points-1] ) { + return _y[_num_points-1]; + } + + // deal with the normal case + for( i=0; i<_num_points-1; i++ ) { + if( x >= _x[i] && x <= _x[i+1] ) { + result = _y[i] + (x - _x[i]) * _slope[i]; + return result; + } + } + + // we should never get here + return x; +} + +// displays the contents of the curve (for debugging) +template +void AP_Curve::dump_curve() +{ + Serial.println("Curve:"); + for( uint8_t i = 0; i<_num_points; i++ ){ + Serial.print("x:"); + Serial.print(_x[i]); + Serial.print("\ty:"); + Serial.print(_y[i]); + Serial.print("\tslope:"); + Serial.print(_slope[i],4); + Serial.println(); + } +} + +#endif // AP_CURVE \ No newline at end of file diff --git a/libraries/AP_Motors/AP_Motors.cpp b/libraries/AP_Motors/AP_Motors.cpp index 842d4c61ab..646fedc6e0 100644 --- a/libraries/AP_Motors/AP_Motors.cpp +++ b/libraries/AP_Motors/AP_Motors.cpp @@ -13,6 +13,9 @@ // parameters for the motor class const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = { AP_GROUPINFO("TB_RATIO", 0, AP_Motors, top_bottom_ratio, AP_MOTORS_TOP_BOTTOM_RATIO), // not used + AP_GROUPINFO("TCRV_ENABLE", 1, AP_Motors, _throttle_curve_enabled, THROTTLE_CURVE_ENABLED), + AP_GROUPINFO("TCRV_MIDPCT", 2, AP_Motors, _throttle_curve_mid, THROTTLE_CURVE_MID_THRUST), + AP_GROUPINFO("TCRV_MAXPCT", 3, AP_Motors, _throttle_curve_max, THROTTLE_CURVE_MAX_THRUST), AP_GROUPEND }; @@ -47,11 +50,59 @@ AP_Motors::AP_Motors( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_ } }; +// 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(); +}; + // throttle_pass_through - passes throttle through to motors - dangerous but used for initialising ESCs -void AP_Motors::throttle_pass_through() { +void AP_Motors::throttle_pass_through() +{ if( armed() ) { for( int16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++ ) { _rc->OutputCh(_motor_to_channel_map[i], _rc_throttle->radio_in); } } } + +// setup_throttle_curve - used to linearlise 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.0); + int16_t max_thrust_pwm = min_pwm + (float)(max_pwm - min_pwm) * ((float)_throttle_curve_max/100.0); + bool retval = true; + + Serial.printf_P(PSTR("AP_Motors: setup_throttle_curve")); + Serial.printf_P(PSTR("mid:%d\tmax:%d\n"),(int)mid_thrust_pwm,(int)max_thrust_pwm); + + // 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 corrrectly + if( !retval ) { + _throttle_curve_enabled = false; + Serial.printf_P(PSTR("AP_Motors: failed to create throttle curve")); + } + + return retval; +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h index c1e0c3b9aa..e110241119 100644 --- a/libraries/AP_Motors/AP_Motors.h +++ b/libraries/AP_Motors/AP_Motors.h @@ -9,6 +9,7 @@ #include #include #include // ArduPilot Mega Vector/Matrix math Library +#include // Curve used to linearlise throttle pwm to thrust #include // RC Channel Library #include // ArduPilot Mega RC Library @@ -45,6 +46,10 @@ // top-bottom ratio (for Y6) #define AP_MOTORS_TOP_BOTTOM_RATIO 1.0 +#define THROTTLE_CURVE_ENABLED 0 // throttle curve disabled by default +#define THROTTLE_CURVE_MID_THRUST 52 // throttle which produces 1/2 the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100) +#define THROTTLE_CURVE_MAX_THRUST 93 // throttle which produces the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100) + /// @class AP_Motors class AP_Motors { public: @@ -53,8 +58,7 @@ public: AP_Motors( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); // init - virtual void Init() { - }; + virtual void Init(); // set mapping from motor number to RC channel virtual void set_motor_to_channel_map( uint8_t mot_1, uint8_t mot_2, uint8_t mot_3, uint8_t mot_4, uint8_t mot_5, uint8_t mot_6, uint8_t mot_7, uint8_t mot_8 ) { @@ -128,6 +132,10 @@ public: // throttle_pass_through - passes throttle through to motors - dangerous but required for initialising ESCs virtual void throttle_pass_through(); + // setup_throttle_curve - used to linearlise thrust output by motors + // returns true if curve is created successfully + virtual bool setup_throttle_curve(); + // 1 if motor is enabled, 0 otherwise AP_Int8 motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; @@ -157,6 +165,10 @@ protected: uint8_t _frame_orientation; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2 int16_t _min_throttle; // the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle) int16_t _max_throttle; // the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle) + 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 }; #endif // AP_MOTORS \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index a28bbd0f32..213afc9c3f 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -15,6 +15,9 @@ void AP_MotorsMatrix::Init() { int8_t i; + // call parent Init function to set-up throttle curve + AP_Motors::Init(); + // setup the motors setup_motors(); @@ -140,6 +143,15 @@ void AP_MotorsMatrix::output_armed() } } + // adjust for throttle curve + if( _throttle_curve_enabled ) { + for( i=0; i