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

AP_Motors: TradHeli - Changed RSC mode 3 to a spline fit throttle curve

Browse Source
This commit is contained in:
bnsgeyer 2018-03-23 00:09:14 -04:00 committed by Andrew Tridgell
parent 600e0dac92
commit 690e8fd3f4
7 changed files with 143 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

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

13
libraries/AP_Motors/AP_MotorsHeli.h
View File

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

24
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

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

26
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -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<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
bool clockwise = x_clockwise[i];
if (_frame_type == MOTOR_FRAME_TYPE_H) {
@ -208,19 +212,19 @@ void AP_MotorsHeli_Quad::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;
}
float collective_range = (_collective_max - _collective_min) / 1000.0f;
float collective_range = (_collective_max - _collective_min)*0.001f;
if (_heliflags.inverted_flight) {
collective_out = 1 - collective_out;
}
// feed power estimate into main rotor controller
_rotor.set_motor_load(fabsf(collective_out - _collective_mid_pct));
_rotor.set_collective(fabsf(collective_out));
// scale collective to -1 to 1
collective_out = collective_out*2-1;
@ -263,7 +267,7 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
}
out[i] += y;
}
// move the servos
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
rc_write(AP_MOTORS_MOT_1+i, calc_pwm_output_1to1(out[i], _servo[i]));

48
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View File

@ -28,11 +28,17 @@ void AP_MotorsHeli_RSC::init_servo()
}
// set_power_output_range
void AP_MotorsHeli_RSC::set_power_output_range(float power_low, float power_high, float power_negc, uint16_t slewrate)
// TODO: Look at possibly calling this at a slower rate. Doesn't need to be called every cycle.
void AP_MotorsHeli_RSC::set_throttle_curve(float thrcrv[5], uint16_t slewrate)
{
_power_output_low = power_low;
_power_output_high = power_high;
_power_output_negc = power_negc;
// Ensure user inputs are within parameter limits
for (uint8_t i = 0; i < 5; i++) {
thrcrv[i] = constrain_float(thrcrv[i], 0.0f, 1.0f);
}
// Calculate the spline polynomials for the throttle curve
splinterp5(thrcrv,_thrcrv_poly);
_power_slewrate = slewrate;
}
@ -42,7 +48,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
float dt;
uint64_t now = AP_HAL::micros64();
float last_control_output = _control_output;
if (_last_update_us == 0) {
_last_update_us = now;
dt = 0.001f;
@ -50,7 +56,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
dt = 1.0e-6f * (now - _last_update_us);
_last_update_us = now;
}
switch (state){
case ROTOR_CONTROL_STOP:
// set rotor ramp to decrease speed to zero, this happens instantly inside update_rotor_ramp()
@ -76,15 +82,9 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
// set control rotor speed to ramp slewed value between idle and desired speed
_control_output = _idle_output + (_rotor_ramp_output * (_desired_speed - _idle_output));
} else if (_control_mode == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT) {
// throttle output depending on estimated power demand. Output is ramped up from idle speed during rotor runup. A negative load
// is for the left side of the V-curve (-ve collective) A positive load is for the right side (+ve collective)
if (_load_feedforward >= 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;
}

25
libraries/AP_Motors/AP_MotorsHeli_RSC.h
View File

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

40
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

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