zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Motors: add PWM_MIN, MAX 

These parameters allow the motor PWM output range to be different from receiver's throttle input range
Also renamed throttle_pass_through to set_throttle_passthrough_for_esc_calibration and change the method's input to the 0 to 1 range instead of direct PWM value
mission-4.1.18
Randy Mackay 9 years ago
parent
bbe3a94175
commit
49b14b3cb6
6 changed files with 78 additions and 34 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      libraries/AP_Motors/AP_MotorsCoax.cpp
  2. 4
      libraries/AP_Motors/AP_MotorsMatrix.cpp
  3. 61
      libraries/AP_Motors/AP_MotorsMulticopter.cpp
  4. 13
      libraries/AP_Motors/AP_MotorsMulticopter.h
  5. 8
      libraries/AP_Motors/AP_MotorsSingle.cpp
  6. 12
      libraries/AP_Motors/AP_MotorsTri.cpp

8
libraries/AP_Motors/AP_MotorsCoax.cpp
Unescape View File

@ -122,8 +122,8 @@ void AP_MotorsCoax::output_to_motors() @@ -122,8 +122,8 @@ void AP_MotorsCoax::output_to_motors()
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_roll_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_pitch_radio_passthrough, _servo4));
rc_write(AP_MOTORS_MOT_5, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_6, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_5, get_pwm_min());
rc_write(AP_MOTORS_MOT_6, get_pwm_min());
hal.rcout->push();
break;
case SPIN_WHEN_ARMED:
@ -133,8 +133,8 @@ void AP_MotorsCoax::output_to_motors() @@ -133,8 +133,8 @@ void AP_MotorsCoax::output_to_motors()
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_6, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_5, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle));
hal.rcout->push();
break;
case SPOOL_UP:

4
libraries/AP_Motors/AP_MotorsMatrix.cpp
Unescape View File

@ -94,7 +94,7 @@ void AP_MotorsMatrix::output_to_motors() @@ -94,7 +94,7 @@ void AP_MotorsMatrix::output_to_motors()
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = _throttle_radio_min;
motor_out[i] = get_pwm_min();
}
}
break;
@ -102,7 +102,7 @@ void AP_MotorsMatrix::output_to_motors() @@ -102,7 +102,7 @@ void AP_MotorsMatrix::output_to_motors()
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle);
motor_out[i] = constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle);
}
}
break;

61
libraries/AP_Motors/AP_MotorsMulticopter.cpp
Unescape View File

@ -106,6 +106,20 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { @@ -106,6 +106,20 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @User: Advanced
AP_GROUPINFO("PWM_TYPE", 15, AP_MotorsMulticopter, _pwm_type, PWM_TYPE_NORMAL),
// @Param: PWM_MIN
// @DisplayName: PWM output miniumum
// @Description: This sets the min PWM output value that will ever be output to the motors, 0 = use input RC3_MIN
// @Range: 0 2000
// @User: Advanced
AP_GROUPINFO("PWM_MIN", 16, AP_MotorsMulticopter, _pwm_min, 0),
// @Param: PWM_MAX
// @DisplayName: PWM output maximum
// @Description: This sets the max PWM value that will ever be output to the motors, 0 = use input RC3_MAX
// @Range: 0 2000
// @User: Advanced
AP_GROUPINFO("PWM_MAX", 17, AP_MotorsMulticopter, _pwm_max, 0),
AP_GROUPEND
};
@ -114,10 +128,7 @@ AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz @@ -114,10 +128,7 @@ AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz
AP_Motors(loop_rate, speed_hz),
_throttle_rpy_mix_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_throttle_rpy_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
_throttle_radio_min(1100),
_throttle_radio_max(1900),
_batt_voltage_resting(0.0f),
_batt_current_resting(0.0f),
_batt_resistance(0.0f),
@ -133,6 +144,9 @@ AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz @@ -133,6 +144,9 @@ AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz
// setup battery voltage filtering
_batt_voltage_filt.set_cutoff_frequency(AP_MOTORS_BATT_VOLT_FILT_HZ);
_batt_voltage_filt.reset(1.0f);
// default throttle ranges (i.e. _min_throttle, _throttle_radio_min, _throttle_radio_max)
set_throttle_range(130, 1100, 1900);
};
// output - sends commands to the motors
@ -314,17 +328,41 @@ float AP_MotorsMulticopter::get_compensation_gain() const @@ -314,17 +328,41 @@ float AP_MotorsMulticopter::get_compensation_gain() const
int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
{
return constrain_int16((_throttle_radio_min + _min_throttle + apply_thrust_curve_and_volt_scaling(thrust_in) *
( _throttle_radio_max - (_throttle_radio_min + _min_throttle))), _throttle_radio_min + _min_throttle, _throttle_radio_max);
return constrain_int16((get_pwm_min() + _min_throttle + apply_thrust_curve_and_volt_scaling(thrust_in) *
( get_pwm_max() - (get_pwm_min() + _min_throttle))), get_pwm_min() + _min_throttle, get_pwm_max());
}
// get minimum or maximum pwm value that can be output to motors
int16_t AP_MotorsMulticopter::get_pwm_min() const
{
// return _pwm_min if both PWM_MIN and PWM_MAX parmeters are defined and valid
if ((_pwm_min > 0) && (_pwm_max > 0) && (_pwm_max > _pwm_min)) {
return _pwm_min;
}
return _throttle_radio_min;
}
// get maximum pwm value that can be output to motors
int16_t AP_MotorsMulticopter::get_pwm_max() const
{
// return _pwm_max if both PWM_MIN and PWM_MAX parmeters are defined and valid
if ((_pwm_min > 0) && (_pwm_max > 0) && (_pwm_max > _pwm_min)) {
return _pwm_max;
}
return _throttle_radio_max;
}
// 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_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max)
{
// sanity check
if ((radio_max > radio_min) && (min_throttle < (radio_max - radio_min))) {
_throttle_radio_min = radio_min;
_throttle_radio_max = radio_max;
_min_throttle = (float)min_throttle * ((_throttle_radio_max - _throttle_radio_min) / 1000.0f);
}
// update _min_throttle
_min_throttle = (float)min_throttle * ((get_pwm_max() - get_pwm_min()) / 1000.0f);
}
void AP_MotorsMulticopter::output_logic()
@ -488,16 +526,17 @@ void AP_MotorsMulticopter::output_logic() @@ -488,16 +526,17 @@ void AP_MotorsMulticopter::output_logic()
}
}
// 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_MotorsMulticopter::throttle_pass_through(int16_t pwm)
// passes throttle directly to all motors for ESC calibration.
// throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_min() and 1 will send get_pwm_max()
void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float throttle_input)
{
if (armed()) {
uint16_t pwm_out = get_pwm_min() + constrain_float(throttle_input, 0.0f, 1.0f) * (get_pwm_max() - get_pwm_min());
// send the pilot's input directly to each enabled motor
hal.rcout->cork();
for (uint16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, pwm);
rc_write(i, pwm_out);
}
}
hal.rcout->push();
@ -516,7 +555,7 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask) @@ -516,7 +555,7 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask)
if (mask & (1U<<i)) {
motor_out = calc_thrust_to_pwm(thrust);
} else {
motor_out = _throttle_radio_min;
motor_out = get_pwm_min();
}
rc_write(i, motor_out);
}

13
libraries/AP_Motors/AP_MotorsMulticopter.h
Unescape View File

@ -10,7 +10,6 @@ @@ -10,7 +10,6 @@
#define AP_MOTORS_DENSITY_COMP 1
#endif
#define AP_MOTORS_DEFAULT_MIN_THROTTLE 130
#define AP_MOTORS_DEFAULT_MID_THROTTLE 500
#define AP_MOTORS_SPIN_WHEN_ARMED 70 // spin motors at this PWM value when armed
@ -78,9 +77,9 @@ public: @@ -78,9 +77,9 @@ public:
void output_logic();
// 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 throttle_pass_through(int16_t pwm);
// passes throttle directly to all motors for ESC calibration.
// throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_min() and 1 will send get_pwm_max()
void set_throttle_passthrough_for_esc_calibration(float throttle_input);
// get_lift_max - get maximum lift ratio - for logging purposes only
float get_lift_max() { return _lift_max; }
@ -143,6 +142,10 @@ protected: @@ -143,6 +142,10 @@ protected:
// convert thrust (0~1) range back to pwm range
int16_t calc_thrust_to_pwm(float thrust_in) const;
// get minimum or maximum pwm value that can be output to motors
int16_t get_pwm_min() const;
int16_t get_pwm_max() const;
// apply any thrust compensation for the frame
virtual void thrust_compensation(void) {}
@ -162,6 +165,8 @@ protected: @@ -162,6 +165,8 @@ protected:
AP_Float _batt_current_max; // current over which maximum throttle is limited
AP_Float _thr_mix_min; // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
AP_Float _thr_mix_max; // throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
AP_Int16 _pwm_min; // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
AP_Int16 _pwm_max; // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
// internal variables
bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled

8
libraries/AP_Motors/AP_MotorsSingle.cpp
Unescape View File

@ -125,8 +125,8 @@ void AP_MotorsSingle::output_to_motors() @@ -125,8 +125,8 @@ void AP_MotorsSingle::output_to_motors()
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough+_yaw_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough+_yaw_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough+_yaw_radio_passthrough, _servo4));
rc_write(AP_MOTORS_MOT_5, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_6, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_5, get_pwm_min());
rc_write(AP_MOTORS_MOT_6, get_pwm_min());
hal.rcout->push();
break;
case SPIN_WHEN_ARMED:
@ -136,8 +136,8 @@ void AP_MotorsSingle::output_to_motors() @@ -136,8 +136,8 @@ void AP_MotorsSingle::output_to_motors()
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_6, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_5, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle));
hal.rcout->push();
break;
case SPOOL_UP:

12
libraries/AP_Motors/AP_MotorsTri.cpp
Unescape View File

@ -128,18 +128,18 @@ void AP_MotorsTri::output_to_motors() @@ -128,18 +128,18 @@ void AP_MotorsTri::output_to_motors()
case SHUT_DOWN:
// sends minimum values out to the motors
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_2, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_4, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_1, get_pwm_min());
rc_write(AP_MOTORS_MOT_2, get_pwm_min());
rc_write(AP_MOTORS_MOT_4, get_pwm_min());
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
hal.rcout->push();
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_2, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_4, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_1, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_2, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_4, constrain_int16(get_pwm_min() + _throttle_low_end_pct * _min_throttle, get_pwm_min(), get_pwm_min() + _min_throttle));
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
hal.rcout->push();
break;

Write Preview
Loading…
Cancel
Save