mirror of https://github.com/ArduPilot/ardupilot
AP_MotorsHeli: Rework how servo setup is done.
This commit is contained in:
parent
bac559d5af
commit
e23e57cc16
|
@ -165,14 +165,14 @@ void AP_MotorsHeli::Init()
|
||||||
// set update rate
|
// set update rate
|
||||||
set_update_rate(_speed_hz);
|
set_update_rate(_speed_hz);
|
||||||
|
|
||||||
// ensure inputs are not passed through to servos
|
// ensure inputs are not passed through to servos on start-up
|
||||||
_servo_manual = 0;
|
_servo_manual = 0;
|
||||||
|
|
||||||
// initialise some scalers
|
|
||||||
recalc_scalers();
|
|
||||||
|
|
||||||
// initialise swash plate
|
// initialise swash plate
|
||||||
init_swash();
|
init_outputs();
|
||||||
|
|
||||||
|
// calculate all scalars
|
||||||
|
calculate_scalars();
|
||||||
}
|
}
|
||||||
|
|
||||||
// output - sends commands to the servos
|
// output - sends commands to the servos
|
||||||
|
@ -221,65 +221,16 @@ bool AP_MotorsHeli::parameter_check() const
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// reset_swash - free up swash for maximum movements. Used for set-up
|
|
||||||
void AP_MotorsHeli::reset_swash()
|
|
||||||
{
|
|
||||||
// free up servo ranges
|
|
||||||
reset_servos();
|
|
||||||
|
|
||||||
// calculate factors based on swash type and servo position
|
|
||||||
calculate_roll_pitch_collective_factors();
|
|
||||||
|
|
||||||
// set roll, pitch and throttle scaling
|
|
||||||
_roll_scaler = 1.0f;
|
|
||||||
_pitch_scaler = 1.0f;
|
|
||||||
_collective_scalar = ((float)(_throttle_radio_max - _throttle_radio_min))/1000.0f;
|
|
||||||
_collective_scalar_manual = 1.0f;
|
|
||||||
|
|
||||||
// we must be in set-up mode so mark swash as uninitialised
|
|
||||||
_heliflags.swash_initialised = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// reset_swash_servo
|
// reset_swash_servo
|
||||||
void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo)
|
void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo)
|
||||||
{
|
{
|
||||||
servo.set_range(0, 1000);
|
servo.set_range(0, 1000);
|
||||||
|
|
||||||
|
// swash servos always use full endpoints as restricting them would lead to scaling errors
|
||||||
servo.radio_min = 1000;
|
servo.radio_min = 1000;
|
||||||
servo.radio_max = 2000;
|
servo.radio_max = 2000;
|
||||||
}
|
}
|
||||||
|
|
||||||
// init_swash - initialise the swash plate
|
|
||||||
void AP_MotorsHeli::init_swash()
|
|
||||||
{
|
|
||||||
|
|
||||||
// swash servo initialisation
|
|
||||||
init_servos();
|
|
||||||
|
|
||||||
// range check collective min, max and mid
|
|
||||||
if( _collective_min >= _collective_max ) {
|
|
||||||
_collective_min = 1000;
|
|
||||||
_collective_max = 2000;
|
|
||||||
}
|
|
||||||
_collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
|
|
||||||
|
|
||||||
// calculate collective mid point as a number from 0 to 1000
|
|
||||||
_collective_mid_pwm = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min))*1000.0f;
|
|
||||||
|
|
||||||
// calculate maximum collective pitch range from full positive pitch to zero pitch
|
|
||||||
_collective_range = 1000 - _collective_mid_pwm;
|
|
||||||
|
|
||||||
// determine roll, pitch and collective input scaling
|
|
||||||
_roll_scaler = (float)_roll_max/4500.0f;
|
|
||||||
_pitch_scaler = (float)_pitch_max/4500.0f;
|
|
||||||
_collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f;
|
|
||||||
|
|
||||||
// calculate factors based on swash type and servo position
|
|
||||||
calculate_roll_pitch_collective_factors();
|
|
||||||
|
|
||||||
// mark swash as initialised
|
|
||||||
_heliflags.swash_initialised = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// update the throttle input filter
|
// update the throttle input filter
|
||||||
void AP_MotorsHeli::update_throttle_filter()
|
void AP_MotorsHeli::update_throttle_filter()
|
||||||
{
|
{
|
||||||
|
|
|
@ -75,7 +75,6 @@ public:
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
|
|
||||||
// initialise flags
|
// initialise flags
|
||||||
_heliflags.swash_initialised = 0;
|
|
||||||
_heliflags.landing_collective = 0;
|
_heliflags.landing_collective = 0;
|
||||||
_heliflags.rotor_runup_complete = 0;
|
_heliflags.rotor_runup_complete = 0;
|
||||||
};
|
};
|
||||||
|
@ -144,8 +143,8 @@ public:
|
||||||
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
|
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
|
||||||
virtual bool rotor_speed_above_critical() const = 0;
|
virtual bool rotor_speed_above_critical() const = 0;
|
||||||
|
|
||||||
// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
|
// calculate_scalars - must be implemented by child classes
|
||||||
virtual void recalc_scalers() = 0;
|
virtual void calculate_scalars() = 0;
|
||||||
|
|
||||||
// var_info for holding Parameter information
|
// var_info for holding Parameter information
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
@ -181,30 +180,20 @@ protected:
|
||||||
// update the throttle input filter
|
// update the throttle input filter
|
||||||
void update_throttle_filter();
|
void update_throttle_filter();
|
||||||
|
|
||||||
// heli_move_swash - moves swash plate to attitude of parameters passed in
|
// move_actuators - moves swash plate and tail rotor
|
||||||
virtual void move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) = 0;
|
virtual void move_actuators(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) = 0;
|
||||||
|
|
||||||
// reset_swash - free up swash for maximum movements. Used for set-up
|
|
||||||
void reset_swash();
|
|
||||||
|
|
||||||
// reset_servos - free up the swash servos for maximum movement
|
|
||||||
virtual void reset_servos() = 0;
|
|
||||||
|
|
||||||
// reset_swash_servo - free up swash servo for maximum movement
|
// reset_swash_servo - free up swash servo for maximum movement
|
||||||
static void reset_swash_servo(RC_Channel& servo);
|
static void reset_swash_servo(RC_Channel& servo);
|
||||||
|
|
||||||
// init_swash - initialise the swash plate
|
// init_outputs - initialise Servo/PWM ranges and endpoints
|
||||||
void init_swash();
|
virtual void init_outputs() = 0;
|
||||||
|
|
||||||
// init_servos - initialize the servos
|
|
||||||
virtual void init_servos() = 0;
|
|
||||||
|
|
||||||
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
||||||
virtual void calculate_roll_pitch_collective_factors() = 0;
|
virtual void calculate_roll_pitch_collective_factors() = 0;
|
||||||
|
|
||||||
// flags bitmask
|
// flags bitmask
|
||||||
struct heliflags_type {
|
struct heliflags_type {
|
||||||
uint8_t swash_initialised : 1; // true if swash has been initialised
|
|
||||||
uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum
|
uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum
|
||||||
uint8_t rotor_runup_complete : 1; // true if the rotors have had enough time to wind up
|
uint8_t rotor_runup_complete : 1; // true if the rotors have had enough time to wind up
|
||||||
} _heliflags;
|
} _heliflags;
|
||||||
|
@ -233,7 +222,6 @@ protected:
|
||||||
float _roll_scaler = 1; // scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range
|
float _roll_scaler = 1; // scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range
|
||||||
float _pitch_scaler = 1; // scaler to convert pitch input from radio (i.e. -4500 ~ 4500) to max pitch range
|
float _pitch_scaler = 1; // scaler to convert pitch input from radio (i.e. -4500 ~ 4500) to max pitch range
|
||||||
float _collective_scalar = 1; // collective scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
|
float _collective_scalar = 1; // collective scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500)
|
||||||
float _collective_scalar_manual = 1; // collective scalar to reduce the range of the collective movement while collective is being controlled manually (i.e. directly by the pilot)
|
|
||||||
int16_t _collective_out = 0; // actual collective pitch value. Required by the main code for calculating cruise throttle
|
int16_t _collective_out = 0; // actual collective pitch value. Required by the main code for calculating cruise throttle
|
||||||
int16_t _collective_mid_pwm = 0; // collective mid parameter value converted to pwm form (i.e. 0 ~ 1000)
|
int16_t _collective_mid_pwm = 0; // collective mid parameter value converted to pwm form (i.e. 0 ~ 1000)
|
||||||
int16_t _delta_phase_angle = 0; // phase angle dynamic compensation
|
int16_t _delta_phase_angle = 0; // phase angle dynamic compensation
|
||||||
|
|
|
@ -108,21 +108,6 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] PROGMEM = {
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
//
|
|
||||||
// public methods
|
|
||||||
//
|
|
||||||
|
|
||||||
// init
|
|
||||||
void AP_MotorsHeli_Single::Init()
|
|
||||||
{
|
|
||||||
AP_MotorsHeli::Init();
|
|
||||||
|
|
||||||
// disable channels 7 and 8 from being used by RC_Channel_aux
|
|
||||||
RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_SINGLE_AUX]);
|
|
||||||
RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_SINGLE_RSC]);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// set update rate to motors - a value in hertz
|
// set update rate to motors - a value in hertz
|
||||||
void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
|
void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
|
||||||
{
|
{
|
||||||
|
@ -138,7 +123,7 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
|
||||||
hal.rcout->set_freq(mask, _speed_hz);
|
hal.rcout->set_freq(mask, _speed_hz);
|
||||||
}
|
}
|
||||||
|
|
||||||
// enable - starts allowing signals to be sent to motors
|
// enable - starts allowing signals to be sent to motors and servos
|
||||||
void AP_MotorsHeli_Single::enable()
|
void AP_MotorsHeli_Single::enable()
|
||||||
{
|
{
|
||||||
// enable output channels
|
// enable output channels
|
||||||
|
@ -146,8 +131,27 @@ void AP_MotorsHeli_Single::enable()
|
||||||
hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2])); // swash servo 2
|
hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2])); // swash servo 2
|
||||||
hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3])); // swash servo 3
|
hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3])); // swash servo 3
|
||||||
hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4])); // yaw
|
hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4])); // yaw
|
||||||
hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_AUX); // output for gyro gain or direct drive variable pitch tail motor
|
hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_AUX); // output for gyro gain or direct drive variable pitch tail motor
|
||||||
hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_RSC); // output for main rotor esc
|
hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_RSC); // output for main rotor esc
|
||||||
|
|
||||||
|
// disable channels 7 and 8 from being used by RC_Channel_aux
|
||||||
|
RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_SINGLE_AUX]);
|
||||||
|
RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_SINGLE_RSC]);
|
||||||
|
}
|
||||||
|
|
||||||
|
// init_outputs - initialise Servo/PWM ranges and endpoints
|
||||||
|
void AP_MotorsHeli_Single::init_outputs()
|
||||||
|
{
|
||||||
|
// reset swash servo range and endpoints
|
||||||
|
reset_swash_servo (_swash_servo_1);
|
||||||
|
reset_swash_servo (_swash_servo_2);
|
||||||
|
reset_swash_servo (_swash_servo_3);
|
||||||
|
|
||||||
|
_yaw_servo.set_angle(4500);
|
||||||
|
|
||||||
|
// set main rotor servo range
|
||||||
|
// tail rotor servo use range as set in vehicle code for rc7
|
||||||
|
_main_rotor.init_servo();
|
||||||
}
|
}
|
||||||
|
|
||||||
// output_test - spin a motor at the pwm value specified
|
// output_test - spin a motor at the pwm value specified
|
||||||
|
@ -216,9 +220,31 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(int16_t desired_speed)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// recalc_scalers - recalculates various scalers used.
|
// calculate_scalars - recalculates various scalers used.
|
||||||
void AP_MotorsHeli_Single::recalc_scalers()
|
void AP_MotorsHeli_Single::calculate_scalars()
|
||||||
{
|
{
|
||||||
|
// range check collective min, max and mid
|
||||||
|
if( _collective_min >= _collective_max ) {
|
||||||
|
_collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN;
|
||||||
|
_collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX;
|
||||||
|
}
|
||||||
|
_collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
|
||||||
|
|
||||||
|
// calculate collective mid point as a number from 0 to 1000
|
||||||
|
_collective_mid_pwm = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min))*1000.0f;
|
||||||
|
|
||||||
|
// calculate maximum collective pitch range from full positive pitch to zero pitch
|
||||||
|
_collective_range = 1000 - _collective_mid_pwm;
|
||||||
|
|
||||||
|
// determine roll, pitch and collective input scaling
|
||||||
|
_roll_scaler = (float)_roll_max/4500.0f;
|
||||||
|
_pitch_scaler = (float)_pitch_max/4500.0f;
|
||||||
|
_collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f;
|
||||||
|
|
||||||
|
// calculate factors based on swash type and servo position
|
||||||
|
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(_rsc_mode);
|
||||||
_main_rotor.set_ramp_time(_rsc_ramp_time);
|
_main_rotor.set_ramp_time(_rsc_ramp_time);
|
||||||
_main_rotor.set_runup_time(_rsc_runup_time);
|
_main_rotor.set_runup_time(_rsc_runup_time);
|
||||||
|
@ -227,6 +253,7 @@ void AP_MotorsHeli_Single::recalc_scalers()
|
||||||
_main_rotor.set_power_output_range(_rsc_power_low, _rsc_power_high);
|
_main_rotor.set_power_output_range(_rsc_power_low, _rsc_power_high);
|
||||||
_main_rotor.recalc_scalers();
|
_main_rotor.recalc_scalers();
|
||||||
|
|
||||||
|
// send setpoints to tail rotor controller and trigger recalculation of scalars
|
||||||
if (_rsc_mode == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
if (_rsc_mode == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
||||||
_tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_SETPOINT);
|
_tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_SETPOINT);
|
||||||
_tail_rotor.set_ramp_time(_rsc_ramp_time);
|
_tail_rotor.set_ramp_time(_rsc_ramp_time);
|
||||||
|
@ -243,151 +270,6 @@ void AP_MotorsHeli_Single::recalc_scalers()
|
||||||
_tail_rotor.recalc_scalers();
|
_tail_rotor.recalc_scalers();
|
||||||
}
|
}
|
||||||
|
|
||||||
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
|
|
||||||
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
|
||||||
uint16_t AP_MotorsHeli_Single::get_motor_mask()
|
|
||||||
{
|
|
||||||
// heli uses channels 1,2,3,4,7 and 8
|
|
||||||
return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC);
|
|
||||||
}
|
|
||||||
|
|
||||||
// output_min - sets servos to neutral point
|
|
||||||
void AP_MotorsHeli_Single::output_min()
|
|
||||||
{
|
|
||||||
// move swash to mid
|
|
||||||
move_swash(0,0,500,0);
|
|
||||||
|
|
||||||
_main_rotor.output(ROTOR_CONTROL_STOP);
|
|
||||||
_tail_rotor.output(ROTOR_CONTROL_STOP);
|
|
||||||
|
|
||||||
// override limits flags
|
|
||||||
limit.roll_pitch = true;
|
|
||||||
limit.yaw = true;
|
|
||||||
limit.throttle_lower = true;
|
|
||||||
limit.throttle_upper = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// sends commands to the motors
|
|
||||||
void AP_MotorsHeli_Single::output_armed_stabilizing()
|
|
||||||
{
|
|
||||||
// if manual override active after arming, deactivate it.
|
|
||||||
if (_servo_manual == 1) {
|
|
||||||
reset_radio_passthrough();
|
|
||||||
_servo_manual = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
|
|
||||||
|
|
||||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
|
||||||
_tail_rotor.output(ROTOR_CONTROL_ACTIVE);
|
|
||||||
|
|
||||||
if (!_tail_rotor.is_runup_complete())
|
|
||||||
{
|
|
||||||
_heliflags.rotor_runup_complete = false;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
_main_rotor.output(ROTOR_CONTROL_ACTIVE);
|
|
||||||
|
|
||||||
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
|
|
||||||
}
|
|
||||||
|
|
||||||
void AP_MotorsHeli_Single::output_armed_not_stabilizing()
|
|
||||||
{
|
|
||||||
// if manual override active after arming, deactivate it.
|
|
||||||
if (_servo_manual == 1) {
|
|
||||||
reset_radio_passthrough();
|
|
||||||
_servo_manual = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
|
|
||||||
|
|
||||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
|
||||||
_tail_rotor.output(ROTOR_CONTROL_ACTIVE);
|
|
||||||
|
|
||||||
if (!_tail_rotor.is_runup_complete())
|
|
||||||
{
|
|
||||||
_heliflags.rotor_runup_complete = false;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
_main_rotor.output(ROTOR_CONTROL_ACTIVE);
|
|
||||||
|
|
||||||
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
|
|
||||||
}
|
|
||||||
|
|
||||||
// output_armed_zero_throttle - sends commands to the motors
|
|
||||||
void AP_MotorsHeli_Single::output_armed_zero_throttle()
|
|
||||||
{
|
|
||||||
// if manual override active after arming, deactivate it.
|
|
||||||
if (_servo_manual == 1) {
|
|
||||||
reset_radio_passthrough();
|
|
||||||
_servo_manual = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
|
|
||||||
|
|
||||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
|
||||||
_tail_rotor.output(ROTOR_CONTROL_IDLE);
|
|
||||||
|
|
||||||
if (!_tail_rotor.is_runup_complete())
|
|
||||||
{
|
|
||||||
_heliflags.rotor_runup_complete = false;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
_main_rotor.output(ROTOR_CONTROL_IDLE);
|
|
||||||
|
|
||||||
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// output_disarmed - sends commands to the motors
|
|
||||||
void AP_MotorsHeli_Single::output_disarmed()
|
|
||||||
{
|
|
||||||
// if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
|
|
||||||
if (_servo_manual == 1) {
|
|
||||||
_roll_control_input = _roll_radio_passthrough;
|
|
||||||
_pitch_control_input = _pitch_radio_passthrough;
|
|
||||||
_throttle_control_input = _throttle_radio_passthrough;
|
|
||||||
_yaw_control_input = _yaw_radio_passthrough;
|
|
||||||
}
|
|
||||||
|
|
||||||
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
|
|
||||||
|
|
||||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
|
||||||
_tail_rotor.output(ROTOR_CONTROL_STOP);
|
|
||||||
}
|
|
||||||
|
|
||||||
_main_rotor.output(ROTOR_CONTROL_STOP);
|
|
||||||
|
|
||||||
_heliflags.rotor_runup_complete = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// reset_servos
|
|
||||||
void AP_MotorsHeli_Single::reset_servos()
|
|
||||||
{
|
|
||||||
reset_swash_servo (_swash_servo_1);
|
|
||||||
reset_swash_servo (_swash_servo_2);
|
|
||||||
reset_swash_servo (_swash_servo_3);
|
|
||||||
}
|
|
||||||
|
|
||||||
// init_servos
|
|
||||||
void AP_MotorsHeli_Single::init_servos()
|
|
||||||
{
|
|
||||||
// reset swash servo range and endpoints
|
|
||||||
reset_servos();
|
|
||||||
|
|
||||||
_yaw_servo.set_angle(4500);
|
|
||||||
|
|
||||||
// set main rotor servo range
|
|
||||||
// tail rotor servo use range as set in vehicle code for rc7
|
|
||||||
_main_rotor.init_servo();
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
||||||
void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
|
void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
|
||||||
{
|
{
|
||||||
|
@ -427,6 +309,141 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
|
||||||
|
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
||||||
|
uint16_t AP_MotorsHeli_Single::get_motor_mask()
|
||||||
|
{
|
||||||
|
// heli uses channels 1,2,3,4,7 and 8
|
||||||
|
return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC);
|
||||||
|
}
|
||||||
|
|
||||||
|
// output_min - sets servos to neutral point
|
||||||
|
void AP_MotorsHeli_Single::output_min()
|
||||||
|
{
|
||||||
|
// move swash to mid
|
||||||
|
move_actuators(0,0,500,0);
|
||||||
|
|
||||||
|
_main_rotor.output(ROTOR_CONTROL_STOP);
|
||||||
|
_tail_rotor.output(ROTOR_CONTROL_STOP);
|
||||||
|
|
||||||
|
// override limits flags
|
||||||
|
limit.roll_pitch = true;
|
||||||
|
limit.yaw = true;
|
||||||
|
limit.throttle_lower = true;
|
||||||
|
limit.throttle_upper = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// sends commands to the motors
|
||||||
|
void AP_MotorsHeli_Single::output_armed_stabilizing()
|
||||||
|
{
|
||||||
|
// if manual override active after arming, deactivate it and reinitialize servos
|
||||||
|
if (_servo_manual == 1) {
|
||||||
|
reset_radio_passthrough();
|
||||||
|
_servo_manual = 0;
|
||||||
|
init_outputs();
|
||||||
|
calculate_scalars();
|
||||||
|
}
|
||||||
|
|
||||||
|
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
|
||||||
|
|
||||||
|
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
||||||
|
_tail_rotor.output(ROTOR_CONTROL_ACTIVE);
|
||||||
|
|
||||||
|
if (!_tail_rotor.is_runup_complete())
|
||||||
|
{
|
||||||
|
_heliflags.rotor_runup_complete = false;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_main_rotor.output(ROTOR_CONTROL_ACTIVE);
|
||||||
|
|
||||||
|
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
|
||||||
|
}
|
||||||
|
|
||||||
|
void AP_MotorsHeli_Single::output_armed_not_stabilizing()
|
||||||
|
{
|
||||||
|
// if manual override active after arming, deactivate it and reinitialize servos
|
||||||
|
if (_servo_manual == 1) {
|
||||||
|
reset_radio_passthrough();
|
||||||
|
_servo_manual = 0;
|
||||||
|
init_outputs();
|
||||||
|
calculate_scalars();
|
||||||
|
}
|
||||||
|
|
||||||
|
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
|
||||||
|
|
||||||
|
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
||||||
|
_tail_rotor.output(ROTOR_CONTROL_ACTIVE);
|
||||||
|
|
||||||
|
if (!_tail_rotor.is_runup_complete())
|
||||||
|
{
|
||||||
|
_heliflags.rotor_runup_complete = false;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_main_rotor.output(ROTOR_CONTROL_ACTIVE);
|
||||||
|
|
||||||
|
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
|
||||||
|
}
|
||||||
|
|
||||||
|
// output_armed_zero_throttle - sends commands to the motors
|
||||||
|
void AP_MotorsHeli_Single::output_armed_zero_throttle()
|
||||||
|
{
|
||||||
|
// if manual override active after arming, deactivate it and reinitialize servos
|
||||||
|
if (_servo_manual == 1) {
|
||||||
|
reset_radio_passthrough();
|
||||||
|
_servo_manual = 0;
|
||||||
|
init_outputs();
|
||||||
|
calculate_scalars();
|
||||||
|
}
|
||||||
|
|
||||||
|
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
|
||||||
|
|
||||||
|
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
||||||
|
_tail_rotor.output(ROTOR_CONTROL_IDLE);
|
||||||
|
|
||||||
|
if (!_tail_rotor.is_runup_complete())
|
||||||
|
{
|
||||||
|
_heliflags.rotor_runup_complete = false;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_main_rotor.output(ROTOR_CONTROL_IDLE);
|
||||||
|
|
||||||
|
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
|
||||||
|
}
|
||||||
|
|
||||||
|
// output_disarmed - sends commands to the motors
|
||||||
|
void AP_MotorsHeli_Single::output_disarmed()
|
||||||
|
{
|
||||||
|
// if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
|
||||||
|
if (_servo_manual == 1) {
|
||||||
|
_roll_control_input = _roll_radio_passthrough;
|
||||||
|
_pitch_control_input = _pitch_radio_passthrough;
|
||||||
|
_throttle_control_input = _throttle_radio_passthrough;
|
||||||
|
_yaw_control_input = _yaw_radio_passthrough;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ensure swash servo endpoints haven't been moved
|
||||||
|
init_outputs();
|
||||||
|
|
||||||
|
// continuously recalculate scalars to allow setup
|
||||||
|
calculate_scalars();
|
||||||
|
|
||||||
|
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
|
||||||
|
|
||||||
|
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
|
||||||
|
_tail_rotor.output(ROTOR_CONTROL_STOP);
|
||||||
|
}
|
||||||
|
|
||||||
|
_main_rotor.output(ROTOR_CONTROL_STOP);
|
||||||
|
|
||||||
|
_heliflags.rotor_runup_complete = false;
|
||||||
|
}
|
||||||
|
|
||||||
// set_delta_phase_angle for setting variable phase angle compensation and force
|
// set_delta_phase_angle for setting variable phase angle compensation and force
|
||||||
// recalculation of collective factors
|
// recalculation of collective factors
|
||||||
void AP_MotorsHeli_Single::set_delta_phase_angle(int16_t angle)
|
void AP_MotorsHeli_Single::set_delta_phase_angle(int16_t angle)
|
||||||
|
@ -437,14 +454,14 @@ void AP_MotorsHeli_Single::set_delta_phase_angle(int16_t angle)
|
||||||
}
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
// heli_move_swash - moves swash plate to attitude of parameters passed in
|
// move_actuators - moves swash plate and tail rotor
|
||||||
// - expected ranges:
|
// - expected ranges:
|
||||||
// roll : -4500 ~ 4500
|
// roll : -4500 ~ 4500
|
||||||
// pitch: -4500 ~ 4500
|
// pitch: -4500 ~ 4500
|
||||||
// collective: 0 ~ 1000
|
// collective: 0 ~ 1000
|
||||||
// yaw: -4500 ~ 4500
|
// yaw: -4500 ~ 4500
|
||||||
//
|
//
|
||||||
void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out)
|
void AP_MotorsHeli_Single::move_actuators(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out)
|
||||||
{
|
{
|
||||||
int16_t yaw_offset = 0;
|
int16_t yaw_offset = 0;
|
||||||
int16_t coll_out_scaled;
|
int16_t coll_out_scaled;
|
||||||
|
@ -455,67 +472,54 @@ void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16
|
||||||
limit.throttle_lower = false;
|
limit.throttle_lower = false;
|
||||||
limit.throttle_upper = false;
|
limit.throttle_upper = false;
|
||||||
|
|
||||||
if (_servo_manual == 1) { // are we in manual servo mode? (i.e. swash set-up mode)?
|
// rescale roll_out and pitch-out into the min and max ranges to provide linear motion
|
||||||
// check if we need to free up the swash
|
// across the input range instead of stopping when the input hits the constrain value
|
||||||
if (_heliflags.swash_initialised) {
|
// these calculations are based on an assumption of the user specified roll_max and pitch_max
|
||||||
reset_swash();
|
// coming into this equation at 4500 or less, and based on the original assumption of the
|
||||||
}
|
// total _servo_x.servo_out range being -4500 to 4500.
|
||||||
// To-Do: This equation seems to be wrong. It probably restricts swash movement so that swash setup doesn't work right.
|
roll_out = roll_out * _roll_scaler;
|
||||||
// _collective_scalar should probably not be used or set to 1?
|
if (roll_out < -_roll_max) {
|
||||||
coll_out_scaled = coll_in * _collective_scalar + _throttle_radio_min - 1000;
|
roll_out = -_roll_max;
|
||||||
}else{ // regular flight mode
|
limit.roll_pitch = true;
|
||||||
|
}
|
||||||
|
if (roll_out > _roll_max) {
|
||||||
|
roll_out = _roll_max;
|
||||||
|
limit.roll_pitch = true;
|
||||||
|
}
|
||||||
|
|
||||||
// check if we need to reinitialise the swash
|
// scale pitch and update limits
|
||||||
if (!_heliflags.swash_initialised) {
|
pitch_out = pitch_out * _pitch_scaler;
|
||||||
init_swash();
|
if (pitch_out < -_pitch_max) {
|
||||||
}
|
pitch_out = -_pitch_max;
|
||||||
|
limit.roll_pitch = true;
|
||||||
|
}
|
||||||
|
if (pitch_out > _pitch_max) {
|
||||||
|
pitch_out = _pitch_max;
|
||||||
|
limit.roll_pitch = true;
|
||||||
|
}
|
||||||
|
|
||||||
// rescale roll_out and pitch-out into the min and max ranges to provide linear motion
|
// constrain collective input
|
||||||
// across the input range instead of stopping when the input hits the constrain value
|
_collective_out = coll_in;
|
||||||
// these calculations are based on an assumption of the user specified roll_max and pitch_max
|
if (_collective_out <= 0) {
|
||||||
// coming into this equation at 4500 or less, and based on the original assumption of the
|
_collective_out = 0;
|
||||||
// total _servo_x.servo_out range being -4500 to 4500.
|
limit.throttle_lower = true;
|
||||||
roll_out = roll_out * _roll_scaler;
|
}
|
||||||
if (roll_out < -_roll_max) {
|
if (_collective_out >= 1000) {
|
||||||
roll_out = -_roll_max;
|
_collective_out = 1000;
|
||||||
limit.roll_pitch = true;
|
limit.throttle_upper = true;
|
||||||
}
|
}
|
||||||
if (roll_out > _roll_max) {
|
|
||||||
roll_out = _roll_max;
|
|
||||||
limit.roll_pitch = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// scale pitch and update limits
|
// ensure not below landed/landing collective
|
||||||
pitch_out = pitch_out * _pitch_scaler;
|
if (_heliflags.landing_collective && _collective_out < _land_collective_min) {
|
||||||
if (pitch_out < -_pitch_max) {
|
_collective_out = _land_collective_min;
|
||||||
pitch_out = -_pitch_max;
|
limit.throttle_lower = true;
|
||||||
limit.roll_pitch = true;
|
}
|
||||||
}
|
|
||||||
if (pitch_out > _pitch_max) {
|
|
||||||
pitch_out = _pitch_max;
|
|
||||||
limit.roll_pitch = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// constrain collective input
|
// scale collective pitch
|
||||||
_collective_out = coll_in;
|
coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000;
|
||||||
if (_collective_out <= 0) {
|
|
||||||
_collective_out = 0;
|
|
||||||
limit.throttle_lower = true;
|
|
||||||
}
|
|
||||||
if (_collective_out >= 1000) {
|
|
||||||
_collective_out = 1000;
|
|
||||||
limit.throttle_upper = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// ensure not below landed/landing collective
|
// if servo output not in manual mode, process pre-compensation factors
|
||||||
if (_heliflags.landing_collective && _collective_out < _land_collective_min) {
|
if (_servo_manual == 0) {
|
||||||
_collective_out = _land_collective_min;
|
|
||||||
limit.throttle_lower = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// scale collective pitch
|
|
||||||
coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000;
|
|
||||||
|
|
||||||
// rudder feed forward based on collective
|
// 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
|
// 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
|
// also not required if we are using external gyro
|
||||||
|
@ -524,6 +528,8 @@ void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16
|
||||||
_collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE);
|
_collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE);
|
||||||
yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm);
|
yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm);
|
||||||
}
|
}
|
||||||
|
} else {
|
||||||
|
yaw_offset = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// feed power estimate into main rotor controller
|
// feed power estimate into main rotor controller
|
||||||
|
|
|
@ -66,14 +66,11 @@ public:
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
};
|
};
|
||||||
|
|
||||||
// init
|
|
||||||
void Init();
|
|
||||||
|
|
||||||
// set update rate to motors - a value in hertz
|
// set update rate to motors - a value in hertz
|
||||||
// you must have setup_motors before calling this
|
// you must have setup_motors before calling this
|
||||||
void set_update_rate(uint16_t speed_hz);
|
void set_update_rate(uint16_t speed_hz);
|
||||||
|
|
||||||
// enable - starts allowing signals to be sent to motors
|
// enable - starts allowing signals to be sent to motors and servos
|
||||||
void enable();
|
void enable();
|
||||||
|
|
||||||
// output_test - spin a motor at the pwm value specified
|
// output_test - spin a motor at the pwm value specified
|
||||||
|
@ -99,8 +96,8 @@ public:
|
||||||
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
|
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
|
||||||
bool rotor_speed_above_critical() const { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); }
|
bool rotor_speed_above_critical() const { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); }
|
||||||
|
|
||||||
// recalc_scalers - recalculates various scalers used.
|
// calculate_scalars - recalculates various scalars used
|
||||||
void recalc_scalers();
|
void calculate_scalars();
|
||||||
|
|
||||||
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
|
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
|
||||||
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
||||||
|
@ -137,17 +134,14 @@ protected:
|
||||||
void output_armed_zero_throttle();
|
void output_armed_zero_throttle();
|
||||||
void output_disarmed();
|
void output_disarmed();
|
||||||
|
|
||||||
// reset_servos - free up the swash servos for maximum movement
|
// init_outputs - initialise Servo/PWM ranges and endpoints
|
||||||
void reset_servos();
|
void init_outputs();
|
||||||
|
|
||||||
// init_servos - initialize the servos
|
|
||||||
void init_servos();
|
|
||||||
|
|
||||||
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
||||||
void calculate_roll_pitch_collective_factors();
|
void calculate_roll_pitch_collective_factors();
|
||||||
|
|
||||||
// heli_move_swash - moves swash plate to attitude of parameters passed in
|
// heli_move_actuators - moves swash plate and tail rotor
|
||||||
void move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out);
|
void move_actuators(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out);
|
||||||
|
|
||||||
// move_yaw - moves the yaw servo
|
// move_yaw - moves the yaw servo
|
||||||
void move_yaw(int16_t yaw_out);
|
void move_yaw(int16_t yaw_out);
|
||||||
|
|
Loading…
Reference in New Issue