diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 138132a0f1..e8d6d98129 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -93,7 +93,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Description: Determines the method of rotor speed control // @Values: 1:Ch8 Input, 2:SetPoint, 3:Throttle Curve // @User: Standard - AP_GROUPINFO("RSC_MODE", 8, AP_MotorsHeli, _rsc_mode, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH), + AP_GROUPINFO("RSC_MODE", 8, AP_MotorsHeli, _rsc_mode, (int8_t)ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH), // @Param: LAND_COL_MIN // @DisplayName: Landing Collective Minimum @@ -280,7 +280,7 @@ bool AP_MotorsHeli::parameter_check() const } // returns false if RSC Mode is not set to a valid control mode - if (_rsc_mode <= AP_MOTORS_HELI_RSC_MODE_DISABLED || _rsc_mode > AP_MOTORS_HELI_RSC_MODE_THROTTLE_CURVE) { + if (_rsc_mode <= (int8_t)ROTOR_CONTROL_MODE_DISABLED || _rsc_mode > (int8_t)ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT) { return false; } diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index dfbd09e9e7..b404fb7686 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -36,12 +36,6 @@ // swash min while landed or landing (as a number from 0 ~ 1000 #define AP_MOTORS_HELI_LAND_COLLECTIVE_MIN 0 -// main rotor speed control types (ch8 out) -#define AP_MOTORS_HELI_RSC_MODE_DISABLED 0 // not a valid RSC Mode -#define AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH 1 // main rotor ESC is connected to RC8 (out), pilot desired rotor speed provided by CH8 input -#define AP_MOTORS_HELI_RSC_MODE_SETPOINT 2 // main rotor ESC is connected to RC8 (out), desired speed is held in RSC_SETPOINT parameter -#define AP_MOTORS_HELI_RSC_MODE_THROTTLE_CURVE 3 // main rotor speed is controlled open-loop by a throttle servo or ESC connected to RC8(out) - // default main rotor speed (ch8 out) as a number from 0 ~ 1000 #define AP_MOTORS_HELI_RSC_SETPOINT 700 @@ -178,7 +172,7 @@ protected: void output_disarmed(); // update_motor_controls - sends commands to motor controllers - virtual void update_motor_control(uint8_t state) = 0; + virtual void update_motor_control(RotorControlState state) = 0; // reset_flight_controls - resets all controls and scalars to flight status void reset_flight_controls(); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 2d97ab980b..2449dc590a 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -241,7 +241,7 @@ void AP_MotorsHeli_Single::calculate_scalars() calculate_roll_pitch_collective_factors(); // send setpoints to main rotor controller and trigger recalculation of scalars - _main_rotor.set_control_mode(_rsc_mode); + _main_rotor.set_control_mode(static_cast(_rsc_mode.get())); _main_rotor.set_ramp_time(_rsc_ramp_time); _main_rotor.set_runup_time(_rsc_runup_time); _main_rotor.set_critical_speed(_rsc_critical); @@ -251,13 +251,13 @@ void AP_MotorsHeli_Single::calculate_scalars() // send setpoints to tail rotor controller and trigger recalculation of scalars if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { - _tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_SETPOINT); + _tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_SPEED_SETPOINT); _tail_rotor.set_ramp_time(AP_MOTORS_HELI_SINGLE_DDVPT_RAMP_TIME); _tail_rotor.set_runup_time(AP_MOTORS_HELI_SINGLE_DDVPT_RUNUP_TIME); _tail_rotor.set_critical_speed(_rsc_critical); _tail_rotor.set_idle_output(_rsc_idle_output); } else { - _tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_DISABLED); + _tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_DISABLED); _tail_rotor.set_ramp_time(0); _tail_rotor.set_runup_time(0); _tail_rotor.set_critical_speed(0); @@ -314,7 +314,7 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask() } // update_motor_controls - sends commands to motor controllers -void AP_MotorsHeli_Single::update_motor_control(uint8_t state) +void AP_MotorsHeli_Single::update_motor_control(RotorControlState state) { // Send state update to motors _tail_rotor.output(state); @@ -470,4 +470,4 @@ void AP_MotorsHeli_Single::write_aux(int16_t servo_out) _servo_aux.servo_out = servo_out; _servo_aux.calc_pwm(); hal.rcout->write(AP_MOTORS_HELI_SINGLE_AUX, _servo_aux.radio_out); -} \ No newline at end of file +} diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 5caf5d341a..e06217ef61 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -131,7 +131,7 @@ protected: void init_outputs(); // update_motor_controls - sends commands to motor controllers - void update_motor_control(uint8_t state); + void update_motor_control(RotorControlState state); // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position void calculate_roll_pitch_collective_factors();