ArduCopter: added throttle curve (although disabled by default) for all multicopters

ArduCopter/ArduCopter.pde

@@ -73,6 +73,7 @@
 #include <AP_Baro.h>
 #include <AP_Compass.h>     // ArduPilot Mega Magnetometer Library
 #include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Curve.h>       // Curve used to linearlise throttle pwm to thrust
 #include <AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
 #include <AP_IMU.h>         // ArduPilot Mega IMU Library
 #include <AP_PeriodicProcess.h>         // Parent header of Timer

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 <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+
+/// @class      AP_Curve
+template <class T, uint8_t SIZE>
+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<int16_t,3> AP_CurveInt16_Size3;
+typedef AP_Curve<int16_t,4> AP_CurveInt16_Size4;
+typedef AP_Curve<int16_t,5> AP_CurveInt16_Size5;
+
+typedef AP_Curve<uint16_t,3> AP_CurveUInt16_Size3;
+typedef AP_Curve<uint16_t,4> AP_CurveUInt16_Size4;
+typedef AP_Curve<uint16_t,5> AP_CurveUInt16_Size5;
+
+// Constructor
+template <class T, uint8_t SIZE>
+AP_Curve<T,SIZE>::AP_Curve() :
+	_num_points(0)
+{
+	// clear the curve
+	clear();
+};
+
+// clear the curve
+template <class T, uint8_t SIZE>
+void AP_Curve<T,SIZE>::clear() {
+	// clear the curve
+	for( uint8_t i=0; i<SIZE; i++ ) {
+		_x[i] = 0;
+		_y[i] = 0;
+		_slope[i] = 0.0;
+	}
+	_num_points = 0;
+}
+
+// add_point - adds a point to the curve
+template <class T, uint8_t SIZE>
+bool AP_Curve<T,SIZE>::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 <class T, uint8_t SIZE>
+T AP_Curve<T,SIZE>::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 <class T, uint8_t SIZE>
+void AP_Curve<T,SIZE>::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

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;
+}

libraries/AP_Motors/AP_Motors.h

@@ -9,6 +9,7 @@
 #include <FastSerial.h>
 #include <AP_Common.h>
 #include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Curve.h>       // Curve used to linearlise throttle pwm to thrust
 #include <RC_Channel.h>     // RC Channel Library
 #include <APM_RC.h>         // 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

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<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+            if( motor_enabled[i] ) {
+                motor_out[i] = _throttle_curve.get_y(motor_out[i]);
+            }
+        }
+    }
+
     // ensure motors are not below the minimum
     for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
         if( motor_enabled[i] ) {

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -13,6 +13,9 @@
 // init
 void AP_MotorsTri::Init()
 {
+    // call parent Init function to set-up throttle curve
+    AP_Motors::Init();
+
     // set update rate for the 3 motors (but not the servo on channel 7)
     set_update_rate(_speed_hz);
 }
@@ -99,6 +102,13 @@ void AP_MotorsTri::output_armed()
         motor_out[AP_MOTORS_MOT_4] = out_max;
     }
 
+    // adjust for throttle curve
+    if( _throttle_curve_enabled ) {
+        motor_out[AP_MOTORS_MOT_1] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_1]);
+        motor_out[AP_MOTORS_MOT_2] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_2]);
+        motor_out[AP_MOTORS_MOT_4] = _throttle_curve.get_y(motor_out[AP_MOTORS_MOT_4]);
+    }
+
     // ensure motors don't drop below a minimum value and stop
     motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1],    out_min);
     motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2],    out_min);