aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Motors: move thrust curve and volt scaling to parent

Browse Source 
Moving from MotorsMatrix to Motors allows it to be used from other frames
This commit is contained in:
Leonard Hall 2015-02-21 16:55:13 +09:00 committed by Randy Mackay
parent 80b498f598
commit 751dbb7df7
3 changed files with 18 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -352,18 +352,13 @@ 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]) {
float temp_out = ((float)(motor_out[i]-out_min_pwm))/((float)(out_max_pwm-out_min_pwm));
if (_thrust_curve_expo > 0.0f){
temp_out = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*temp_out))/(2.0f*_thrust_curve_expo*_batt_voltage_filt);
}
motor_out[i] = temp_out*(_thrust_curve_max*out_max_pwm-out_min_pwm)+out_min_pwm;
}
// apply thrust curve and voltage scaling
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = apply_thrust_curve_and_volt_scaling(motor_out[i], out_min_pwm, out_max_pwm);
}
}
// clip motor output if required (shouldn't be)
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {

10
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -284,3 +284,13 @@ void AP_Motors::update_max_throttle()
_flags.slow_start = false;
}
}
// apply_thrust_curve_and_volt_scaling - returns throttle curve adjusted pwm value (i.e. 1000 ~ 2000)
int16_t AP_Motors::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const
{
float temp_out = ((float)(pwm_out-pwm_min))/((float)(pwm_max-pwm_min));
if (_thrust_curve_expo > 0.0f){
temp_out = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*temp_out))/(2.0f*_thrust_curve_expo*_batt_voltage_filt);
}
return (temp_out*(_thrust_curve_max*pwm_max-pwm_min)+pwm_min);
}

3
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -180,6 +180,9 @@ protected:
// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
void update_max_throttle();
// apply_thrust_curve_and_volt_scaling - thrust curve and voltage adjusted pwm value (i.e. 1000 ~ 2000)
int16_t apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const;
// flag bitmask
struct AP_Motors_flags {
uint8_t armed : 1; // 1 if the motors are armed, 0 if disarmed