aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: Move traditional helicopter controls into AP_MotorsHeli_Single.

Browse Source 
Original commit by fhedberg, had to fix merge conflicts and now it appears I did the commit?
This commit is contained in:
Fredrik Hedberg 2015-07-22 15:46:53 +02:00 committed by Randy Mackay
parent af1eee44ee
commit cde94078b7
5 changed files with 708 additions and 570 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Motors/AP_Motors.h
View File

@ -12,7 +12,7 @@
#include "AP_MotorsY6.h"
#include "AP_MotorsOcta.h"
#include "AP_MotorsOctaQuad.h"
#include "AP_MotorsHeli.h"
#include "AP_MotorsHeli_Single.h"
#include "AP_MotorsSingle.h"
#include "AP_MotorsCoax.h"

483
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -27,33 +27,6 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Param: SV1_POS
// @DisplayName: Servo 1 Position
// @Description: Angular location of swash servo #1
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli, _servo1_pos, AP_MOTORS_HELI_SERVO1_POS),
// @Param: SV2_POS
// @DisplayName: Servo 2 Position
// @Description: Angular location of swash servo #2
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli, _servo2_pos, AP_MOTORS_HELI_SERVO2_POS),
// @Param: SV3_POS
// @DisplayName: Servo 3 Position
// @Description: Angular location of swash servo #3
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli, _servo3_pos, AP_MOTORS_HELI_SERVO3_POS),
// @Param: ROL_MAX
// @DisplayName: Swash Roll Angle Max
// @Description: Maximum roll angle of the swash plate
@ -61,7 +34,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Units: Centi-Degrees
// @Increment: 100
// @User: Advanced
AP_GROUPINFO("ROL_MAX", 4, AP_MotorsHeli, _roll_max, AP_MOTORS_HELI_SWASH_ROLL_MAX),
AP_GROUPINFO("ROL_MAX", 1, AP_MotorsHeli, _roll_max, AP_MOTORS_HELI_SWASH_ROLL_MAX),
// @Param: PIT_MAX
// @DisplayName: Swash Pitch Angle Max
@ -70,7 +43,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Units: Centi-Degrees
// @Increment: 100
// @User: Advanced
AP_GROUPINFO("PIT_MAX", 5, AP_MotorsHeli, _pitch_max, AP_MOTORS_HELI_SWASH_PITCH_MAX),
AP_GROUPINFO("PIT_MAX", 2, AP_MotorsHeli, _pitch_max, AP_MOTORS_HELI_SWASH_PITCH_MAX),
// @Param: COL_MIN
// @DisplayName: Collective Pitch Minimum
@ -79,7 +52,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("COL_MIN", 6, AP_MotorsHeli, _collective_min, AP_MOTORS_HELI_COLLECTIVE_MIN),
AP_GROUPINFO("COL_MIN", 3, AP_MotorsHeli, _collective_min, AP_MOTORS_HELI_COLLECTIVE_MIN),
// @Param: COL_MAX
// @DisplayName: Collective Pitch Maximum
@ -88,7 +61,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("COL_MAX", 7, AP_MotorsHeli, _collective_max, AP_MOTORS_HELI_COLLECTIVE_MAX),
AP_GROUPINFO("COL_MAX", 4, AP_MotorsHeli, _collective_max, AP_MOTORS_HELI_COLLECTIVE_MAX),
// @Param: COL_MID
// @DisplayName: Collective Pitch Mid-Point
@ -97,53 +70,14 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("COL_MID", 8, AP_MotorsHeli, _collective_mid, AP_MOTORS_HELI_COLLECTIVE_MID),
// @Param: TAIL_TYPE
// @DisplayName: Tail Type
// @Description: Tail type selection. Simpler yaw controller used if external gyro is selected
// @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch
// @User: Standard
AP_GROUPINFO("TAIL_TYPE",9, AP_MotorsHeli, _tail_type, AP_MOTORS_HELI_TAILTYPE_SERVO),
// @Param: SWASH_TYPE
// @DisplayName: Swash Type
// @Description: Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing
// @Values: 0:3-Servo CCPM, 1:H1 Mechanical Mixing
// @User: Standard
AP_GROUPINFO("SWASH_TYPE",10, AP_MotorsHeli, _swash_type, AP_MOTORS_HELI_SWASH_CCPM),
// @Param: GYR_GAIN
// @DisplayName: External Gyro Gain
// @Description: PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("GYR_GAIN", 11, AP_MotorsHeli, _ext_gyro_gain, AP_MOTORS_HELI_EXT_GYRO_GAIN),
AP_GROUPINFO("COL_MID", 5, AP_MotorsHeli, _collective_mid, AP_MOTORS_HELI_COLLECTIVE_MID),
// @Param: SV_MAN
// @DisplayName: Manual Servo Mode
// @Description: Pass radio inputs directly to servos for set-up. Do not set this manually!
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("SV_MAN", 12, AP_MotorsHeli, _servo_manual, 0),
// @Param: PHANG
// @DisplayName: Swashplate Phase Angle Compensation
// @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem
// @Range: -90 90
// @Units: Degrees
// @User: Advanced
// @Increment: 1
AP_GROUPINFO("PHANG", 13, AP_MotorsHeli, _phase_angle, 0),
// @Param: COLYAW
// @DisplayName: Collective-Yaw Mixing
// @Description: Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.
// @Range: -10 10
// @Increment: 0.1
AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, _collective_yaw_effect, 0),
AP_GROUPINFO("SV_MAN", 7, AP_MotorsHeli, _servo_manual, 0),
// @Param: GOV_SETPOINT
// @DisplayName: External Motor Governor Setpoint
@ -152,21 +86,14 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Units: PWM
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_SETPOINT", 15, AP_MotorsHeli, _rsc_setpoint, AP_MOTORS_HELI_RSC_SETPOINT),
AP_GROUPINFO("RSC_SETPOINT", 8, AP_MotorsHeli, _rsc_setpoint, AP_MOTORS_HELI_RSC_SETPOINT),
// @Param: RSC_MODE
// @DisplayName: Rotor Speed Control Mode
// @Description: Controls the source of the desired rotor speed, either ch8 or RSC_SETPOINT
// @Values: 0:None, 1:Ch8 Input, 2:SetPoint
// @User: Standard
AP_GROUPINFO("RSC_MODE", 16, AP_MotorsHeli, _rsc_mode, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH),
// @Param: FLYBAR_MODE
// @DisplayName: Flybar Mode Selector
// @Description: Flybar present or not. Affects attitude controller used during ACRO flight mode
// @Range: 0:NoFlybar 1:Flybar
// @User: Standard
AP_GROUPINFO("FLYBAR_MODE", 17, AP_MotorsHeli, _flybar_mode, AP_MOTORS_HELI_NOFLYBAR),
AP_GROUPINFO("RSC_MODE", 9, AP_MotorsHeli, _rsc_mode, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH),
// @Param: LAND_COL_MIN
// @DisplayName: Landing Collective Minimum
@ -175,7 +102,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Units: pwm
// @Increment: 1
// @User: Standard
AP_GROUPINFO("LAND_COL_MIN", 18, AP_MotorsHeli, _land_collective_min, AP_MOTORS_HELI_LAND_COLLECTIVE_MIN),
AP_GROUPINFO("LAND_COL_MIN", 10, AP_MotorsHeli, _land_collective_min, AP_MOTORS_HELI_LAND_COLLECTIVE_MIN),
// @Param: RSC_RAMP_TIME
// @DisplayName: RSC Ramp Time
@ -183,7 +110,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Range: 0 60
// @Units: Seconds
// @User: Standard
AP_GROUPINFO("RSC_RAMP_TIME", 19, AP_MotorsHeli,_rsc_ramp_time, AP_MOTORS_HELI_RSC_RAMP_TIME),
AP_GROUPINFO("RSC_RAMP_TIME", 11, AP_MotorsHeli, _rsc_ramp_time, AP_MOTORS_HELI_RSC_RAMP_TIME),
// @Param: RSC_RUNUP_TIME
// @DisplayName: RSC Runup Time
@ -191,24 +118,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Range: 0 60
// @Units: Seconds
// @User: Standard
AP_GROUPINFO("RSC_RUNUP_TIME", 20, AP_MotorsHeli,_rsc_runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME),
// @Param: TAIL_SPEED
// @DisplayName: Direct Drive VarPitch Tail ESC speed
// @Description: Direct Drive VarPitch Tail ESC speed. Only used when TailType is DirectDrive VarPitch
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("TAIL_SPEED", 21, AP_MotorsHeli, _direct_drive_tailspeed, AP_MOTOR_HELI_DDTAIL_DEFAULT),
AP_GROUPINFO("RSC_RUNUP_TIME", 12, AP_MotorsHeli, _rsc_runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME),
// @Param: RSC_CRITICAL
// @DisplayName: Critical Rotor Speed
// @Description: Rotor speed below which flight is not possible
// @Range: 0 1000
// @Range: 0 0-1000
// @Increment: 10
// @User: Standard
AP_GROUPINFO("RSC_CRITICAL", 22, AP_MotorsHeli, _rsc_critical, AP_MOTORS_HELI_RSC_CRITICAL),
AP_GROUPINFO("RSC_CRITICAL", 13, AP_MotorsHeli, _rsc_critical, AP_MOTORS_HELI_RSC_CRITICAL),
// parameters 1 ~ 29 reserved for tradheli
// parameters 30 ~ 39 reserved for tricopter
@ -235,37 +153,6 @@ void AP_MotorsHeli::Init()
// initialise swash plate
init_swash();
// 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_AUX]);
RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_RSC]);
}
// set update rate to motors - a value in hertz
void AP_MotorsHeli::set_update_rate( uint16_t speed_hz )
{
// record requested speed
_speed_hz = speed_hz;
// setup fast channels
uint32_t mask =
1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]) |
1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]) |
1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]) |
1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]);
hal.rcout->set_freq(mask, _speed_hz);
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsHeli::enable()
{
// enable output channels
hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1])); // swash servo 1
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_AUX); // output for gyro gain or direct drive variable pitch tail motor
hal.rcout->enable_ch(AP_MOTORS_HELI_RSC); // output for main rotor esc
}
// output - sends commands to the servos
@ -300,60 +187,6 @@ void AP_MotorsHeli::output_min()
limit.throttle_upper = false;
}
// output_test - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsHeli::output_test(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!armed()) {
return;
}
// output to motors and servos
switch (motor_seq) {
case 1:
// swash servo 1
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm);
break;
case 2:
// swash servo 2
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), pwm);
break;
case 3:
// swash servo 3
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), pwm);
break;
case 4:
// external gyro & tail servo
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) {
write_aux(_ext_gyro_gain);
}
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm);
break;
case 5:
// main rotor
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_HELI_RSC]), pwm);
break;
default:
// do nothing
break;
}
}
// allow_arming - check if it's safe to arm
bool AP_MotorsHeli::allow_arming() const
{
// returns false if main rotor speed is not zero
if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _main_rotor.get_estimated_speed() > 0) {
return false;
}
// all other cases it is OK to arm
return true;
}
// parameter_check - check if helicopter specific parameters are sensible
bool AP_MotorsHeli::parameter_check() const
{
@ -366,86 +199,18 @@ bool AP_MotorsHeli::parameter_check() const
return true;
}
// set_desired_rotor_speed
void AP_MotorsHeli::set_desired_rotor_speed(int16_t desired_speed)
{
_main_rotor.set_desired_speed(desired_speed);
if (desired_speed > 0 && _tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_desired_speed(_direct_drive_tailspeed);
} else {
_tail_rotor.set_desired_speed(0);
}
}
// return true if the main rotor is up to speed
bool AP_MotorsHeli::rotor_runup_complete() const
{
return _heliflags.rotor_runup_complete;
}
// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
void AP_MotorsHeli::recalc_scalers()
{
if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_SETPOINT) {
_tail_rotor.set_ramp_time(0);
_tail_rotor.set_runup_time(0);
_tail_rotor.set_critical_speed(0);
} else {
_main_rotor.set_ramp_time(_rsc_ramp_time);
_main_rotor.set_runup_time(_rsc_runup_time);
_main_rotor.set_critical_speed(_rsc_critical);
}
_main_rotor.recalc_scalers();
if (_rsc_mode != AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_ramp_time(0);
_tail_rotor.set_runup_time(0);
_tail_rotor.set_critical_speed(0);
} else {
_tail_rotor.set_ramp_time(_rsc_ramp_time);
_tail_rotor.set_runup_time(_rsc_runup_time);
_tail_rotor.set_critical_speed(_rsc_critical);
}
_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::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_AUX | 1U << AP_MOTORS_HELI_RSC);
}
void AP_MotorsHeli::output_armed_not_stabilizing()
{
// stabilizing servos always operate for helicopters
output_armed_stabilizing();
}
// sends commands to the motors
void AP_MotorsHeli::output_armed_stabilizing()
{
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.output_armed();
if (!_tail_rotor.is_runup_complete())
{
_heliflags.rotor_runup_complete = false;
return;
}
}
_main_rotor.output_armed();
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
}
// output_armed_zero_throttle - sends commands to the motors
void AP_MotorsHeli::output_armed_zero_throttle()
{
@ -454,19 +219,6 @@ void AP_MotorsHeli::output_armed_zero_throttle()
output_armed_stabilizing();
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.output_disarmed();
}
_main_rotor.output_disarmed();
_heliflags.rotor_runup_complete = false;
}
// reset_swash - free up swash for maximum movements. Used for set-up
void AP_MotorsHeli::reset_swash()
@ -487,14 +239,6 @@ void AP_MotorsHeli::reset_swash()
_heliflags.swash_initialised = false;
}
// reset_servos
void AP_MotorsHeli::reset_servos()
{
reset_swash_servo (_servo_1);
reset_swash_servo (_servo_2);
reset_swash_servo (_servo_3);
}
// reset_swash_servo
void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo)
{
@ -531,16 +275,6 @@ void AP_MotorsHeli::init_swash()
_heliflags.swash_initialised = true;
}
// init_servos
void AP_MotorsHeli::init_servos()
{
init_swash_servo (_servo_1);
init_swash_servo (_servo_2);
init_swash_servo (_servo_3);
_servo_4.set_angle(4500);
}
// init_swash_servo
void AP_MotorsHeli::init_swash_servo(RC_Channel& servo)
{
@ -550,197 +284,6 @@ void AP_MotorsHeli::init_swash_servo(RC_Channel& servo)
servo.radio_max = 2000;
}
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void AP_MotorsHeli::calculate_roll_pitch_collective_factors()
{
if (_swash_type == AP_MOTORS_HELI_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;
}
}
//
// heli_move_swash - moves swash plate to attitude of parameters passed in
// - expected ranges:
// roll : -4500 ~ 4500
// pitch: -4500 ~ 4500
// collective: 0 ~ 1000
// yaw: -4500 ~ 4500
//
void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out)
{
// 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;
}
int16_t yaw_offset = 0;
int16_t coll_out_scaled;
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
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;
}
// 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;
}
// 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;
// rudder feed forward based on collective
// the feed-forward is not required when the motor is shut down and not creating torque
// also not required if we are using external gyro
if ((_main_rotor.get_desired_speed() > 0) && _tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) {
// sanity check collective_yaw_effect
_collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTOR_HELI_COLYAW_RANGE, AP_MOTOR_HELI_COLYAW_RANGE);
yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm);
}
}
// swashplate servos
_servo_1.servo_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out)/10 + _collectiveFactor[CH_1] * coll_out_scaled + (_servo_1.radio_trim-1500);
_servo_2.servo_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out)/10 + _collectiveFactor[CH_2] * coll_out_scaled + (_servo_2.radio_trim-1500);
if (_swash_type == AP_MOTORS_HELI_SWASH_H1) {
_servo_1.servo_out += 500;
_servo_2.servo_out += 500;
}
_servo_3.servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_servo_3.radio_trim-1500);
// use servo_out to calculate pwm_out and radio_out
_servo_1.calc_pwm();
_servo_2.calc_pwm();
_servo_3.calc_pwm();
// actually move the servos
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_out);
// update the yaw rate using the tail rotor/servo
output_yaw(yaw_out + yaw_offset);
}
// output_yaw
void AP_MotorsHeli::output_yaw(int16_t yaw_out)
{
_servo_4.servo_out = constrain_int16(yaw_out, -4500, 4500);
if (_servo_4.servo_out != yaw_out) {
limit.yaw = true;
}
_servo_4.calc_pwm();
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_out);
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) {
// output gain to exernal gyro
write_aux(_ext_gyro_gain);
} else if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0) {
// output yaw servo to tail rsc
write_aux(_servo_4.servo_out);
}
}
// write_aux - outputs pwm onto output aux channel (ch7)
// servo_out parameter is of the range 0 ~ 1000
void AP_MotorsHeli::write_aux(int16_t servo_out)
{
_servo_aux.servo_out = servo_out;
_servo_aux.calc_pwm();
hal.rcout->write(AP_MOTORS_HELI_AUX, _servo_aux.radio_out);
}
// set_delta_phase_angle for setting variable phase angle compensation and force
// recalculation of collective factors
void AP_MotorsHeli::set_delta_phase_angle(int16_t angle)

122
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -7,6 +7,7 @@
#define __AP_MOTORS_HELI_H__
#include <inttypes.h>
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
@ -21,19 +22,6 @@
#define AP_MOTORS_HELI_SPEED_DIGITAL_SERVOS 125 // update rate for digital servos
#define AP_MOTORS_HELI_SPEED_ANALOG_SERVOS 125 // update rate for analog servos
// TradHeli Aux Function Output Channels
#define AP_MOTORS_HELI_AUX CH_7
#define AP_MOTORS_HELI_RSC CH_8
// servo position defaults
#define AP_MOTORS_HELI_SERVO1_POS -60
#define AP_MOTORS_HELI_SERVO2_POS 60
#define AP_MOTORS_HELI_SERVO3_POS 180
// swash type definitions
#define AP_MOTORS_HELI_SWASH_CCPM 0
#define AP_MOTORS_HELI_SWASH_H1 1
// default swash min and max angles and positions
#define AP_MOTORS_HELI_SWASH_ROLL_MAX 2500
#define AP_MOTORS_HELI_SWASH_PITCH_MAX 2500
@ -48,21 +36,6 @@
// swash min while landed or landing (as a number from 0 ~ 1000
#define AP_MOTORS_HELI_LAND_COLLECTIVE_MIN 0
// tail types
#define AP_MOTORS_HELI_TAILTYPE_SERVO 0
#define AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO 1
#define AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH 2
#define AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH 3
// default external gyro gain (ch7 out)
#define AP_MOTORS_HELI_EXT_GYRO_GAIN 350
// minimum outputs for direct drive motors
#define AP_MOTOR_HELI_DDTAIL_DEFAULT 500
// COLYAW parameter min and max values
#define AP_MOTOR_HELI_COLYAW_RANGE 10.0f
// main rotor speed control types (ch8 out)
#define AP_MOTORS_HELI_RSC_MODE_NONE 0 // main rotor ESC is directly connected to receiver, pilot controls ESC speed through transmitter directly
#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
@ -77,7 +50,6 @@
// default main rotor ramp up time in seconds
#define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 second to ramp output to main rotor ESC to full power (most people use exterrnal govenors so we can ramp up quickly)
#define AP_MOTORS_HELI_RSC_RUNUP_TIME 10 // 10 seconds for rotor to reach full speed
#define AP_MOTORS_HELI_TAIL_RAMP_INCREMENT 5 // 5 is 2 seconds for direct drive tail rotor to reach to full speed (5 = (2sec*100hz)/1000)
// flybar types
#define AP_MOTORS_HELI_NOFLYBAR 0
@ -90,23 +62,9 @@ class AP_MotorsHeli : public AP_Motors {
public:
/// Constructor
AP_MotorsHeli( RC_Channel& servo_aux,
RC_Channel& servo_rotor,
RC_Channel& swash_servo_1,
RC_Channel& swash_servo_2,
RC_Channel& swash_servo_3,
RC_Channel& yaw_servo,
uint16_t loop_rate,
AP_MotorsHeli( uint16_t loop_rate,
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
AP_Motors(loop_rate, speed_hz),
_servo_aux(servo_aux),
_servo_rsc(servo_rotor),
_servo_1(swash_servo_1),
_servo_2(swash_servo_2),
_servo_3(swash_servo_3),
_servo_4(yaw_servo),
_main_rotor(servo_rotor, AP_MOTORS_HELI_RSC, loop_rate),
_tail_rotor(servo_aux, AP_MOTORS_HELI_AUX, loop_rate)
AP_Motors(loop_rate, speed_hz)
{
AP_Param::setup_object_defaults(this, var_info);
@ -121,10 +79,10 @@ public:
// 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 );
virtual void set_update_rate( uint16_t speed_hz ) = 0;
// enable - starts allowing signals to be sent to motors
void enable();
virtual void enable() = 0;
// output_min - sets servos to neutral point
void output_min();
@ -132,7 +90,7 @@ public:
// output_test - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
virtual void output_test(uint8_t motor_seq, int16_t pwm);
virtual void output_test(uint8_t motor_seq, int16_t pwm) = 0;
// slow_start - ignored by helicopters
void slow_start(bool true_false) {};
@ -142,20 +100,13 @@ public:
//
// allow_arming - returns true if main rotor is spinning and it is ok to arm
bool allow_arming() const;
virtual bool allow_arming() const = 0;
// parameter_check - returns true if helicopter specific parameters are sensible, used for pre-arm check
bool parameter_check() const;
// _tail_type - returns the tail type (servo, servo with ext gyro, direct drive var pitch, direct drive fixed pitch)
int16_t tail_type() const { return _tail_type; }
// ext_gyro_gain - gets and sets external gyro gain as a pwm (1000~2000)
int16_t ext_gyro_gain() const { return _ext_gyro_gain; }
void ext_gyro_gain(int16_t pwm) { _ext_gyro_gain = pwm; }
// has_flybar - returns true if we have a mechical flybar
bool has_flybar() const { return _flybar_mode; }
virtual bool has_flybar() const { return AP_MOTORS_HELI_NOFLYBAR; }
// get_collective_mid - returns collective mid position as a number from 0 ~ 1000
int16_t get_collective_mid() const { return _collective_mid; }
@ -173,25 +124,22 @@ public:
int16_t get_rsc_setpoint() const { return _rsc_setpoint; }
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
void set_desired_rotor_speed(int16_t desired_speed);
virtual void set_desired_rotor_speed(int16_t desired_speed) = 0;
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000
int16_t get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); }
virtual int16_t get_desired_rotor_speed() const = 0;
// get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000
int16_t get_estimated_rotor_speed() { return _main_rotor.get_estimated_speed(); }
virtual int16_t get_estimated_rotor_speed() const = 0;
// return true if the main rotor is up to speed
bool rotor_runup_complete() const;
// 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_estimated_speed() > _main_rotor.get_critical_speed(); }
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
void recalc_scalers();
// get_phase_angle - returns phase angle
int16_t get_phase_angle() const { return _phase_angle; }
virtual void recalc_scalers() = 0;
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -202,7 +150,7 @@ public:
// 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
virtual uint16_t get_motor_mask();
virtual uint16_t get_motor_mask() = 0;
// set_radio_passthrough used to pass radio inputs directly to outputs
void set_radio_passthrough(int16_t radio_roll_input, int16_t radio_pitch_input, int16_t radio_throttle_input, int16_t radio_yaw_input);
@ -213,28 +161,28 @@ public:
// output - sends commands to the motors
void output();
// supports_yaw_passthrough
virtual bool supports_yaw_passthrough() const { return false; }
protected:
// output - sends commands to the motors
void output_armed_stabilizing();
virtual void output_armed_stabilizing() = 0;
void output_armed_not_stabilizing();
void output_armed_zero_throttle();
void output_disarmed();
void output_yaw(int16_t yaw_out);
virtual void output_disarmed() = 0;
// update the throttle input filter
void update_throttle_filter();
private:
// 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);
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
void reset_servos();
virtual void reset_servos() = 0;
// reset_swash_servo - free up swash servo for maximum movement
static void reset_swash_servo(RC_Channel& servo);
@ -243,27 +191,13 @@ private:
void init_swash();
// init_servos - initialize the servos
void init_servos();
virtual void init_servos() = 0;
// init_swash_servo - initialize a swash servo
static void init_swash_servo(RC_Channel& servo);
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void calculate_roll_pitch_collective_factors();
// write_aux - outputs pwm onto output aux channel (ch7). servo_out parameter is of the range 0 ~ 1000
void write_aux(int16_t servo_out);
// external objects we depend upon
RC_Channel& _servo_aux; // output to ext gyro gain and tail direct drive esc (ch7)
RC_Channel& _servo_rsc; // output to main rotor esc (ch8)
RC_Channel& _servo_1; // swash plate servo #1
RC_Channel& _servo_2; // swash plate servo #2
RC_Channel& _servo_3; // swash plate servo #3
RC_Channel& _servo_4; // tail servo
AP_MotorsHeli_RSC _main_rotor; // main rotor
AP_MotorsHeli_RSC _tail_rotor; // tail rotor
virtual void calculate_roll_pitch_collective_factors() = 0;
// flags bitmask
struct heliflags_type {
@ -273,27 +207,17 @@ private:
} _heliflags;
// parameters
AP_Int16 _servo1_pos; // Angular location of swash servo #1
AP_Int16 _servo2_pos; // Angular location of swash servo #2
AP_Int16 _servo3_pos; // Angular location of swash servo #3
AP_Int16 _roll_max; // Maximum roll angle of the swash plate in centi-degrees
AP_Int16 _pitch_max; // Maximum pitch angle of the swash plate in centi-degrees
AP_Int16 _collective_min; // Lowest possible servo position for the swashplate
AP_Int16 _collective_max; // Highest possible servo position for the swashplate
AP_Int16 _collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Assymetrical blades)
AP_Int16 _tail_type; // Tail type used: Servo, Servo with external gyro, direct drive variable pitch or direct drive fixed pitch
AP_Int8 _swash_type; // Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing
AP_Int16 _ext_gyro_gain; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro
AP_Int8 _servo_manual; // Pass radio inputs directly to servos during set-up through mission planner
AP_Int16 _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem
AP_Float _collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.
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_ramp_time; // Time in seconds for the output to the main rotor's ESC to reach full speed
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 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode
AP_Int16 _land_collective_min; // Minimum collective when landed or landing
AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000)
AP_Int16 _rsc_critical; // Rotor speed below which flight is not possible
// internal variables

498
libraries/AP_Motors/AP_MotorsHeli_Single.cpp Normal file
View File

@ -0,0 +1,498 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include <AP_HAL.h>
#include "AP_MotorsHeli_Single.h"
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] PROGMEM = {
AP_NESTEDGROUPINFO(AP_MotorsHeli, 0),
// @Param: SV1_POS
// @DisplayName: Servo 1 Position
// @Description: Angular location of swash servo #1
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli_Single, _servo1_pos, AP_MOTORS_HELI_SINGLE_SERVO1_POS),
// @Param: SV2_POS
// @DisplayName: Servo 2 Position
// @Description: Angular location of swash servo #2
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli_Single, _servo2_pos, AP_MOTORS_HELI_SINGLE_SERVO2_POS),
// @Param: SV3_POS
// @DisplayName: Servo 3 Position
// @Description: Angular location of swash servo #3
// @Range: -180 180
// @Units: Degrees
// @User: Standard
// @Increment: 1
AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli_Single, _servo3_pos, AP_MOTORS_HELI_SINGLE_SERVO3_POS),
// @Param: TAIL_TYPE
// @DisplayName: Tail Type
// @Description: Tail type selection. Simpler yaw controller used if external gyro is selected
// @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch
// @User: Standard
AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO),
// @Param: SWASH_TYPE
// @DisplayName: Swash Type
// @Description: Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing
// @Values: 0:3-Servo CCPM, 1:H1 Mechanical Mixing
// @User: Standard
AP_GROUPINFO("SWASH_TYPE", 5, AP_MotorsHeli_Single, _swash_type, AP_MOTORS_HELI_SINGLE_SWASH_CCPM),
// @Param: GYR_GAIN
// @DisplayName: External Gyro Gain
// @Description: PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("GYR_GAIN", 6, AP_MotorsHeli_Single, _ext_gyro_gain, AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN),
// @Param: PHANG
// @DisplayName: Swashplate Phase Angle Compensation
// @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem
// @Range: -90 90
// @Units: Degrees
// @User: Advanced
// @Increment: 1
AP_GROUPINFO("PHANG", 7, AP_MotorsHeli_Single, _phase_angle, 0),
// @Param: COLYAW
// @DisplayName: Collective-Yaw Mixing
// @Description: Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.
// @Range: -10 10
// @Increment: 0.1
AP_GROUPINFO("COLYAW", 8, AP_MotorsHeli_Single, _collective_yaw_effect, 0),
// @Param: FLYBAR_MODE
// @DisplayName: Flybar Mode Selector
// @Description: Flybar present or not. Affects attitude controller used during ACRO flight mode
// @Range: 0:NoFlybar 1:Flybar
// @User: Standard
AP_GROUPINFO("FLYBAR_MODE", 9, AP_MotorsHeli_Single, _flybar_mode, AP_MOTORS_HELI_NOFLYBAR),
// @Param: TAIL_SPEED
// @DisplayName: Direct Drive VarPitch Tail ESC speed
// @Description: Direct Drive VarPitch Tail ESC speed. Only used when TailType is DirectDrive VarPitch
// @Range: 0 1000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("TAIL_SPEED", 10, AP_MotorsHeli_Single, _direct_drive_tailspeed, AP_MOTOR_HELI_SINGLE_DDTAIL_DEFAULT),
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 )
{
// record requested speed
_speed_hz = speed_hz;
// setup fast channels
uint32_t mask =
1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]) |
1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]) |
1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]) |
1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]);
hal.rcout->set_freq(mask, _speed_hz);
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsHeli_Single::enable()
{
// enable output channels
hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1])); // swash servo 1
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
}
// output_test - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!armed()) {
return;
}
// output to motors and servos
switch (motor_seq) {
case 1:
// swash servo 1
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm);
break;
case 2:
// swash servo 2
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), pwm);
break;
case 3:
// swash servo 3
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), pwm);
break;
case 4:
// external gyro & tail servo
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
write_aux(_ext_gyro_gain);
}
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm);
break;
case 5:
// main rotor
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_HELI_SINGLE_RSC]), pwm);
break;
default:
// do nothing
break;
}
}
// allow_arming - check if it's safe to arm
bool AP_MotorsHeli_Single::allow_arming() const
{
// returns false if main rotor speed is not zero
if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _main_rotor.get_estimated_speed() > 0) {
return false;
}
// all other cases it is OK to arm
return true;
}
// set_desired_rotor_speed
void AP_MotorsHeli_Single::set_desired_rotor_speed(int16_t desired_speed)
{
_main_rotor.set_desired_speed(desired_speed);
if (desired_speed > 0 && _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_desired_speed(_direct_drive_tailspeed);
} else {
_tail_rotor.set_desired_speed(0);
}
}
// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
void AP_MotorsHeli_Single::recalc_scalers()
{
if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_SETPOINT) {
_tail_rotor.set_ramp_time(0);
_tail_rotor.set_runup_time(0);
_tail_rotor.set_critical_speed(0);
} else {
_main_rotor.set_ramp_time(_rsc_ramp_time);
_main_rotor.set_runup_time(_rsc_runup_time);
_main_rotor.set_critical_speed(_rsc_critical);
}
_main_rotor.recalc_scalers();
if (_rsc_mode != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_ramp_time(0);
_tail_rotor.set_runup_time(0);
_tail_rotor.set_critical_speed(0);
} else {
_tail_rotor.set_ramp_time(_rsc_ramp_time);
_tail_rotor.set_runup_time(_rsc_runup_time);
_tail_rotor.set_critical_speed(_rsc_critical);
}
_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);
}
// sends commands to the motors
void AP_MotorsHeli_Single::output_armed_stabilizing()
{
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_armed();
if (!_tail_rotor.is_runup_complete())
{
_heliflags.rotor_runup_complete = false;
return;
}
}
_main_rotor.output_armed();
_heliflags.rotor_runup_complete = _main_rotor.is_runup_complete();
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli_Single::output_disarmed()
{
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_disarmed();
}
_main_rotor.output_disarmed();
_heliflags.rotor_runup_complete = false;
}
// reset_servos
void AP_MotorsHeli_Single::reset_servos()
{
reset_swash_servo (_servo_1);
reset_swash_servo (_servo_2);
reset_swash_servo (_servo_3);
}
// init_servos
void AP_MotorsHeli_Single::init_servos()
{
init_swash_servo (_servo_1);
init_swash_servo (_servo_2);
init_swash_servo (_servo_3);
_servo_4.set_angle(4500);
}
// 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;
}
}
//
// heli_move_swash - moves swash plate to attitude of parameters passed in
// - 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)
{
// 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;
}
int16_t yaw_offset = 0;
int16_t coll_out_scaled;
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
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;
}
// 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;
}
// 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;
// rudder feed forward based on collective
// the feed-forward is not required when the motor is shut down and not creating torque
// also not required if we are using external gyro
if ((_main_rotor.get_desired_speed() > 0) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// sanity check collective_yaw_effect
_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);
}
}
// swashplate servos
_servo_1.servo_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out)/10 + _collectiveFactor[CH_1] * coll_out_scaled + (_servo_1.radio_trim-1500);
_servo_2.servo_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out)/10 + _collectiveFactor[CH_2] * coll_out_scaled + (_servo_2.radio_trim-1500);
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H1) {
_servo_1.servo_out += 500;
_servo_2.servo_out += 500;
}
_servo_3.servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_servo_3.radio_trim-1500);
// use servo_out to calculate pwm_out and radio_out
_servo_1.calc_pwm();
_servo_2.calc_pwm();
_servo_3.calc_pwm();
// actually move the servos
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_out);
// update the yaw rate using the tail rotor/servo
output_yaw(yaw_out + yaw_offset);
}
// output_yaw
void AP_MotorsHeli_Single::output_yaw(int16_t yaw_out)
{
_servo_4.servo_out = constrain_int16(yaw_out, -4500, 4500);
if (_servo_4.servo_out != yaw_out) {
limit.yaw = true;
}
_servo_4.calc_pwm();
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_out);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// output gain to exernal gyro
write_aux(_ext_gyro_gain);
} else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0) {
// output yaw servo to tail rsc
write_aux(_servo_4.servo_out);
}
}
// write_aux - outputs pwm onto output aux channel (ch7)
// servo_out parameter is of the range 0 ~ 1000
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);
}

173
libraries/AP_Motors/AP_MotorsHeli_Single.h Normal file
View File

@ -0,0 +1,173 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AP_MotorsHeli_Single.h
/// @brief Motor control class for traditional heli
#ifndef __AP_MOTORS_HELI_SINGLE_H__
#define __AP_MOTORS_HELI_SINGLE_H__
#include <inttypes.h>
#include <AP_Common.h>
#include <AP_Math.h>
#include <RC_Channel.h>
#include "AP_MotorsHeli.h"
#include "AP_MotorsHeli_RSC.h"
// rsc and aux function output channels
#define AP_MOTORS_HELI_SINGLE_RSC CH_8
#define AP_MOTORS_HELI_SINGLE_AUX CH_7
// servo position defaults
#define AP_MOTORS_HELI_SINGLE_SERVO1_POS -60
#define AP_MOTORS_HELI_SINGLE_SERVO2_POS 60
#define AP_MOTORS_HELI_SINGLE_SERVO3_POS 180
// swash type definitions
#define AP_MOTORS_HELI_SINGLE_SWASH_CCPM 0
#define AP_MOTORS_HELI_SINGLE_SWASH_H1 1
// 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 3
// default direct-drive variable pitch speed
#define AP_MOTOR_HELI_SINGLE_DDTAIL_DEFAULT 500
// default external gyro gain
#define AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN 350
// COLYAW parameter min and max values
#define AP_MOTORS_HELI_SINGLE_COLYAW_RANGE 10.0f
#define AP_MOTORS_HELI_SINGLE_TAIL_RAMP_INCREMENT 5 // 5 is 2 seconds for direct drive tail rotor to reach to full speed (5 = (2sec*100hz)/1000)
/// @class AP_MotorsHeli_Single
class AP_MotorsHeli_Single : public AP_MotorsHeli {
public:
// constructor
AP_MotorsHeli_Single(RC_Channel& servo_aux,
RC_Channel& servo_rsc,
RC_Channel& servo_1,
RC_Channel& servo_2,
RC_Channel& servo_3,
RC_Channel& servo_4,
uint16_t loop_rate,
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
AP_MotorsHeli(loop_rate, speed_hz),
_servo_aux(servo_aux),
_servo_1(servo_1),
_servo_2(servo_2),
_servo_3(servo_3),
_servo_4(servo_4),
_main_rotor(servo_rsc, AP_MOTORS_HELI_SINGLE_RSC, loop_rate),
_tail_rotor(servo_aux, AP_MOTORS_HELI_SINGLE_AUX, loop_rate)
{
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
void enable();
// output_test - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void output_test(uint8_t motor_seq, int16_t pwm);
// allow_arming - returns true if main rotor is spinning and it is ok to arm
bool allow_arming() const;
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
void set_desired_rotor_speed(int16_t desired_speed);
// get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000
int16_t get_estimated_rotor_speed() const { return _main_rotor.get_estimated_speed(); }
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000
int16_t get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); }
// 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_estimated_speed() > _main_rotor.get_critical_speed(); }
// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
void 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 get_motor_mask();
// _tail_type - returns the tail type (servo, servo with ext gyro, direct drive var pitch, direct drive fixed pitch)
int16_t tail_type() const { return _tail_type; }
// ext_gyro_gain - gets and sets external gyro gain as a pwm (1000~2000)
int16_t ext_gyro_gain() const { return _ext_gyro_gain; }
void ext_gyro_gain(int16_t pwm) { _ext_gyro_gain = pwm; }
// has_flybar - returns true if we have a mechical flybar
bool has_flybar() const { return _flybar_mode; }
// get_phase_angle - returns phase angle
int16_t get_phase_angle() const { return _phase_angle; }
// supports_yaw_passthrought - returns true if we support yaw passthrough
bool supports_yaw_passthrough() const { return _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO; }
// var_info
static const struct AP_Param::GroupInfo var_info[];
protected:
// output - sends commands to the motors
void output_armed_stabilizing();
void output_disarmed();
void output_yaw(int16_t yaw_out);
// reset_servos - free up the swash servos for maximum movement
void reset_servos();
// init_servos - initialize the servos
void init_servos();
// 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);
// write_aux - outputs pwm onto output aux channel (ch7). servo_out parameter is of the range 0 ~ 1000
void write_aux(int16_t servo_out);
// external objects we depend upon
RC_Channel& _servo_aux; // output to ext gyro gain and tail direct drive esc (ch7)
RC_Channel& _servo_1; // swash plate servo #1
RC_Channel& _servo_2; // swash plate servo #2
RC_Channel& _servo_3; // swash plate servo #3
RC_Channel& _servo_4; // tail servo
AP_MotorsHeli_RSC _main_rotor; // main rotor
AP_MotorsHeli_RSC _tail_rotor; // tail rotor
// parameters
AP_Int16 _servo1_pos; // Angular location of swash servo #1
AP_Int16 _servo2_pos; // Angular location of swash servo #2
AP_Int16 _servo3_pos; // Angular location of swash servo #3
AP_Int16 _tail_type; // Tail type used: Servo, Servo with external gyro, direct drive variable pitch or direct drive fixed pitch
AP_Int8 _swash_type; // Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing
AP_Int16 _ext_gyro_gain; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro
AP_Int16 _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem
AP_Float _collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.
AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode
AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000)
};
#endif // __AP_MOTORS_HELI_SINGLE_H__