From 690e8fd3f46be9dea728e9ee309cb37db281ab4c Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Fri, 23 Mar 2018 00:09:14 -0400 Subject: [PATCH] AP_Motors: TradHeli - Changed RSC mode 3 to a spline fit throttle curve --- libraries/AP_Motors/AP_MotorsHeli.cpp | 64 +++++++++++++------- libraries/AP_Motors/AP_MotorsHeli.h | 13 ++-- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 24 +++++--- libraries/AP_Motors/AP_MotorsHeli_Quad.cpp | 26 ++++---- libraries/AP_Motors/AP_MotorsHeli_RSC.cpp | 48 ++++++++++----- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 25 ++++---- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 40 ++++++------ 7 files changed, 143 insertions(+), 97 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index c1b66bfaf1..a2fc077099 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -122,21 +122,9 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { // @User: Standard AP_GROUPINFO("RSC_IDLE", 13, AP_MotorsHeli, _rsc_idle_output, AP_MOTORS_HELI_RSC_IDLE_DEFAULT), - // @Param: RSC_POWER_LOW - // @DisplayName: Throttle Servo Low Power Position - // @Description: Throttle output at zero collective pitch. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by H_RSC_PWM_MIN and H_RSC_PWM_MAX. Zero collective pitch is defined by H_COL_MID. - // @Range: 0 1000 - // @Increment: 10 - // @User: Standard - AP_GROUPINFO("RSC_POWER_LOW", 14, AP_MotorsHeli, _rsc_power_low, AP_MOTORS_HELI_RSC_POWER_LOW_DEFAULT), + // index 14 was RSC_POWER_LOW. Do not use this index in the future. - // @Param: RSC_POWER_HIGH - // @DisplayName: Throttle Servo High Power Position - // @Description: Throttle output at maximum collective pitch. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by H_RSC_PWM_MIN and H_RSC_PWM_MAX. - // @Range: 0 1000 - // @Increment: 10 - // @User: Standard - AP_GROUPINFO("RSC_POWER_HIGH", 15, AP_MotorsHeli, _rsc_power_high, AP_MOTORS_HELI_RSC_POWER_HIGH_DEFAULT), + // index 15 was RSC_POWER_HIGH. Do not use this index in the future. // @Param: CYC_MAX // @DisplayName: Cyclic Pitch Angle Max @@ -155,13 +143,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { // @User: Standard AP_GROUPINFO("SV_TEST", 17, AP_MotorsHeli, _servo_test, 0), - // @Param: RSC_POWER_NEGC - // @DisplayName: Throttle servo negative collective power position - // @Description: Throttle output at full negative collective pitch. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by H_RSC_PWM_MIN and H_RSC_PWM_MAX. If this is equal to H_RSC_POWER_HIGH then you will have a symmetric V-curve for the throttle response. - // @Range: 1 1000 - // @Increment: 10 - // @User: Standard - AP_GROUPINFO("RSC_POWER_NEGC", 18, AP_MotorsHeli, _rsc_power_negc, AP_MOTORS_HELI_RSC_POWER_HIGH_DEFAULT), + // index 18 was RSC_POWER_NEGC. Do not use this index in the future. // @Param: RSC_SLEWRATE // @DisplayName: Throttle servo slew rate @@ -171,6 +153,46 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = { // @User: Standard AP_GROUPINFO("RSC_SLEWRATE", 19, AP_MotorsHeli, _rsc_slewrate, 0), + // @Param: RSC_THRCRV_0 + // @DisplayName: Throttle Servo Position for 0 percent collective + // @Description: Throttle Servo Position for 0 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. + // @Range: 0 1000 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("RSC_THRCRV_0", 20, AP_MotorsHeli, _rsc_thrcrv[0], AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT), + + // @Param: RSC_THRCRV_25 + // @DisplayName: Throttle Servo Position for 25 percent collective + // @Description: Throttle Servo Position for 25 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. + // @Range: 0 1000 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("RSC_THRCRV_25", 21, AP_MotorsHeli, _rsc_thrcrv[1], AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT), + + // @Param: RSC_THRCRV_50 + // @DisplayName: Throttle Servo Position for 50 percent collective + // @Description: Throttle Servo Position for 50 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. + // @Range: 0 1000 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("RSC_THRCRV_50", 22, AP_MotorsHeli, _rsc_thrcrv[2], AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT), + + // @Param: RSC_THRCRV_75 + // @DisplayName: Throttle Servo Position for 75 percent collective + // @Description: Throttle Servo Position for 75 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. + // @Range: 0 1000 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("RSC_THRCRV_75", 23, AP_MotorsHeli, _rsc_thrcrv[3], AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT), + + // @Param: RSC_THRCRV_100 + // @DisplayName: Throttle Servo Position for 100 percent collective + // @Description: Throttle Servo Position for 100 percent collective. This is on a scale from 0 to 1000, where 1000 is full throttle and 0 is zero throttle. Actual PWM values are controlled by SERVOX_MIN and SERVOX_MAX. The 0 percent collective is defined by H_COL_MIN and 100 percent collective is defined by H_COL_MAX. + // @Range: 0 1000 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("RSC_THRCRV_100", 24, AP_MotorsHeli, _rsc_thrcrv[4], AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT), + AP_GROUPEND }; diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 6c2f978884..9d39f125f8 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -31,11 +31,14 @@ // RSC output defaults #define AP_MOTORS_HELI_RSC_IDLE_DEFAULT 0 -#define AP_MOTORS_HELI_RSC_POWER_LOW_DEFAULT 200 -#define AP_MOTORS_HELI_RSC_POWER_HIGH_DEFAULT 700 +#define AP_MOTORS_HELI_RSC_THRCRV_0_DEFAULT 250 +#define AP_MOTORS_HELI_RSC_THRCRV_25_DEFAULT 320 +#define AP_MOTORS_HELI_RSC_THRCRV_50_DEFAULT 380 +#define AP_MOTORS_HELI_RSC_THRCRV_75_DEFAULT 500 +#define AP_MOTORS_HELI_RSC_THRCRV_100_DEFAULT 1000 // default main rotor ramp up time in seconds -#define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 second to ramp output to main rotor ESC to full power (most people use exterrnal govenors so we can ramp up quickly) +#define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 second to ramp output to main rotor ESC to setpoint #define AP_MOTORS_HELI_RSC_RUNUP_TIME 10 // 10 seconds for rotor to reach full speed // flybar types @@ -201,9 +204,7 @@ protected: AP_Int16 _land_collective_min; // Minimum collective when landed or landing AP_Int16 _rsc_critical; // Rotor speed below which flight is not possible AP_Int16 _rsc_idle_output; // Rotor control output while at idle - AP_Int16 _rsc_power_low; // throttle value sent to throttle servo at zero collective pitch - AP_Int16 _rsc_power_high; // throttle value sent to throttle servo at maximum collective pitch - AP_Int16 _rsc_power_negc; // throttle value sent to throttle servo at full negative collective pitch + AP_Int16 _rsc_thrcrv[5]; // throttle value sent to throttle servo at 0, 25, 50, 75 and 100 percent collective AP_Int16 _rsc_slewrate; // throttle slew rate (percentage per second) AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 90f8257345..dadc8c998b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -260,11 +260,15 @@ void AP_MotorsHeli_Dual::set_desired_rotor_speed(float desired_speed) // calculate_armed_scalars void AP_MotorsHeli_Dual::calculate_armed_scalars() { + float thrcrv[5]; + for (uint8_t i = 0; i < 5; i++) { + thrcrv[i]=_rsc_thrcrv[i]*0.001f; + } _rotor.set_ramp_time(_rsc_ramp_time); _rotor.set_runup_time(_rsc_runup_time); - _rotor.set_critical_speed(_rsc_critical/1000.0f); - _rotor.set_idle_output(_rsc_idle_output/1000.0f); - _rotor.set_power_output_range(_rsc_power_low/1000.0f, _rsc_power_high/1000.0f, _rsc_power_high/1000.0f, 0); + _rotor.set_critical_speed(_rsc_critical*0.001f); + _rotor.set_idle_output(_rsc_idle_output*0.001f); + _rotor.set_throttle_curve(thrcrv, (uint16_t)_rsc_slewrate.get()); } // calculate_scalars @@ -477,22 +481,22 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c // ensure not below landed/landing collective - if (_heliflags.landing_collective && collective_out < (_land_collective_min/1000.0f)) { - collective_out = _land_collective_min/1000.0f; + if (_heliflags.landing_collective && collective_out < (_land_collective_min*0.001f)) { + collective_out = _land_collective_min*0.001f; limit.throttle_lower = true; } // scale collective pitch for front swashplate (servos 1,2,3) - float collective_scaler = ((float)(_collective_max-_collective_min))/1000.0f; - float collective_out_scaled = collective_out * collective_scaler + (_collective_min - 1000)/1000.0f; + float collective_scaler = ((float)(_collective_max-_collective_min))*0.001f; + float collective_out_scaled = collective_out * collective_scaler + (_collective_min - 1000)*0.001f; // scale collective pitch for rear swashplate (servos 4,5,6) - float collective2_scaler = ((float)(_collective2_max-_collective2_min))/1000.0f; - float collective2_out_scaled = collective2_out * collective2_scaler + (_collective2_min - 1000)/1000.0f; + float collective2_scaler = ((float)(_collective2_max-_collective2_min))*0.001f; + float collective2_out_scaled = collective2_out * collective2_scaler + (_collective2_min - 1000)*0.001f; // feed power estimate into main rotor controller // ToDo: add main rotor cyclic power? - _rotor.set_motor_load(fabsf(collective_out - _collective_mid_pct)); + _rotor.set_collective(fabsf(collective_out)); // swashplate servos float servo1_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)*0.45f + _collectiveFactor[CH_1] * collective_out_scaled; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 8ab075ebdd..dbc17f728f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -56,7 +56,7 @@ bool AP_MotorsHeli_Quad::init_outputs() return false; } } - + // set rotor servo range _rotor.init_servo(); @@ -99,11 +99,15 @@ void AP_MotorsHeli_Quad::set_desired_rotor_speed(float desired_speed) // calculate_armed_scalars void AP_MotorsHeli_Quad::calculate_armed_scalars() { + float thrcrv[5]; + for (uint8_t i = 0; i < 5; i++) { + thrcrv[i]=_rsc_thrcrv[i]*0.001f; + } _rotor.set_ramp_time(_rsc_ramp_time); _rotor.set_runup_time(_rsc_runup_time); - _rotor.set_critical_speed(_rsc_critical/1000.0f); - _rotor.set_idle_output(_rsc_idle_output/1000.0f); - _rotor.set_power_output_range(_rsc_power_low/1000.0f, _rsc_power_high/1000.0f, _rsc_power_high/1000.0f, 0); + _rotor.set_critical_speed(_rsc_critical*0.001f); + _rotor.set_idle_output(_rsc_idle_output*0.001f); + _rotor.set_throttle_curve(thrcrv, (uint16_t)_rsc_slewrate.get()); } // calculate_scalars @@ -136,7 +140,7 @@ void AP_MotorsHeli_Quad::calculate_roll_pitch_collective_factors() const float angles[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { 45, 225, 315, 135 }; const bool x_clockwise[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { false, false, true, true }; const float cos45 = cosf(radians(45)); - + for (uint8_t i=0; i= 0) { - float range = _power_output_high - _power_output_low; - _control_output = _idle_output + (_rotor_ramp_output * ((_power_output_low - _idle_output) + (range * _load_feedforward))); - } else { - float range = _power_output_negc - _power_output_low; - _control_output = _idle_output + (_rotor_ramp_output * ((_power_output_low - _idle_output) - (range * _load_feedforward))); - } + // throttle output from throttle curve based on collective position + float desired_throttle = calculate_desired_throttle(_collective_in); + _control_output = _idle_output + (_rotor_ramp_output * (desired_throttle - _idle_output)); } break; } @@ -97,7 +97,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) float max_delta = dt * _power_slewrate * 0.01f; _control_output = constrain_float(_control_output, last_control_output-max_delta, last_control_output+max_delta); } - + // output to rsc servo write_rsc(_control_output); } @@ -190,3 +190,19 @@ void AP_MotorsHeli_RSC::write_rsc(float servo_out) SRV_Channels::set_output_scaled(_aux_fn, (uint16_t) (servo_out * 1000)); } } + + // calculate_desired_throttle - uses throttle curve and collective input to determine throttle setting +float AP_MotorsHeli_RSC::calculate_desired_throttle(float collective_in) +{ + + const float inpt = collective_in * 4.0f + 1.0f; + uint8_t idx = constrain_int16(int8_t(collective_in * 4), 0, 3); + const float a = inpt - (idx + 1.0f); + const float b = (idx + 1.0f) - inpt + 1.0f; + float throttle = _thrcrv_poly[idx][0] * a + _thrcrv_poly[idx][1] * b + _thrcrv_poly[idx][2] * (powf(a,3.0f) - a) / 6.0f + _thrcrv_poly[idx][3] * (powf(b,3.0f) - b) / 6.0f; + + throttle = constrain_float(throttle, 0.0f, 1.0f); + return throttle; + +} + diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 0013535d92..92726caadb 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -26,7 +26,7 @@ public: friend class AP_MotorsHeli_Single; friend class AP_MotorsHeli_Dual; friend class AP_MotorsHeli_Quad; - + AP_MotorsHeli_RSC(SRV_Channel::Aux_servo_function_t aux_fn, uint8_t default_channel) : _aux_fn(aux_fn), @@ -41,7 +41,7 @@ public: // set_critical_speed void set_critical_speed(float critical_speed) { _critical_speed = critical_speed; } - + // get_critical_speed float get_critical_speed() const { return _critical_speed; } @@ -70,22 +70,22 @@ public: // set_runup_time void set_runup_time(int8_t runup_time) { _runup_time = runup_time; } - // set_power_output_range - void set_power_output_range(float power_low, float power_high, float power_negc, uint16_t slewrate); + // set_throttle_curve + void set_throttle_curve(float thrcrv[5], uint16_t slewrate); - // set_motor_load. +ve numbers for +ve collective. -ve numbers for negative collective - void set_motor_load(float load) { _load_feedforward = load; } + // set_collective. collective for throttle curve calculation + void set_collective(float collective) { _collective_in = collective; } // output - update value to send to ESC/Servo void output(RotorControlState state); private: uint64_t _last_update_us; - + // channel setup for aux function SRV_Channel::Aux_servo_function_t _aux_fn; uint8_t _default_channel; - + // internal variables RotorControlMode _control_mode = ROTOR_CONTROL_MODE_DISABLED; // motor control mode, Passthrough or Setpoint float _critical_speed = 0.0f; // rotor speed below which flight is not possible @@ -97,11 +97,9 @@ private: int8_t _ramp_time = 0; // time in seconds for the output to the main rotor's ESC to reach full speed int8_t _runup_time = 0; // time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time bool _runup_complete = false; // flag for determining if runup is complete - float _power_output_low = 0.0f; // setpoint for power output at minimum rotor power - float _power_output_high = 0.0f; // setpoint for power output at maximum rotor power - float _power_output_negc = 0.0f; // setpoint for power output at full negative collective + float _thrcrv_poly[4][4]; // spline polynomials for throttle curve interpolation uint16_t _power_slewrate = 0; // slewrate for throttle (percentage per second) - float _load_feedforward = 0.0f; // estimate of motor load, range 0-1.0f + float _collective_in; // collective in for throttle curve calculation, range 0-1.0f // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output void update_rotor_ramp(float rotor_ramp_input, float dt); @@ -111,4 +109,7 @@ private: // write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1 void write_rsc(float servo_out); + + // calculate_desired_throttle - uses throttle curve and collective input to determine throttle setting + float calculate_desired_throttle(float collective_in); }; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index d1801f205c..4d2161b5b8 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -200,9 +200,9 @@ void AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm) // external gyro & tail servo if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { if (_acro_tail && _ext_gyro_gain_acro > 0) { - write_aux(_ext_gyro_gain_acro/1000.0f); + write_aux(_ext_gyro_gain_acro*0.001f); } else { - write_aux(_ext_gyro_gain_std/1000.0f); + write_aux(_ext_gyro_gain_std*0.001f); } } rc_write(AP_MOTORS_MOT_4, pwm); @@ -223,17 +223,21 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(float desired_speed) _main_rotor.set_desired_speed(desired_speed); // always send desired speed to tail rotor control, will do nothing if not DDVP not enabled - _tail_rotor.set_desired_speed(_direct_drive_tailspeed/1000.0f); + _tail_rotor.set_desired_speed(_direct_drive_tailspeed*0.001f); } // calculate_scalars - recalculates various scalers used. void AP_MotorsHeli_Single::calculate_armed_scalars() { + float thrcrv[5]; + for (uint8_t i = 0; i < 5; i++) { + thrcrv[i]=_rsc_thrcrv[i]*0.001f; + } _main_rotor.set_ramp_time(_rsc_ramp_time); _main_rotor.set_runup_time(_rsc_runup_time); - _main_rotor.set_critical_speed(_rsc_critical/1000.0f); - _main_rotor.set_idle_output(_rsc_idle_output/1000.0f); - _main_rotor.set_power_output_range(_rsc_power_low/1000.0f, _rsc_power_high/1000.0f, _rsc_power_negc/1000.0f, (uint16_t)_rsc_slewrate.get()); + _main_rotor.set_critical_speed(_rsc_critical*0.001f); + _main_rotor.set_idle_output(_rsc_idle_output*0.001f); + _main_rotor.set_throttle_curve(thrcrv, (uint16_t)_rsc_slewrate.get()); } @@ -262,8 +266,8 @@ void AP_MotorsHeli_Single::calculate_scalars() _tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_SPEED_SETPOINT); _tail_rotor.set_ramp_time(_rsc_ramp_time); _tail_rotor.set_runup_time(_rsc_runup_time); - _tail_rotor.set_critical_speed(_rsc_critical/1000.0f); - _tail_rotor.set_idle_output(_rsc_idle_output/1000.0f); + _tail_rotor.set_critical_speed(_rsc_critical*0.001f); + _tail_rotor.set_idle_output(_rsc_idle_output*0.001f); } else { _tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_DISABLED); _tail_rotor.set_ramp_time(0); @@ -386,8 +390,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float } // ensure not below landed/landing collective - if (_heliflags.landing_collective && collective_out < (_land_collective_min/1000.0f)) { - collective_out = (_land_collective_min/1000.0f); + if (_heliflags.landing_collective && collective_out < (_land_collective_min*0.001f)) { + collective_out = (_land_collective_min*0.001f); limit.throttle_lower = true; } @@ -409,17 +413,11 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float // feed power estimate into main rotor controller // ToDo: include tail rotor power? // ToDo: add main rotor cyclic power? - if (collective_out > _collective_mid_pct) { - // +ve motor load for +ve collective - _main_rotor.set_motor_load((collective_out - _collective_mid_pct) / (1.0f - _collective_mid_pct)); - } else { - // -ve motor load for -ve collective - _main_rotor.set_motor_load((collective_out - _collective_mid_pct) / _collective_mid_pct); - } + _main_rotor.set_collective(fabsf(collective_out)); // swashplate servos - float collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f; - float coll_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)/1000.0f; + float collective_scalar = ((float)(_collective_max-_collective_min))*0.001f; + float coll_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)*0.001f; float servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * coll_out_scaled; float servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * coll_out_scaled; if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H1) { @@ -471,9 +469,9 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out) if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { // output gain to exernal gyro if (_acro_tail && _ext_gyro_gain_acro > 0) { - write_aux(_ext_gyro_gain_acro/1000.0f); + write_aux(_ext_gyro_gain_acro*0.001f); } else { - write_aux(_ext_gyro_gain_std/1000.0f); + write_aux(_ext_gyro_gain_std*0.001f); } } }