AP_MotorsMulticopter: multicopter features moved in from parent

Also rename from multirotor to multicopter

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -15,17 +15,17 @@
  */
 
 /*
- *       AP_Motors_Multirotor.cpp - ArduCopter multirotor motors library
+ *       AP_MotorsMulticopter.cpp - ArduCopter multicopter motors library
  *       Code by Randy Mackay and Robert Lefebvre. DIYDrones.com
  *
  */
 
-#include "AP_Motors_Multirotor.h"
+#include "AP_MotorsMulticopter.h"
 #include <AP_HAL.h>
 extern const AP_HAL::HAL& hal;
 
 // parameters for the motor class
-const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
+const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] PROGMEM = {
     // 0 was used by TB_RATIO
     // 1,2,3 were used by throttle curve
 
@@ -34,7 +34,7 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
     // @Description: Controls whether motors always spin when armed (must be below THR_MIN)
     // @Values: 0:Do Not Spin,70:VerySlow,100:Slow,130:Medium,150:Fast
     // @User: Standard
-    AP_GROUPINFO("SPIN_ARMED", 5, AP_Motors_Multirotor, _spin_when_armed, AP_MOTORS_SPIN_WHEN_ARMED),
+    AP_GROUPINFO("SPIN_ARMED", 5, AP_MotorsMulticopter, _spin_when_armed, AP_MOTORS_SPIN_WHEN_ARMED),
 
     // @Param: YAW_HEADROOM
     // @DisplayName: Matrix Yaw Min
@@ -42,7 +42,7 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
     // @Range: 0 500
     // @Units: pwm
     // @User: Advanced
-    AP_GROUPINFO("YAW_HEADROOM", 6, AP_Motors_Multirotor, _yaw_headroom, AP_MOTORS_YAW_HEADROOM_DEFAULT),
+    AP_GROUPINFO("YAW_HEADROOM", 6, AP_MotorsMulticopter, _yaw_headroom, AP_MOTORS_YAW_HEADROOM_DEFAULT),
 
     // 7 was THR_LOW_CMP
 
@@ -51,14 +51,14 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
     // @Description: Motor thrust curve exponent (from 0 for linear to 1.0 for second order curve)
     // @Range: 0.25 0.8
     // @User: Advanced
-    AP_GROUPINFO("THST_EXPO", 8, AP_Motors_Multirotor, _thrust_curve_expo, AP_MOTORS_THST_EXPO_DEFAULT),
+    AP_GROUPINFO("THST_EXPO", 8, AP_MotorsMulticopter, _thrust_curve_expo, AP_MOTORS_THST_EXPO_DEFAULT),
 
     // @Param: THST_MAX
     // @DisplayName: Thrust Curve Max
     // @Description: Point at which the thrust saturates
     // @Values: 0.9:Low, 1.0:High
     // @User: Advanced
-    AP_GROUPINFO("THST_MAX", 9, AP_Motors_Multirotor, _thrust_curve_max, AP_MOTORS_THST_MAX_DEFAULT),
+    AP_GROUPINFO("THST_MAX", 9, AP_MotorsMulticopter, _thrust_curve_max, AP_MOTORS_THST_MAX_DEFAULT),
 
     // @Param: THST_BAT_MAX
     // @DisplayName: Battery voltage compensation maximum voltage
@@ -66,7 +66,7 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
     // @Range: 6 35
     // @Units: Volts
     // @User: Advanced
-    AP_GROUPINFO("THST_BAT_MAX", 10, AP_Motors_Multirotor, _batt_voltage_max, AP_MOTORS_THST_BAT_MAX_DEFAULT),
+    AP_GROUPINFO("THST_BAT_MAX", 10, AP_MotorsMulticopter, _batt_voltage_max, AP_MOTORS_THST_BAT_MAX_DEFAULT),
 
     // @Param: THST_BAT_MIN
     // @DisplayName: Battery voltage compensation minimum voltage
@@ -74,7 +74,7 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
     // @Range: 6 35
     // @Units: Volts
     // @User: Advanced
-    AP_GROUPINFO("THST_BAT_MIN", 11, AP_Motors_Multirotor, _batt_voltage_min, AP_MOTORS_THST_BAT_MIN_DEFAULT),
+    AP_GROUPINFO("THST_BAT_MIN", 11, AP_MotorsMulticopter, _batt_voltage_min, AP_MOTORS_THST_BAT_MIN_DEFAULT),
 
     // @Param: CURR_MAX
     // @DisplayName: Motor Current Max
@@ -82,43 +82,52 @@ const AP_Param::GroupInfo AP_Motors_Multirotor::var_info[] PROGMEM = {
     // @Range: 0 200
     // @Units: Amps
     // @User: Advanced
-    AP_GROUPINFO("CURR_MAX", 12, AP_Motors_Multirotor, _batt_current_max, AP_MOTORS_CURR_MAX_DEFAULT),
+    AP_GROUPINFO("CURR_MAX", 12, AP_MotorsMulticopter, _batt_current_max, AP_MOTORS_CURR_MAX_DEFAULT),
 
     // @Param: THR_MIX_MIN
     // @DisplayName: Throttle Mix Minimum
     // @Description: Minimum ratio that the average throttle can increase above the desired throttle after roll, pitch and yaw are mixed
     // @Range: 0.1 0.25
     // @User: Advanced
-    AP_GROUPINFO("THR_MIX_MIN", 13, AP_Motors_Multirotor, _thr_mix_min, AP_MOTORS_THR_MIX_MIN_DEFAULT),
+    AP_GROUPINFO("THR_MIX_MIN", 13, AP_MotorsMulticopter, _thr_mix_min, AP_MOTORS_THR_MIX_MIN_DEFAULT),
 
     AP_GROUPEND
 };
 
 // Constructor
-AP_Motors_Multirotor::AP_Motors_Multirotor(uint16_t loop_rate, uint16_t speed_hz) :
+AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) :
     AP_Motors(loop_rate, speed_hz),
-    _throttle_thr_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT),
+    _spin_when_armed_ramped(0),
     _throttle_thr_mix_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT),
+    _throttle_thr_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT),
     _min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
     _max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
-    _hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE)
+    _hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
-
+    _batt_voltage_resting(0.0f),
+    _batt_current_resting(0.0f),
+    _batt_resistance(0.0f),
+    _batt_timer(0),
+    _lift_max(1.0f),
+    _throttle_limit(1.0f)
 {
     AP_Param::setup_object_defaults(this, var_info);
 
+    // disable all motors by default
+    memset(motor_enabled, false, sizeof(motor_enabled));
+
     // setup battery voltage filtering
     _batt_voltage_filt.set_cutoff_frequency(AP_MOTORS_BATT_VOLT_FILT_HZ);
     _batt_voltage_filt.reset(1.0f);
 };
 
 // get_hover_throttle_as_pwm - converts hover throttle to pwm (i.e. range 1000 ~ 2000)
-int16_t AP_Motors_Multirotor::get_hover_throttle_as_pwm() const
+int16_t AP_MotorsMulticopter::get_hover_throttle_as_pwm() const
 {
     return (_throttle_radio_min + (float)(_throttle_radio_max - _throttle_radio_min) * _hover_out / 1000.0f);
 }
 
 // output - sends commands to the motors
-void AP_Motors_Multirotor::output()
+void AP_MotorsMulticopter::output()
 {
     // update throttle filter
     update_throttle_filter();
@@ -144,12 +153,13 @@ void AP_Motors_Multirotor::output()
             output_armed_not_stabilizing();
         }
     } else {
+        _multicopter_flags.slow_start_low_end = true;
         output_disarmed();
     }
 };
 
 // update the throttle input filter
-void AP_Motors_Multirotor::update_throttle_filter()
+void AP_MotorsMulticopter::update_throttle_filter()
 {
     if (armed()) {
         _throttle_filter.apply(_throttle_in, 1.0f/_loop_rate);
@@ -162,26 +172,26 @@ void AP_Motors_Multirotor::update_throttle_filter()
 }
 
 // update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
-void AP_Motors_Multirotor::update_max_throttle()
+void AP_MotorsMulticopter::update_max_throttle()
 {
     // ramp up minimum spin speed if necessary
-    if (_flags.slow_start_low_end) {
+    if (_multicopter_flags.slow_start_low_end) {
         _spin_when_armed_ramped += AP_MOTOR_SLOW_START_LOW_END_INCREMENT;
         if (_spin_when_armed_ramped >= _spin_when_armed) {
             _spin_when_armed_ramped = _spin_when_armed;
-            _flags.slow_start_low_end = false;
+            _multicopter_flags.slow_start_low_end = false;
         }
     }
 
     // implement slow start
-    if (_flags.slow_start) {
+    if (_multicopter_flags.slow_start) {
     // increase slow start throttle
     _max_throttle += AP_MOTOR_SLOW_START_INCREMENT;
 
     // turn off slow start if we've reached max throttle
     if (_max_throttle >= _throttle_control_input) {
         _max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
-        _flags.slow_start = false;
+        _multicopter_flags.slow_start = false;
     }
         return;
     }
@@ -191,7 +201,7 @@ void AP_Motors_Multirotor::update_max_throttle()
 }
 
 // current_limit_max_throttle - limits maximum throttle based on current
-void AP_Motors_Multirotor::current_limit_max_throttle()
+void AP_MotorsMulticopter::current_limit_max_throttle()
 {
     // return maximum if current limiting is disabled
     if (_batt_current_max <= 0) {
@@ -225,7 +235,7 @@ void AP_Motors_Multirotor::current_limit_max_throttle()
 }
 
 // apply_thrust_curve_and_volt_scaling - returns throttle curve adjusted pwm value (i.e. 1000 ~ 2000)
-int16_t AP_Motors_Multirotor::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const
+int16_t AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const
 {
     // convert to 0.0 to 1.0 ratio
     float throttle_ratio = ((float)(pwm_out-pwm_min))/((float)(pwm_max-pwm_min));
@@ -243,7 +253,7 @@ int16_t AP_Motors_Multirotor::apply_thrust_curve_and_volt_scaling(int16_t pwm_ou
 }
 
 // update_lift_max from battery voltage - used for voltage compensation
-void AP_Motors_Multirotor::update_lift_max_from_batt_voltage()
+void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
 {
     // sanity check battery_voltage_min is not too small
     // if disabled or misconfigured exit immediately
@@ -267,7 +277,7 @@ void AP_Motors_Multirotor::update_lift_max_from_batt_voltage()
 }
 
 // update_battery_resistance - calculate battery resistance when throttle is above hover_out
-void AP_Motors_Multirotor::update_battery_resistance()
+void AP_MotorsMulticopter::update_battery_resistance()
 {
     // if motors are stopped, reset resting voltage and current
     if (_throttle_control_input <= 0 || !_flags.armed) {
@@ -290,7 +300,7 @@ void AP_Motors_Multirotor::update_battery_resistance()
 }
 
 // update_throttle_thr_mix - slew set_throttle_thr_mix to requested value
-void AP_Motors_Multirotor::update_throttle_thr_mix()
+void AP_MotorsMulticopter::update_throttle_thr_mix()
 {
     // slew _throttle_thr_mix to _throttle_thr_mix_desired
     if (_throttle_thr_mix < _throttle_thr_mix_desired) {
@@ -303,7 +313,7 @@ void AP_Motors_Multirotor::update_throttle_thr_mix()
     _throttle_thr_mix = constrain_float(_throttle_thr_mix, 0.1f, 1.0f);
 }
 
-float AP_Motors_Multirotor::get_compensation_gain() const
+float AP_MotorsMulticopter::get_compensation_gain() const
 {
     // avoid divide by zero
     if (_lift_max <= 0.0f) {
@@ -319,19 +329,19 @@ float AP_Motors_Multirotor::get_compensation_gain() const
     return ret;
 }
 
-float AP_Motors_Multirotor::rel_pwm_to_thr_range(float pwm) const
+float AP_MotorsMulticopter::rel_pwm_to_thr_range(float pwm) const
 {
     return pwm/_throttle_pwm_scalar;
 }
 
-float AP_Motors_Multirotor::thr_range_to_rel_pwm(float thr) const
+float AP_MotorsMulticopter::thr_range_to_rel_pwm(float thr) const
 {
     return _throttle_pwm_scalar*thr;
 }
 
 // set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
 // also sets throttle channel minimum and maximum pwm
-void AP_Motors_Multirotor::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max)
+void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max)
 {
     _throttle_radio_min = radio_min;
     _throttle_radio_max = radio_max;
@@ -341,10 +351,10 @@ void AP_Motors_Multirotor::set_throttle_range(uint16_t min_throttle, int16_t rad
 
 // 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_Multirotor::slow_start(bool true_false)
+void AP_MotorsMulticopter::slow_start(bool true_false)
 {
     // set slow_start flag
-    _flags.slow_start = true;
+    _multicopter_flags.slow_start = true;
 
     // initialise maximum throttle to current throttle
     _max_throttle = constrain_int16(_throttle_control_input, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
@@ -352,7 +362,7 @@ void AP_Motors_Multirotor::slow_start(bool true_false)
 
 // throttle_pass_through - passes provided pwm directly to all motors - dangerous but used for initialising ESCs
 //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
-void AP_Motors_Multirotor::throttle_pass_through(int16_t pwm)
+void AP_MotorsMulticopter::throttle_pass_through(int16_t pwm)
 {
     if (armed()) {
         // send the pilot's input directly to each enabled motor
@@ -362,4 +372,4 @@ void AP_Motors_Multirotor::throttle_pass_through(int16_t pwm)
             }
         }
     }
-}
+}

libraries/AP_Motors/AP_MotorsMulticopter.h

@@ -1,13 +1,17 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-/// @file	AP_Motors_Multirotor.h
+/// @file	AP_MotorsMulticopter.h
-/// @brief	Motor control class for Multirotors
+/// @brief	Motor control class for Multicopters
 
-#ifndef __AP_MOTORS_MULTIROTOR_H__
+#ifndef __AP_MOTORS_MULTICOPTER_H__
-#define __AP_MOTORS_MULTIROTOR_H__
+#define __AP_MOTORS_MULTICOPTER_H__
 
 #include "AP_Motors_Class.h"
 
+#define AP_MOTORS_DEFAULT_MIN_THROTTLE  130
+#define AP_MOTORS_DEFAULT_MID_THROTTLE  500
+#define AP_MOTORS_DEFAULT_MAX_THROTTLE  1000
+
 #define AP_MOTORS_SPIN_WHEN_ARMED       70      // spin motors at this PWM value when armed
 #define AP_MOTORS_YAW_HEADROOM_DEFAULT  200
 #define AP_MOTORS_THR_LOW_CMP_DEFAULT   0.5f    // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control.  Higher favours Attitude over pilot input
@@ -32,12 +36,12 @@
  #define AP_MOTOR_SLOW_START_LOW_END_INCREMENT   1       // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 0.3 second)
 #endif
 
-/// @class      AP_Motors_Multirotor
+/// @class      AP_MotorsMulticopter
-class AP_Motors_Multirotor :public AP_Motors {
+class AP_MotorsMulticopter : public AP_Motors {
 public:
 
     // Constructor
-    AP_Motors_Multirotor(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
+    AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
 
     // output - sends commands to the motors
     virtual void        output();
@@ -56,13 +60,12 @@ public:
     void                set_throttle_mix_max() { _throttle_thr_mix_desired = AP_MOTORS_THR_MIX_MAX_DEFAULT; }
 
     // get_throttle_thr_mix - get low throttle compensation value
-    bool                is_throttle_mix_min() { return (_throttle_thr_mix < 1.25f*_thr_mix_min); }
+    bool                is_throttle_mix_min() const { return (_throttle_thr_mix < 1.25f*_thr_mix_min); }
 
     // returns warning throttle
-    float               get_throttle_warn() { return rel_pwm_to_thr_range(_spin_when_armed); }
+    float               get_throttle_warn() const { return rel_pwm_to_thr_range(_spin_when_armed); }
 
-    int16_t             throttle_min() const { return _min_throttle;}
+    int16_t             throttle_min() const { return rel_pwm_to_thr_range(_min_throttle); }
-    int16_t             throttle_max() const { return _max_throttle;}
 
     // set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
     // also sets throttle channel minimum and maximum pwm
@@ -80,6 +83,18 @@ public:
     //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
     void                throttle_pass_through(int16_t pwm);
 
+    // get_lift_max - get maximum lift ratio - for logging purposes only
+    float               get_lift_max() { return _lift_max; }
+
+    // get_batt_voltage_filt - get battery voltage ratio - for logging purposes only
+    float               get_batt_voltage_filt() const { return _batt_voltage_filt.get(); }
+
+    // get_batt_resistance - get battery resistance approximation - for logging purposes only
+    float               get_batt_resistance() const { return _batt_resistance; }
+
+    // get_throttle_limit - throttle limit ratio - for logging purposes only
+    float               get_throttle_limit() const { return _throttle_limit; }
+
     // var_info for holding Parameter information
     static const struct AP_Param::GroupInfo        var_info[];
 
@@ -112,6 +127,21 @@ protected:
     float               rel_pwm_to_thr_range(float pwm) const;
     float               thr_range_to_rel_pwm(float thr) const;
 
+    // convert RPY and Throttle servo ranges from legacy controller scheme back into PWM values
+    // RPY channels typically +/-45 degrees servo travel between +/-400 PWM
+    // Throttle channel typically 0-1000 range converts to 1100-1900 PWM for final output signal to motors
+    // ToDo: this should all be handled as floats +/- 1.0 instead of PWM and fake angle ranges
+    int16_t             calc_roll_pwm() { return (_roll_control_input / 11.25f);}
+    int16_t             calc_pitch_pwm() { return (_pitch_control_input / 11.25f);}
+    int16_t             calc_yaw_pwm() { return (_yaw_control_input / 11.25f);}
+    int16_t             calc_throttle_radio_output() { return (_throttle_control_input * _throttle_pwm_scalar) + _throttle_radio_min;}
+
+    // flag bitmask
+    struct {
+        uint8_t slow_start         : 1;    // 1 if slow start is active
+        uint8_t slow_start_low_end : 1;    // 1 just after arming so we can ramp up the spin_when_armed value
+    } _multicopter_flags;
+
     // parameters
     AP_Int16            _spin_when_armed;       // used to control whether the motors always spin when armed.  pwm value above radio_min
 
@@ -124,11 +154,21 @@ protected:
     AP_Float            _thr_mix_min;           // current over which maximum throttle is limited
 
     // internal variables
+    bool                motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];    // true if motor is enabled
+    int16_t             _spin_when_armed_ramped;    // equal to _spin_when_armed parameter but slowly ramped up from zero
     float               _throttle_thr_mix_desired;  // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
     float               _throttle_thr_mix;          // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
     int16_t             _min_throttle;              // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
     int16_t             _max_throttle;              // the maximum throttle to be sent to the motors (sometimes limited by slow start)
     int16_t             _hover_out;                 // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
 
+    // battery voltage, current and air pressure compensation variables
+    float               _batt_voltage_resting;  // battery voltage reading at minimum throttle
+    LowPassFilterFloat  _batt_voltage_filt;     // filtered battery voltage expressed as a percentage (0 ~ 1.0) of batt_voltage_max
+    float               _batt_current_resting;  // battery's current when motors at minimum
+    float               _batt_resistance;       // battery's resistance calculated by comparing resting voltage vs in flight voltage
+    int16_t             _batt_timer;            // timer used in battery resistance calcs
+    float               _lift_max;              // maximum lift ratio from battery voltage
+    float               _throttle_limit;        // ratio of throttle limit between hover and maximum
 };
-#endif  // __AP_MOTORS_MULTIROTOR_H__
+#endif  // __AP_MOTORS_MULTICOPTER_H__