mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: added throttle curve (although disabled by default) for all multicopters
This commit is contained in:
parent
3306ba95a7
commit
aaab3c08aa
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -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;
|
||||
}
|
|
@ -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
|
|
@ -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] ) {
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue