diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index fe13951f4d1da..30f10cda24bc3 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -31,7 +31,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @Description: Tail type selection. Simpler yaw controller used if external gyro is selected. Direct Drive Variable Pitch is used for tails that have a motor that is governed at constant speed by an ESC. Tail pitch is still accomplished with a servo. Direct Drive Fixed Pitch (DDFP) CW is used for helicopters with a rotor that spins clockwise when viewed from above. Direct Drive Fixed Pitch (DDFP) CCW is used for helicopters with a rotor that spins counter clockwise when viewed from above. In both DDFP cases, no servo is used for the tail and the tail motor esc is controlled by the yaw axis. // @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch CW,4:DirectDrive FixedPitch CCW,5:DDVP with external governor // @User: Standard - AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO), + AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, float(TAIL_TYPE::SERVO)), // Indice 5 was used by SWASH_TYPE and should not be used @@ -210,25 +210,33 @@ void AP_MotorsHeli_Single::init_outputs() // initialize main rotor servo _main_rotor.init_servo(); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { - _tail_rotor.init_servo(); - } else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // external gyro output - add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO); - } - } + switch (get_tail_type()) { + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW: + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW: + // DDFP tails use range as it is easier to ignore servo trim in making for simple implementation of thrust linearisation. + SRV_Channels::set_range(SRV_Channel::k_motor4, 1.0f); + break; - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // External Gyro uses PWM output thus servo endpoints are forced - SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000); - } + case TAIL_TYPE::DIRECTDRIVE_VARPITCH: + case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV: + _tail_rotor.init_servo(); + break; + + case TAIL_TYPE::SERVO_EXTGYRO: + // external gyro output + add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO); + + + // External Gyro uses PWM output thus servo endpoints are forced + SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000); + FALLTHROUGH; - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // DDFP tails use range as it is easier to ignore servo trim in making for simple implementation of thrust linearisation. - SRV_Channels::set_range(SRV_Channel::k_motor4, 1.0f); - } else if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { - // yaw servo is an angle from -4500 to 4500 - SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE); + case TAIL_TYPE::SERVO: + default: + // yaw servo is an angle from -4500 to 4500 + SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE); + break; + } } set_initialised_ok(_frame_class == MOTOR_FRAME_HELI); @@ -266,13 +274,13 @@ void AP_MotorsHeli_Single::calculate_armed_scalars() _main_rotor.set_autorotation_flag(_heliflags.in_autorotation); // set bailout ramp time _main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { + if (use_tail_RSC()) { _tail_rotor.set_autorotation_flag(_heliflags.in_autorotation); _tail_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); } } else { _main_rotor.set_autorotation_flag(false); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { + if (use_tail_RSC()) { _tail_rotor.set_autorotation_flag(false); } } @@ -310,7 +318,7 @@ void AP_MotorsHeli_Single::calculate_scalars() calculate_armed_scalars(); // send setpoints to DDVP rotor controller and trigger recalculation of scalars - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { + if (use_tail_RSC()) { _tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_SETPOINT); _tail_rotor.set_ramp_time(_main_rotor._ramp_time.get()); _tail_rotor.set_runup_time(_main_rotor._runup_time.get()); @@ -370,8 +378,6 @@ void AP_MotorsHeli_Single::update_motor_control(RotorControlState state) // void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) { - float yaw_offset = 0.0f; - // initialize limits flag limit.throttle_lower = false; limit.throttle_upper = false; @@ -422,26 +428,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float // updates takeoff collective flag based on 50% hover collective update_takeoff_collective_flag(collective_out); - // if servo output not in manual mode and heli is not in autorotation, process pre-compensation factors - if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED && !_heliflags.in_autorotation) { - // rudder feed forward based on collective - // the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque - // also not required if we are using external gyro - if ((get_control_output() > _main_rotor.get_idle_output()) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // sanity check collective_yaw_scale - _collective_yaw_scale.set(constrain_float(_collective_yaw_scale, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE)); - // This feedforward compensation follows the hover performance theory that hover power required - // is a function of gross weight to the 3/2 power - yaw_offset = _collective_yaw_scale * powf(fabsf(collective_out - _collective_zero_thrust_pct),1.5f); - - // Add yaw trim for DDFP tails - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - yaw_offset += _yaw_trim.get(); - } - } - } else { - yaw_offset = 0.0f; - } + // Get yaw offset required to cancel out steady state main rotor torque + const float yaw_offset = get_yaw_offset(collective_out); // feed power estimate into main rotor controller // ToDo: include tail rotor power? @@ -475,72 +463,110 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out) _servo4_out = yaw_out; } -void AP_MotorsHeli_Single::output_to_motors() +// Get yaw offset required to cancel out steady state main rotor torque +float AP_MotorsHeli_Single::get_yaw_offset(float collective) { - if (!initialised_ok()) { - return; + if ((get_tail_type() == TAIL_TYPE::SERVO_EXTGYRO) || (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED)) { + // Not in direct control of tail with external gyro or manual servo mode + return 0.0; } - // Write swashplate outputs - _swashplate.output(); - - if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE); - } - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // output gain to exernal gyro - if (_acro_tail && _ext_gyro_gain_acro > 0) { - rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_acro); - } else { - rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std); - } + if (_heliflags.in_autorotation || (get_control_output() <= _main_rotor.get_idle_output())) { + // Motor is stopped or at idle, and thus not creating torque + return 0.0; } - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - _servo4_out = -_servo4_out; + // sanity check collective_yaw_scale + _collective_yaw_scale.set(constrain_float(_collective_yaw_scale, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE)); + + // This feedforward compensation follows the hover performance theory that hover power required + // is a function of gross weight to the 3/2 power + float yaw_offset = _collective_yaw_scale * powf(fabsf(collective - _collective_zero_thrust_pct),1.5f); + + // Add yaw trim for DDFP tails + if (have_DDFP_tail()) { + yaw_offset += _yaw_trim.get(); } - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // calc filtered battery voltage and lift_max - thr_lin.update_lift_max_from_batt_voltage(); + return yaw_offset; +} + +void AP_MotorsHeli_Single::output_to_motors() +{ + if (!initialised_ok()) { + return; } - // Warning: This spool state logic is what prevents DDFP tails from actuating when doing H_SV_MAN and H_SV_TEST tests + // Write swashplate outputs + _swashplate.output(); + + // Output main rotor switch (_spool_state) { case SpoolState::SHUT_DOWN: // sends minimum values out to the motors update_motor_control(ROTOR_CONTROL_STOP); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // Set DDFP to servo min - output_to_ddfp_tail(0.0); - } break; case SpoolState::GROUND_IDLE: // sends idle output to motors when armed. rotor could be static or turning (autorotation) update_motor_control(ROTOR_CONTROL_IDLE); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // Set DDFP to servo min - output_to_ddfp_tail(0.0); - } break; case SpoolState::SPOOLING_UP: case SpoolState::THROTTLE_UNLIMITED: // set motor output based on thrust requests update_motor_control(ROTOR_CONTROL_ACTIVE); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // Operate DDFP to between DDFP_SPIN_MIN and DDFP_SPIN_MAX using thrust linearisation - output_to_ddfp_tail(thr_lin.thrust_to_actuator(_servo4_out)); - } break; case SpoolState::SPOOLING_DOWN: // sends idle output to motors and wait for rotor to stop update_motor_control(ROTOR_CONTROL_IDLE); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW){ - // Set DDFP to servo min - output_to_ddfp_tail(0.0); + break; + } + + // Output tail rotor + switch (get_tail_type()) { + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW: + // Invert output for CCW tail + _servo4_out *= -1.0; + FALLTHROUGH; + + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW: { + // calc filtered battery voltage and lift_max + thr_lin.update_lift_max_from_batt_voltage(); + + // Only throttle up if in active spool state + switch (_spool_state) { + case AP_Motors::SpoolState::SHUT_DOWN: + case AP_Motors::SpoolState::GROUND_IDLE: + case AP_Motors::SpoolState::SPOOLING_DOWN: + // Set DDFP to servo min + output_to_ddfp_tail(0.0); + break; + + case AP_Motors::SpoolState::SPOOLING_UP: + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: + // Operate DDFP to between DDFP_SPIN_MIN and DDFP_SPIN_MAX using thrust linearisation + output_to_ddfp_tail(thr_lin.thrust_to_actuator(_servo4_out)); + break; } break; + } + + case TAIL_TYPE::SERVO_EXTGYRO: + // output gain to external gyro + if (_acro_tail && _ext_gyro_gain_acro > 0) { + rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_acro); + } else { + rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std); + } + FALLTHROUGH; + + case TAIL_TYPE::SERVO: + case TAIL_TYPE::DIRECTDRIVE_VARPITCH: + case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV: + default: + rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE); + break; } + } // handle output limit flags and send throttle to servos lib @@ -655,9 +681,7 @@ void AP_MotorsHeli_Single::heli_motors_param_conversions(void) // Previous DDFP configs used servo trim for setting the yaw trim, which no longer works with thrust linearisation. Convert servo trim // to H_YAW_TRIM. Default thrust linearisation gives linear thrust to throttle relationship to preserve previous setup behaviours so // we can assume linear relationship in the conversion. - if ((_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || - _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) && - !_yaw_trim.configured()) { + if (have_DDFP_tail() && !_yaw_trim.configured()) { SRV_Channel *c = SRV_Channels::get_channel_for(SRV_Channel::k_motor4); if (c != nullptr) { @@ -673,3 +697,19 @@ void AP_MotorsHeli_Single::heli_motors_param_conversions(void) _yaw_trim.save(); } } + +// Helper to return true for direct drive fixed pitch tail, either CW or CCW +bool AP_MotorsHeli_Single::have_DDFP_tail() const +{ + const TAIL_TYPE type = get_tail_type(); + return (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW) || + (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW); +} + +// Helper to return true if the tail RSC should be used +bool AP_MotorsHeli_Single::use_tail_RSC() const +{ + const TAIL_TYPE type = get_tail_type(); + return (type == TAIL_TYPE::DIRECTDRIVE_VARPITCH) || + (type == TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV); +} diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 595ce98a04f90..870d4801c9605 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -14,15 +14,6 @@ #define AP_MOTORS_HELI_SINGLE_EXTGYRO CH_7 #define AP_MOTORS_HELI_SINGLE_TAILRSC CH_7 -// tail types -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO 0 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO 1 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH 2 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW 3 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW 4 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV 5 - - // direct-drive variable pitch defaults #define AP_MOTORS_HELI_SINGLE_DDVP_SPEED_DEFAULT 50 @@ -72,8 +63,8 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli { // has_flybar - returns true if we have a mechical flybar bool has_flybar() const override { return _flybar_mode; } - // supports_yaw_passthrought - returns true if we support yaw passthrough - bool supports_yaw_passthrough() const override { return _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO; } + // supports_yaw_passthrough - returns true if we support yaw passthrough + bool supports_yaw_passthrough() const override { return get_tail_type() == TAIL_TYPE::SERVO_EXTGYRO; } void set_acro_tail(bool set) override { _acro_tail = set; } @@ -103,12 +94,33 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli { // move_yaw - moves the yaw servo void move_yaw(float yaw_out); + // Get yaw offset required to cancel out steady state main rotor torque + float get_yaw_offset(float collective); + // handle output limit flags and send throttle to servos lib void output_to_ddfp_tail(float throttle); // servo_test - move servos through full range of movement void servo_test() override; + // Tail types + enum class TAIL_TYPE { + SERVO = 0, + SERVO_EXTGYRO = 1, + DIRECTDRIVE_VARPITCH = 2, + DIRECTDRIVE_FIXEDPITCH_CW = 3, + DIRECTDRIVE_FIXEDPITCH_CCW = 4, + DIRECTDRIVE_VARPIT_EXT_GOV = 5 + }; + + TAIL_TYPE get_tail_type() const { return TAIL_TYPE(_tail_type.get()); } + + // Helper to return true for direct drive fixed pitch tail, either CW or CCW + bool have_DDFP_tail() const; + + // Helper to return true if the tail RSC should be used + bool use_tail_RSC() const; + // external objects we depend upon AP_MotorsHeli_RSC _tail_rotor; // tail rotor AP_MotorsHeli_Swash _swashplate; // swashplate diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index d65c940cd86f8..491bb0fc60bff 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -38,6 +38,7 @@ SRV_Channels srvs; AP_BattMonitor _battmonitor{0, nullptr, nullptr}; AP_Motors *motors; +AP_MotorsHeli *motors_heli; AP_MotorsMatrix *motors_matrix; bool thrust_boost = false; @@ -130,6 +131,30 @@ void setup() AP_Int8 *dual_mode = (AP_Int8*)motors + AP_MotorsHeli_Dual::var_info[1].offset; dual_mode->set(value); + } else if (strcmp(cmd,"tail_type") == 0) { + if (frame_class != AP_Motors::MOTOR_FRAME_HELI) { + ::printf("tail_type only supported by single heli frame type (%i), got %i\n", AP_Motors::MOTOR_FRAME_HELI, frame_class); + exit(1); + } + + // Union allows pointers to be aligned despite different sizes + // avoids "increases required alignment of target type" error when casting from char* to AP_Int16* + union { + char *char_ptr; + AP_Int16 *int16; + } tail_type; + + // look away now, more dodgy param access. + tail_type.char_ptr = (char*)motors + AP_MotorsHeli_Single::var_info[1].offset; + tail_type.int16->set(value); + + // Re-init motors to switch to the new tail type + // Have to do this twice to make sure the tail type sticks + motors->set_initialised_ok(false); + motors->init(frame_class, AP_Motors::MOTOR_FRAME_TYPE_X); + motors->set_initialised_ok(false); + motors->init(frame_class, AP_Motors::MOTOR_FRAME_TYPE_X); + } else if (strcmp(cmd,"frame_class") == 0) { // We must have the frame_class argument 2nd as resulting class is used to determine if // we have access to certain functions in the multicopter motors child class @@ -151,19 +176,27 @@ void setup() break; case AP_Motors::MOTOR_FRAME_HELI: - motors = new AP_MotorsHeli_Single(400); - // Mot 1-3 swashplate, mot 4 tail rotor pitch, mot 5 for 4th servo in H4-90 swash - num_outputs = 5; + motors_heli = new AP_MotorsHeli_Single(400); + motors = motors_heli; + // Mot 1-3: Swash plate 1 to 3 + // Mot 4: Tail rotor + // Mot 5: 4th servo in H4-90 swash + // Mot 6: Unused + // Mot 7: Tail rotor RSC / external governor output + // Mot 8: Main rotor RSC + num_outputs = 8; break; case AP_Motors::MOTOR_FRAME_HELI_DUAL: - motors = new AP_MotorsHeli_Dual(400); + motors_heli = new AP_MotorsHeli_Dual(400); + motors = motors_heli; // Mot 1-3 swashplate 1, mot 4-6 swashplate 2, mot 7 and 8 for 4th servos on H4-90 swash plates front and back, respectively num_outputs = 8; break; case AP_Motors::MOTOR_FRAME_HELI_QUAD: - motors = new AP_MotorsHeli_Quad(400); + motors_heli = new AP_MotorsHeli_Quad(400); + motors = motors_heli; num_outputs = 4; // Only 4 collective servos break; @@ -186,6 +219,29 @@ void setup() exit(1); } + } else if (strcmp(cmd,"COL2YAW") == 0) { + if (frame_class != AP_Motors::MOTOR_FRAME_HELI) { + ::printf("COL2YAW only supported by single heli frame type (%i), got %i\n", AP_Motors::MOTOR_FRAME_HELI, frame_class); + exit(1); + } + + // Union allows pointers to be aligned despite different sizes + // avoids "increases required alignment of target type" error when casting from char* to AP_Int16* + union { + char *char_ptr; + AP_Float *ap_float; + } collective_yaw_scale; + + collective_yaw_scale.char_ptr = (char*)motors + AP_MotorsHeli_Single::var_info[7].offset; + collective_yaw_scale.ap_float->set(value); + + } else if (strcmp(cmd,"autorotation") == 0) { + if (motors_heli == nullptr) { + ::printf("autorotation only supported by heli frame types, got %i\n", frame_class); + exit(1); + } + motors_heli->set_in_autorotation(!is_zero(value)); + } else { ::printf("Expected \"frame_class\", \"yaw_headroom\" or \"throttle_avg_max\"\n"); exit(1); diff --git a/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py b/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py index 118a5de193fcb..7136c65fef806 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py +++ b/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py @@ -100,8 +100,15 @@ def get_fields(self): 1: 'Transverse', 2: 'Intermeshing_or_Coaxial'} +tail_type_lookup = {0: 'Servo_only', + 1: 'Servo_with_ExtGyro', + 2: 'DirectDrive_VarPitch', + 3: 'DirectDrive_FixedPitch_CW', + 4: 'DirectDrive_FixedPitch_CCW', + 5: 'DDVP_with_external_governor'} + # Run sweep over range of types -def run_sweep(frame_class, swash_type, dual_mode, dir_name): +def run_sweep(frame_class, swash_type, secondary_iter, secondary_lookup, secondary_name, dir_name): # configure and build the test os.system('./waf configure --board linux') @@ -110,28 +117,28 @@ def run_sweep(frame_class, swash_type, dual_mode, dir_name): # Run sweep for fc in frame_class: for swash in swash_type: - for mode in dual_mode: + for sec in secondary_iter: if swash is not None: swash_cmd = 'swash=%d' % swash - if mode is not None: - name = 'frame class = %s (%i), swash = %s (%i), dual mode = %s (%i)' % (frame_class_lookup[fc], fc, swash_type_lookup[swash], swash, dual_mode_lookup[mode], mode) - filename = '%s_%s_%s_motor_test.csv' % (frame_class_lookup[fc], swash_type_lookup[swash], dual_mode_lookup[mode]) - mode_cmd = 'dual_mode=%d' % mode + if sec is not None: + name = 'frame class = %s (%i), swash = %s (%i), %s = %s (%i)' % (frame_class_lookup[fc], fc, swash_type_lookup[swash], swash, secondary_name.replace('_', ' '), secondary_lookup[sec], sec) + filename = '%s_%s_%s_motor_test.csv' % (frame_class_lookup[fc], swash_type_lookup[swash], secondary_lookup[sec]) + sec_cmd = '%s=%d' % (secondary_name, sec) else: name = 'frame class = %s (%i), swash = %s (%i)' % (frame_class_lookup[fc], fc, swash_type_lookup[swash], swash) filename = '%s_%s_motor_test.csv' % (frame_class_lookup[fc], swash_type_lookup[swash]) - mode_cmd = '' + sec_cmd = '' else: name = 'frame class = %s (%i)' % (frame_class_lookup[fc], fc) filename = '%s_motor_test.csv' % (frame_class_lookup[fc]) swash_cmd = '' - mode_cmd = '' + sec_cmd = '' print('Running motors test for %s' % name) - os.system('./build/linux/examples/AP_Motors_test s frame_class=%d %s %s > %s/%s' % (fc, swash_cmd, mode_cmd, dir_name, filename)) + os.system('./build/linux/examples/AP_Motors_test s frame_class=%d %s %s > %s/%s' % (fc, swash_cmd, sec_cmd, dir_name, filename)) print('%s complete\n' % name) @@ -149,6 +156,7 @@ def run_sweep(frame_class, swash_type, dual_mode, dir_name): parser.add_argument("-c", "--compare", action='store_true', help='Compare only, do not re-run tests') parser.add_argument("-p", "--plot", action='store_true', help='Plot comparison results') parser.add_argument("-m", "--dual_mode", type=int, dest='dual_mode', help='Set DUAL_MODE, 0:Longitudinal, 1:Transverse, 2:Intermeshing/Coaxial, default:run all') + parser.add_argument("-t", "--tail_type", type=int, dest='tail_type', help='Set TAIL_TYPE, 0:Servo Only, 1:Servo with ExtGyro, 2:DirectDrive VarPitch, 3:DirectDrive FixedPitch CW, 4:DirectDrive FixedPitch CCW, 5:DDVP with external governor, default:run all') args = parser.parse_args() if 13 in args.frame_class: @@ -171,6 +179,30 @@ def run_sweep(frame_class, swash_type, dual_mode, dir_name): else: args.dual_mode = [None] + if (args.frame_class != [6]) and (args.tail_type is not None): + print('Only Frame %s (%i) supports tail_type' % (frame_class_lookup[6], 6)) + quit() + + if args.frame_class == [6]: + if args.tail_type is None: + args.tail_type = (0, 1, 2, 3, 4, 5) + else: + args.tail_type = [None] + + # Secondary iterator, tail type for single heli and dual mode for dual heli + secondary_iter = [None] + secondary_lookup = None + secondary_name = None + if args.dual_mode != [None]: + secondary_iter = args.dual_mode + secondary_lookup = dual_mode_lookup + secondary_name = 'dual_mode' + + elif args.tail_type != [None]: + secondary_iter = args.tail_type + secondary_lookup = tail_type_lookup + secondary_name = 'tail_type' + dir_name = 'motors_comparison' if not args.compare: @@ -185,7 +217,7 @@ def run_sweep(frame_class, swash_type, dual_mode, dir_name): print('\nRunning motor tests with current changes\n') # run the test - run_sweep(args.frame_class, args.swash_type, args.dual_mode, new_name) + run_sweep(args.frame_class, args.swash_type, secondary_iter, secondary_lookup, secondary_name, new_name) if args.head: # rewind head and repeat test @@ -237,14 +269,14 @@ def run_sweep(frame_class, swash_type, dual_mode, dir_name): # Print comparison for fc in args.frame_class: for sw in args.swash_type: - for dm in args.dual_mode: + for sec in secondary_iter: frame = frame_class_lookup[fc] if sw is not None: swash = swash_type_lookup[sw] - if dm is not None: - mode = dual_mode_lookup[dm] - name = frame + ' ' + swash + ' ' + mode - filename = '%s_%s_%s_motor_test.csv' % (frame, swash, mode) + if sec is not None: + sec_str = secondary_lookup[sec] + name = frame + ' ' + swash + ' ' + sec_str + filename = '%s_%s_%s_motor_test.csv' % (frame, swash, sec_str) else: name = frame + ' ' + swash