Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: add PWM_MIN, MAX

Browse Source 
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
This commit is contained in:
Randy Mackay 2016-05-21 15:47:46 +09:00
parent bbe3a94175
commit 49b14b3cb6
6 changed files with 78 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -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()
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
View File

@ -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()
// 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
View File

@ -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
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
// 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
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()
}
}
// 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)
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
View File

@ -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:
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:
// 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:
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
View File

@ -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()
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
View File

@ -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;