zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_MotorsHeli: Rework how servo setup is done.

mission-4.1.18
Robert Lefebvre 10 years ago committed by Randy Mackay
parent
345663f705
commit
84102c3e3f
4 changed files with 173 additions and 234 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 63
      libraries/AP_Motors/AP_MotorsHeli.cpp
  2. 24
      libraries/AP_Motors/AP_MotorsHeli.h
  3. 300
      libraries/AP_Motors/AP_MotorsHeli_Single.cpp
  4. 20
      libraries/AP_Motors/AP_MotorsHeli_Single.h

63
libraries/AP_Motors/AP_MotorsHeli.cpp
Unescape View File

@ -165,14 +165,14 @@ void AP_MotorsHeli::Init() @@ -165,14 +165,14 @@ void AP_MotorsHeli::Init()
// set update rate
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;
// initialise some scalers
recalc_scalers();
// initialise swash plate
init_swash();
init_outputs();
// calculate all scalars
calculate_scalars();
}
// output - sends commands to the servos
@ -221,65 +221,16 @@ bool AP_MotorsHeli::parameter_check() const @@ -221,65 +221,16 @@ bool AP_MotorsHeli::parameter_check() const
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
void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo)
{
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_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
void AP_MotorsHeli::update_throttle_filter()
{

24
libraries/AP_Motors/AP_MotorsHeli.h
Unescape View File

@ -75,7 +75,6 @@ public: @@ -75,7 +75,6 @@ public:
AP_Param::setup_object_defaults(this, var_info);
// initialise flags
_heliflags.swash_initialised = 0;
_heliflags.landing_collective = 0;
_heliflags.rotor_runup_complete = 0;
};
@ -144,8 +143,8 @@ public: @@ -144,8 +143,8 @@ public:
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
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
virtual void recalc_scalers() = 0;
// calculate_scalars - must be implemented by child classes
virtual void calculate_scalars() = 0;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -181,30 +180,20 @@ protected: @@ -181,30 +180,20 @@ protected:
// update the throttle input filter
void update_throttle_filter();
// heli_move_swash - moves swash plate to attitude of parameters passed in
virtual void move_swash(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;
// move_actuators - moves swash plate and tail rotor
virtual void move_actuators(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) = 0;
// reset_swash_servo - free up swash servo for maximum movement
static void reset_swash_servo(RC_Channel& servo);
// init_swash - initialise the swash plate
void init_swash();
// init_servos - initialize the servos
virtual void init_servos() = 0;
// init_outputs - initialise Servo/PWM ranges and endpoints
virtual void init_outputs() = 0;
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
virtual void calculate_roll_pitch_collective_factors() = 0;
// flags bitmask
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 rotor_runup_complete : 1; // true if the rotors have had enough time to wind up
} _heliflags;
@ -233,7 +222,6 @@ protected: @@ -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 _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_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_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

300
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
Unescape View File

@ -108,21 +108,6 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] PROGMEM = { @@ -108,21 +108,6 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] PROGMEM = {
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
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 ) @@ -138,7 +123,7 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t 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()
{
// enable output channels
@ -146,8 +131,27 @@ void AP_MotorsHeli_Single::enable() @@ -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_3])); // swash servo 3
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_RSC); // output for main rotor esc
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
// 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
@ -216,9 +220,31 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(int16_t desired_speed) @@ -216,9 +220,31 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(int16_t desired_speed)
}
}
// recalc_scalers - recalculates various scalers used.
void AP_MotorsHeli_Single::recalc_scalers()
// calculate_scalars - recalculates various scalers used.
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_ramp_time(_rsc_ramp_time);
_main_rotor.set_runup_time(_rsc_runup_time);
@ -227,6 +253,7 @@ void AP_MotorsHeli_Single::recalc_scalers() @@ -227,6 +253,7 @@ void AP_MotorsHeli_Single::recalc_scalers()
_main_rotor.set_power_output_range(_rsc_power_low, _rsc_power_high);
_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) {
_tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_SETPOINT);
_tail_rotor.set_ramp_time(_rsc_ramp_time);
@ -243,6 +270,45 @@ void AP_MotorsHeli_Single::recalc_scalers() @@ -243,6 +270,45 @@ void AP_MotorsHeli_Single::recalc_scalers()
_tail_rotor.recalc_scalers();
}
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
{
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_CCPM) { //CCPM Swashplate, perform control mixing
// roll factors
_rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - (_phase_angle + _delta_phase_angle)));
_rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - (_phase_angle + _delta_phase_angle)));
_rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - (_phase_angle + _delta_phase_angle)));
// pitch factors
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - (_phase_angle + _delta_phase_angle)));
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - (_phase_angle + _delta_phase_angle)));
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - (_phase_angle + _delta_phase_angle)));
// collective factors
_collectiveFactor[CH_1] = 1;
_collectiveFactor[CH_2] = 1;
_collectiveFactor[CH_3] = 1;
}else{ //H1 Swashplate, keep servo outputs seperated
// roll factors
_rollFactor[CH_1] = 1;
_rollFactor[CH_2] = 0;
_rollFactor[CH_3] = 0;
// pitch factors
_pitchFactor[CH_1] = 0;
_pitchFactor[CH_2] = 1;
_pitchFactor[CH_3] = 0;
// collective factors
_collectiveFactor[CH_1] = 0;
_collectiveFactor[CH_2] = 0;
_collectiveFactor[CH_3] = 1;
}
}
// 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()
@ -255,7 +321,7 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask() @@ -255,7 +321,7 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask()
void AP_MotorsHeli_Single::output_min()
{
// move swash to mid
move_swash(0,0,500,0);
move_actuators(0,0,500,0);
_main_rotor.output(ROTOR_CONTROL_STOP);
_tail_rotor.output(ROTOR_CONTROL_STOP);
@ -270,13 +336,15 @@ void AP_MotorsHeli_Single::output_min() @@ -270,13 +336,15 @@ void AP_MotorsHeli_Single::output_min()
// sends commands to the motors
void AP_MotorsHeli_Single::output_armed_stabilizing()
{
// if manual override active after arming, deactivate it.
// 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_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
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);
@ -295,13 +363,15 @@ void AP_MotorsHeli_Single::output_armed_stabilizing() @@ -295,13 +363,15 @@ void AP_MotorsHeli_Single::output_armed_stabilizing()
void AP_MotorsHeli_Single::output_armed_not_stabilizing()
{
// if manual override active after arming, deactivate it.
// 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_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
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);
@ -321,13 +391,15 @@ void AP_MotorsHeli_Single::output_armed_not_stabilizing() @@ -321,13 +391,15 @@ void AP_MotorsHeli_Single::output_armed_not_stabilizing()
// 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 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_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
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);
@ -344,7 +416,6 @@ void AP_MotorsHeli_Single::output_armed_zero_throttle() @@ -344,7 +416,6 @@ void AP_MotorsHeli_Single::output_armed_zero_throttle()
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli_Single::output_disarmed()
{
@ -356,7 +427,13 @@ void AP_MotorsHeli_Single::output_disarmed() @@ -356,7 +427,13 @@ void AP_MotorsHeli_Single::output_disarmed()
_yaw_control_input = _yaw_radio_passthrough;
}
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
// 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);
@ -367,66 +444,6 @@ void AP_MotorsHeli_Single::output_disarmed() @@ -367,66 +444,6 @@ void AP_MotorsHeli_Single::output_disarmed()
_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
void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
{
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_CCPM) { //CCPM Swashplate, perform control mixing
// roll factors
_rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - (_phase_angle + _delta_phase_angle)));
_rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - (_phase_angle + _delta_phase_angle)));
_rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - (_phase_angle + _delta_phase_angle)));
// pitch factors
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - (_phase_angle + _delta_phase_angle)));
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - (_phase_angle + _delta_phase_angle)));
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - (_phase_angle + _delta_phase_angle)));
// collective factors
_collectiveFactor[CH_1] = 1;
_collectiveFactor[CH_2] = 1;
_collectiveFactor[CH_3] = 1;
}else{ //H1 Swashplate, keep servo outputs seperated
// roll factors
_rollFactor[CH_1] = 1;
_rollFactor[CH_2] = 0;
_rollFactor[CH_3] = 0;
// pitch factors
_pitchFactor[CH_1] = 0;
_pitchFactor[CH_2] = 1;
_pitchFactor[CH_3] = 0;
// collective factors
_collectiveFactor[CH_1] = 0;
_collectiveFactor[CH_2] = 0;
_collectiveFactor[CH_3] = 1;
}
}
// set_delta_phase_angle for setting variable phase angle compensation and force
// recalculation of collective factors
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) @@ -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:
// roll : -4500 ~ 4500
// pitch: -4500 ~ 4500
// collective: 0 ~ 1000
// 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 coll_out_scaled;
@ -455,67 +472,54 @@ void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16 @@ -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_upper = false;
if (_servo_manual == 1) { // are we in manual servo mode? (i.e. swash set-up mode)?
// check if we need to free up the swash
if (_heliflags.swash_initialised) {
reset_swash();
}
// To-Do: This equation seems to be wrong. It probably restricts swash movement so that swash setup doesn't work right.
// _collective_scalar should probably not be used or set to 1?
coll_out_scaled = coll_in * _collective_scalar + _throttle_radio_min - 1000;
}else{ // regular flight mode
// check if we need to reinitialise the swash
if (!_heliflags.swash_initialised) {
init_swash();
}
// rescale roll_out and pitch-out into the min and max ranges to provide linear motion
// across the input range instead of stopping when the input hits the constrain value
// these calculations are based on an assumption of the user specified roll_max and pitch_max
// 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.
roll_out = roll_out * _roll_scaler;
if (roll_out < -_roll_max) {
roll_out = -_roll_max;
limit.roll_pitch = true;
}
if (roll_out > _roll_max) {
roll_out = _roll_max;
limit.roll_pitch = true;
}
// rescale roll_out and pitch-out into the min and max ranges to provide linear motion
// across the input range instead of stopping when the input hits the constrain value
// these calculations are based on an assumption of the user specified roll_max and pitch_max
// 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.
roll_out = roll_out * _roll_scaler;
if (roll_out < -_roll_max) {
roll_out = -_roll_max;
limit.roll_pitch = true;
}
if (roll_out > _roll_max) {
roll_out = _roll_max;
limit.roll_pitch = true;
}
// scale pitch and update limits
pitch_out = pitch_out * _pitch_scaler;
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;
}
// scale pitch and update limits
pitch_out = pitch_out * _pitch_scaler;
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;
}
// constrain collective input
_collective_out = coll_in;
if (_collective_out <= 0) {
_collective_out = 0;
limit.throttle_lower = true;
}
if (_collective_out >= 1000) {
_collective_out = 1000;
limit.throttle_upper = true;
}
// constrain collective input
_collective_out = coll_in;
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 (_heliflags.landing_collective && _collective_out < _land_collective_min) {
_collective_out = _land_collective_min;
limit.throttle_lower = true;
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && _collective_out < _land_collective_min) {
_collective_out = _land_collective_min;
limit.throttle_lower = true;
}
// scale collective pitch
coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000;
// scale collective pitch
coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000;
// if servo output not in manual mode, process pre-compensation factors
if (_servo_manual == 0) {
// 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
@ -524,6 +528,8 @@ void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16 @@ -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);
yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm);
}
} else {
yaw_offset = 0;
}
// feed power estimate into main rotor controller

20
libraries/AP_Motors/AP_MotorsHeli_Single.h
Unescape View File

@ -66,14 +66,11 @@ public: @@ -66,14 +66,11 @@ public:
AP_Param::setup_object_defaults(this, var_info);
};
// init
void Init();
// set update rate to motors - a value in hertz
// you must have setup_motors before calling this
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();
// output_test - spin a motor at the pwm value specified
@ -99,8 +96,8 @@ public: @@ -99,8 +96,8 @@ public:
// 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(); }
// recalc_scalers - recalculates various scalers used.
void recalc_scalers();
// calculate_scalars - recalculates various scalars used
void calculate_scalars();
// 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
@ -137,17 +134,14 @@ protected: @@ -137,17 +134,14 @@ protected:
void output_armed_zero_throttle();
void output_disarmed();
// reset_servos - free up the swash servos for maximum movement
void reset_servos();
// init_servos - initialize the servos
void init_servos();
// init_outputs - initialise Servo/PWM ranges and endpoints
void init_outputs();
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void calculate_roll_pitch_collective_factors();
// heli_move_swash - moves swash plate to attitude of parameters passed in
void move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out);
// heli_move_actuators - moves swash plate and tail rotor
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
void move_yaw(int16_t yaw_out);

Write Preview
Loading…
Cancel
Save