Browse Source

AP_Motors:TradHeli - DDVP Ramp/Runup

Change to ramp and runup DDVP tail rotor to prevent torque pitching the frame and provide runup in sync with main
rotor like a mechanically driven tail.

Fix some comments and remove indents found in whitespace in AP_MotorsHeli.cpp and AP_MotorsHeli.h
mission-4.1.18
ChristopherOlson 7 years ago committed by Randy Mackay
parent
commit
96793a3ae7
  1. 2
      libraries/AP_Motors/AP_MotorsHeli.h
  2. 10
      libraries/AP_Motors/AP_MotorsHeli_Single.cpp
  3. 6
      libraries/AP_Motors/AP_MotorsHeli_Single.h

2
libraries/AP_Motors/AP_MotorsHeli.h

@ -196,7 +196,7 @@ protected:
AP_Int8 _servo_mode; // Pass radio inputs directly to servos during set-up through mission planner AP_Int8 _servo_mode; // Pass radio inputs directly to servos during set-up through mission planner
AP_Int16 _rsc_setpoint; // rotor speed when RSC mode is set to is enabledv AP_Int16 _rsc_setpoint; // rotor speed when RSC mode is set to is enabledv
AP_Int8 _rsc_mode; // Which main rotor ESC control mode is active AP_Int8 _rsc_mode; // Which main rotor ESC control mode is active
AP_Int8 _rsc_ramp_time; // Time in seconds for the output to the main rotor's ESC to reach full speed AP_Int8 _rsc_ramp_time; // Time in seconds for the output to the main rotor's ESC to reach setpoint
AP_Int8 _rsc_runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time AP_Int8 _rsc_runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time
AP_Int16 _land_collective_min; // Minimum collective when landed or landing 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_critical; // Rotor speed below which flight is not possible

10
libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@ -105,7 +105,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @Units: PWM // @Units: PWM
// @Increment: 1 // @Increment: 1
// @User: Standard // @User: Standard
AP_GROUPINFO("TAIL_SPEED", 10, AP_MotorsHeli_Single, _direct_drive_tailspeed, AP_MOTORS_HELI_SINGLE_DDVPT_SPEED_DEFAULT), AP_GROUPINFO("TAIL_SPEED", 10, AP_MotorsHeli_Single, _direct_drive_tailspeed, AP_MOTORS_HELI_SINGLE_DDVP_SPEED_DEFAULT),
// @Param: GYR_GAIN_ACRO // @Param: GYR_GAIN_ACRO
// @DisplayName: External Gyro Gain for ACRO // @DisplayName: External Gyro Gain for ACRO
@ -222,7 +222,7 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(float desired_speed)
{ {
_main_rotor.set_desired_speed(desired_speed); _main_rotor.set_desired_speed(desired_speed);
// always send desired speed to tail rotor control, will do nothing if not DDVPT not enabled // 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/1000.0f);
} }
@ -257,11 +257,11 @@ void AP_MotorsHeli_Single::calculate_scalars()
_main_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get())); _main_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));
calculate_armed_scalars(); calculate_armed_scalars();
// send setpoints to tail rotor controller and trigger recalculation of scalars // send setpoints to DDVP rotor controller and trigger recalculation of scalars
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_SPEED_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_ramp_time(_rsc_ramp_time);
_tail_rotor.set_runup_time(AP_MOTORS_HELI_SINGLE_DDVPT_RUNUP_TIME); _tail_rotor.set_runup_time(_rsc_runup_time);
_tail_rotor.set_critical_speed(_rsc_critical/1000.0f); _tail_rotor.set_critical_speed(_rsc_critical/1000.0f);
_tail_rotor.set_idle_output(_rsc_idle_output/1000.0f); _tail_rotor.set_idle_output(_rsc_idle_output/1000.0f);
} else { } else {

6
libraries/AP_Motors/AP_MotorsHeli_Single.h

@ -27,10 +27,8 @@
#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH 2 #define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH 2
#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH 3 #define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH 3
// default direct-drive variable pitch tail defaults // direct-drive variable pitch defaults
#define AP_MOTORS_HELI_SINGLE_DDVPT_SPEED_DEFAULT 500 #define AP_MOTORS_HELI_SINGLE_DDVP_SPEED_DEFAULT 500
#define AP_MOTORS_HELI_SINGLE_DDVPT_RAMP_TIME 2
#define AP_MOTORS_HELI_SINGLE_DDVPT_RUNUP_TIME 3
// default external gyro gain // default external gyro gain
#define AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN 350 #define AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN 350

Loading…
Cancel
Save