mirror of https://github.com/ArduPilot/ardupilot
AP_MotorsHeli: add spool logic support
Also moved heli init_target_on_arming flag in from vehicle code
This commit is contained in:
parent
ac87b3e1e5
commit
28f4c68f2f
|
@ -229,6 +229,10 @@ void AP_MotorsHeli::init(motor_frame_class frame_class, motor_frame_type frame_t
|
|||
|
||||
// record successful initialisation if what we setup was the desired frame_class
|
||||
_flags.initialised_ok = (frame_class == MOTOR_FRAME_HELI);
|
||||
|
||||
// set flag to true so targets are initialized once aircraft is armed for first time
|
||||
_heliflags.init_targets_on_arming = true;
|
||||
|
||||
}
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
|
@ -258,6 +262,9 @@ void AP_MotorsHeli::output()
|
|||
// update throttle filter
|
||||
update_throttle_filter();
|
||||
|
||||
// run spool logic
|
||||
output_logic();
|
||||
|
||||
if (_flags.armed) {
|
||||
calculate_armed_scalars();
|
||||
if (!_flags.interlock) {
|
||||
|
@ -268,6 +275,9 @@ void AP_MotorsHeli::output()
|
|||
} else {
|
||||
output_disarmed();
|
||||
}
|
||||
|
||||
output_to_motors();
|
||||
|
||||
};
|
||||
|
||||
// sends commands to the motors
|
||||
|
@ -279,8 +289,6 @@ void AP_MotorsHeli::output_armed_stabilizing()
|
|||
}
|
||||
|
||||
move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
|
||||
|
||||
update_motor_control(ROTOR_CONTROL_ACTIVE);
|
||||
}
|
||||
|
||||
// output_armed_zero_throttle - sends commands to the motors
|
||||
|
@ -292,8 +300,6 @@ void AP_MotorsHeli::output_armed_zero_throttle()
|
|||
}
|
||||
|
||||
move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
|
||||
|
||||
update_motor_control(ROTOR_CONTROL_IDLE);
|
||||
}
|
||||
|
||||
// output_disarmed - sends commands to the motors
|
||||
|
@ -351,8 +357,92 @@ void AP_MotorsHeli::output_disarmed()
|
|||
|
||||
// helicopters always run stabilizing flight controls
|
||||
move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
|
||||
}
|
||||
|
||||
update_motor_control(ROTOR_CONTROL_STOP);
|
||||
// run spool logic
|
||||
void AP_MotorsHeli::output_logic()
|
||||
{
|
||||
// force desired and current spool mode if disarmed and armed with interlock enabled
|
||||
if (_flags.armed) {
|
||||
if (!_flags.interlock) {
|
||||
_spool_desired = DESIRED_GROUND_IDLE;
|
||||
} else {
|
||||
_heliflags.init_targets_on_arming = false;
|
||||
}
|
||||
} else {
|
||||
_heliflags.init_targets_on_arming = true;
|
||||
_spool_desired = DESIRED_SHUT_DOWN;
|
||||
_spool_mode = SHUT_DOWN;
|
||||
}
|
||||
|
||||
switch (_spool_mode) {
|
||||
case SHUT_DOWN:
|
||||
// Motors should be stationary.
|
||||
// Servos set to their trim values or in a test condition.
|
||||
|
||||
// make sure the motors are spooling in the correct direction
|
||||
if (_spool_desired != DESIRED_SHUT_DOWN) {
|
||||
_spool_mode = GROUND_IDLE;
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case GROUND_IDLE: {
|
||||
// Motors should be stationary or at ground idle.
|
||||
// Servos should be moving to correct the current attitude.
|
||||
if (_spool_desired == DESIRED_SHUT_DOWN){
|
||||
_spool_mode = SHUT_DOWN;
|
||||
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
|
||||
_spool_mode = SPOOL_UP;
|
||||
} else { // _spool_desired == GROUND_IDLE
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case SPOOL_UP:
|
||||
// Maximum throttle should move from minimum to maximum.
|
||||
// Servos should exhibit normal flight behavior.
|
||||
|
||||
// make sure the motors are spooling in the correct direction
|
||||
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED ){
|
||||
_spool_mode = SPOOL_DOWN;
|
||||
break;
|
||||
}
|
||||
|
||||
if (_heliflags.rotor_runup_complete){
|
||||
_spool_mode = THROTTLE_UNLIMITED;
|
||||
}
|
||||
break;
|
||||
|
||||
case THROTTLE_UNLIMITED:
|
||||
// Throttle should exhibit normal flight behavior.
|
||||
// Servos should exhibit normal flight behavior.
|
||||
|
||||
// make sure the motors are spooling in the correct direction
|
||||
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED) {
|
||||
_spool_mode = SPOOL_DOWN;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
|
||||
case SPOOL_DOWN:
|
||||
// Maximum throttle should move from maximum to minimum.
|
||||
// Servos should exhibit normal flight behavior.
|
||||
|
||||
// make sure the motors are spooling in the correct direction
|
||||
if (_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
|
||||
_spool_mode = SPOOL_UP;
|
||||
break;
|
||||
}
|
||||
if (!rotor_speed_above_critical()){
|
||||
_spool_mode = GROUND_IDLE;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// parameter_check - check if helicopter specific parameters are sensible
|
||||
|
|
|
@ -129,6 +129,9 @@ public:
|
|||
|
||||
float get_throttle_hover() const override { return 0.5f; }
|
||||
|
||||
// support passing init_targets_on_arming flag to greater code
|
||||
bool init_targets_on_arming() const { return _heliflags.init_targets_on_arming; }
|
||||
|
||||
// var_info for holding Parameter information
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
|
@ -152,6 +155,12 @@ protected:
|
|||
// update_motor_controls - sends commands to motor controllers
|
||||
virtual void update_motor_control(RotorControlState state) = 0;
|
||||
|
||||
// run spool logic
|
||||
void output_logic();
|
||||
|
||||
// output_to_motors - sends commands to the motors
|
||||
virtual void output_to_motors() = 0;
|
||||
|
||||
// reset_flight_controls - resets all controls and scalars to flight status
|
||||
void reset_flight_controls();
|
||||
|
||||
|
@ -188,6 +197,7 @@ protected:
|
|||
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 inverted_flight : 1; // true for inverted flight
|
||||
uint8_t init_targets_on_arming : 1; // 0 if targets were initialized, 1 if targets were not initialized after arming
|
||||
} _heliflags;
|
||||
|
||||
// parameters
|
||||
|
|
|
@ -506,27 +506,51 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
|
|||
_rotor.set_collective(fabsf(collective_out));
|
||||
|
||||
// swashplate servos
|
||||
float servo_out[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
|
||||
|
||||
servo_out[CH_1] = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
|
||||
servo_out[CH_2] = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out + _yawFactor[CH_2] * yaw_out)*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
|
||||
servo_out[CH_3] = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out + _yawFactor[CH_3] * yaw_out)*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
|
||||
_servo_out[CH_1] = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
|
||||
_servo_out[CH_2] = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out + _yawFactor[CH_2] * yaw_out)*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
|
||||
_servo_out[CH_3] = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out + _yawFactor[CH_3] * yaw_out)*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
|
||||
|
||||
servo_out[CH_4] = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)*0.45f + _collectiveFactor[CH_4] * collective2_out_scaled;
|
||||
servo_out[CH_5] = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)*0.45f + _collectiveFactor[CH_5] * collective2_out_scaled;
|
||||
servo_out[CH_6] = (_rollFactor[CH_6] * roll_out + _pitchFactor[CH_6] * pitch_out + _yawFactor[CH_6] * yaw_out)*0.45f + _collectiveFactor[CH_6] * collective2_out_scaled;
|
||||
_servo_out[CH_4] = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)*0.45f + _collectiveFactor[CH_4] * collective2_out_scaled;
|
||||
_servo_out[CH_5] = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)*0.45f + _collectiveFactor[CH_5] * collective2_out_scaled;
|
||||
_servo_out[CH_6] = (_rollFactor[CH_6] * roll_out + _pitchFactor[CH_6] * pitch_out + _yawFactor[CH_6] * yaw_out)*0.45f + _collectiveFactor[CH_6] * collective2_out_scaled;
|
||||
|
||||
// rescale from -1..1, so we can use the pwm calc that includes trim
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
servo_out[i] = 2*servo_out[i] - 1;
|
||||
}
|
||||
|
||||
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
rc_write_swash(i, servo_out[i]);
|
||||
_servo_out[i] = 2*_servo_out[i] - 1;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_MotorsHeli_Dual::output_to_motors()
|
||||
{
|
||||
if (!_flags.initialised_ok) {
|
||||
return;
|
||||
}
|
||||
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
rc_write_swash(i, _servo_out[i]);
|
||||
}
|
||||
|
||||
switch (_spool_mode) {
|
||||
case SHUT_DOWN:
|
||||
// sends minimum values out to the motors
|
||||
update_motor_control(ROTOR_CONTROL_STOP);
|
||||
break;
|
||||
case GROUND_IDLE:
|
||||
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
|
||||
update_motor_control(ROTOR_CONTROL_IDLE);
|
||||
break;
|
||||
case SPOOL_UP:
|
||||
case THROTTLE_UNLIMITED:
|
||||
// set motor output based on thrust requests
|
||||
update_motor_control(ROTOR_CONTROL_ACTIVE);
|
||||
break;
|
||||
case SPOOL_DOWN:
|
||||
// sends idle output to motors and wait for rotor to stop
|
||||
update_motor_control(ROTOR_CONTROL_IDLE);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// servo_test - move servos through full range of movement
|
||||
void AP_MotorsHeli_Dual::servo_test()
|
||||
|
|
|
@ -61,6 +61,9 @@ public:
|
|||
// output_test_seq - spin a motor at the pwm value specified
|
||||
virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override;
|
||||
|
||||
// output_to_motors - sends values out to the motors
|
||||
void output_to_motors() override;
|
||||
|
||||
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
|
||||
void set_desired_rotor_speed(float desired_speed) override;
|
||||
|
||||
|
@ -117,6 +120,7 @@ protected:
|
|||
float _collective_test = 0.0f; // over-ride for collective output, used by servo_test function
|
||||
float _roll_test = 0.0f; // over-ride for roll output, used by servo_test function
|
||||
float _pitch_test = 0.0f; // over-ride for pitch output, used by servo_test function
|
||||
float _servo_out[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS]; // output value sent to motor
|
||||
|
||||
// parameters
|
||||
AP_Int16 _collective2_min; // Lowest possible servo position for the rear swashplate
|
||||
|
|
|
@ -232,10 +232,8 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
|
|||
// reserve some collective for attitude control
|
||||
collective_out *= collective_range;
|
||||
|
||||
float out[AP_MOTORS_HELI_QUAD_NUM_MOTORS] {};
|
||||
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
|
||||
out[i] =
|
||||
_out[i] =
|
||||
_rollFactor[CH_1+i] * roll_out +
|
||||
_pitchFactor[CH_1+i] * pitch_out +
|
||||
_collectiveFactor[CH_1+i] * collective_out;
|
||||
|
@ -244,16 +242,16 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
|
|||
// see if we need to scale down yaw_out
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
|
||||
float y = _yawFactor[CH_1+i] * yaw_out;
|
||||
if (out[i] < 0) {
|
||||
if (_out[i] < 0) {
|
||||
// the slope of the yaw effect changes at zero collective
|
||||
y = -y;
|
||||
}
|
||||
if (out[i] * (out[i] + y) < 0) {
|
||||
if (_out[i] * (_out[i] + y) < 0) {
|
||||
// applying this yaw demand would change the sign of the
|
||||
// collective, which means the yaw would not be applied
|
||||
// evenly. We scale down the overall yaw demand to prevent
|
||||
// it crossing over zero
|
||||
float s = -(out[i] / y);
|
||||
float s = -(_out[i] / y);
|
||||
yaw_out *= s;
|
||||
}
|
||||
}
|
||||
|
@ -261,20 +259,47 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
|
|||
// now apply the yaw correction
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
|
||||
float y = _yawFactor[CH_1+i] * yaw_out;
|
||||
if (out[i] < 0) {
|
||||
if (_out[i] < 0) {
|
||||
// the slope of the yaw effect changes at zero collective
|
||||
y = -y;
|
||||
}
|
||||
out[i] += y;
|
||||
_out[i] += y;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void AP_MotorsHeli_Quad::output_to_motors()
|
||||
{
|
||||
if (!_flags.initialised_ok) {
|
||||
return;
|
||||
}
|
||||
|
||||
// move the servos
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
|
||||
rc_write_angle(AP_MOTORS_MOT_1+i, out[i] * QUAD_SERVO_MAX_ANGLE);
|
||||
rc_write_angle(AP_MOTORS_MOT_1+i, _out[i] * QUAD_SERVO_MAX_ANGLE);
|
||||
}
|
||||
|
||||
switch (_spool_mode) {
|
||||
case SHUT_DOWN:
|
||||
// sends minimum values out to the motors
|
||||
update_motor_control(ROTOR_CONTROL_STOP);
|
||||
break;
|
||||
case GROUND_IDLE:
|
||||
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
|
||||
update_motor_control(ROTOR_CONTROL_IDLE);
|
||||
break;
|
||||
case SPOOL_UP:
|
||||
case THROTTLE_UNLIMITED:
|
||||
// set motor output based on thrust requests
|
||||
update_motor_control(ROTOR_CONTROL_ACTIVE);
|
||||
break;
|
||||
case SPOOL_DOWN:
|
||||
// sends idle output to motors and wait for rotor to stop
|
||||
update_motor_control(ROTOR_CONTROL_IDLE);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// servo_test - move servos through full range of movement
|
||||
void AP_MotorsHeli_Quad::servo_test()
|
||||
{
|
||||
|
|
|
@ -36,6 +36,9 @@ public:
|
|||
// output_test_seq - spin a motor at the pwm value specified
|
||||
virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override;
|
||||
|
||||
// output_to_motors - sends values out to the motors
|
||||
void output_to_motors() override;
|
||||
|
||||
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
|
||||
void set_desired_rotor_speed(float desired_speed) override;
|
||||
|
||||
|
@ -91,6 +94,7 @@ protected:
|
|||
float _pitchFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS];
|
||||
float _collectiveFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS];
|
||||
float _yawFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS];
|
||||
float _out[AP_MOTORS_HELI_QUAD_NUM_MOTORS];
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -461,23 +461,18 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
|
|||
if (_collective_direction == AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_REVERSED){
|
||||
collective_out_scaled = 1 - collective_out_scaled;
|
||||
}
|
||||
float servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
|
||||
float servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
|
||||
_servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
|
||||
_servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
|
||||
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H1) {
|
||||
servo1_out += 0.5f;
|
||||
servo2_out += 0.5f;
|
||||
_servo1_out += 0.5f;
|
||||
_servo2_out += 0.5f;
|
||||
}
|
||||
float servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
|
||||
_servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
|
||||
|
||||
// rescale from -1..1, so we can use the pwm calc that includes trim
|
||||
servo1_out = 2*servo1_out - 1;
|
||||
servo2_out = 2*servo2_out - 1;
|
||||
servo3_out = 2*servo3_out - 1;
|
||||
|
||||
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
|
||||
rc_write_swash(AP_MOTORS_MOT_1, servo1_out);
|
||||
rc_write_swash(AP_MOTORS_MOT_2, servo2_out);
|
||||
rc_write_swash(AP_MOTORS_MOT_3, servo3_out);
|
||||
_servo1_out = 2*_servo1_out - 1;
|
||||
_servo2_out = 2*_servo2_out - 1;
|
||||
_servo3_out = 2*_servo3_out - 1;
|
||||
|
||||
// update the yaw rate using the tail rotor/servo
|
||||
move_yaw(yaw_out + yaw_offset);
|
||||
|
@ -496,18 +491,21 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out)
|
|||
limit.yaw = true;
|
||||
}
|
||||
|
||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
|
||||
if (_main_rotor.get_desired_speed() > 0.0f && hal.util->get_soft_armed()) {
|
||||
// constrain output so that motor never fully stops
|
||||
yaw_out = constrain_float(yaw_out, -0.9f, 1.0f);
|
||||
// output yaw servo to tail rsc
|
||||
rc_write_angle(AP_MOTORS_MOT_4, yaw_out * YAW_SERVO_MAX_ANGLE);
|
||||
} else {
|
||||
// output zero speed to tail rsc
|
||||
rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
|
||||
}
|
||||
} else {
|
||||
rc_write_angle(AP_MOTORS_MOT_4, yaw_out * YAW_SERVO_MAX_ANGLE);
|
||||
_servo4_out = yaw_out;
|
||||
}
|
||||
|
||||
void AP_MotorsHeli_Single::output_to_motors()
|
||||
{
|
||||
if (!_flags.initialised_ok) {
|
||||
return;
|
||||
}
|
||||
|
||||
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
|
||||
rc_write_swash(AP_MOTORS_MOT_1, _servo1_out);
|
||||
rc_write_swash(AP_MOTORS_MOT_2, _servo2_out);
|
||||
rc_write_swash(AP_MOTORS_MOT_3, _servo3_out);
|
||||
if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
|
||||
rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
|
||||
}
|
||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
|
||||
// output gain to exernal gyro
|
||||
|
@ -517,6 +515,42 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out)
|
|||
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std);
|
||||
}
|
||||
}
|
||||
|
||||
switch (_spool_mode) {
|
||||
case SHUT_DOWN:
|
||||
// sends minimum values out to the motors
|
||||
update_motor_control(ROTOR_CONTROL_STOP);
|
||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
|
||||
rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
|
||||
}
|
||||
break;
|
||||
case GROUND_IDLE:
|
||||
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
|
||||
update_motor_control(ROTOR_CONTROL_IDLE);
|
||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
|
||||
rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
|
||||
}
|
||||
break;
|
||||
case SPOOL_UP:
|
||||
case THROTTLE_UNLIMITED:
|
||||
// set motor output based on thrust requests
|
||||
update_motor_control(ROTOR_CONTROL_ACTIVE);
|
||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
|
||||
// constrain output so that motor never fully stops
|
||||
_servo4_out = constrain_float(_servo4_out, -0.9f, 1.0f);
|
||||
// output yaw servo to tail rsc
|
||||
rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
|
||||
}
|
||||
break;
|
||||
case SPOOL_DOWN:
|
||||
// sends idle output to motors and wait for rotor to stop
|
||||
update_motor_control(ROTOR_CONTROL_IDLE);
|
||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
|
||||
rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// servo_test - move servos through full range of movement
|
||||
|
|
|
@ -66,6 +66,9 @@ public:
|
|||
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
|
||||
virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override;
|
||||
|
||||
// output_to_motors - sends values out to the motors
|
||||
void output_to_motors() override;
|
||||
|
||||
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1
|
||||
void set_desired_rotor_speed(float desired_speed) override;
|
||||
|
||||
|
@ -136,6 +139,10 @@ protected:
|
|||
float _roll_test = 0.0f; // over-ride for roll output, used by servo_test function
|
||||
float _pitch_test = 0.0f; // over-ride for pitch output, used by servo_test function
|
||||
float _yaw_test = 0.0f; // over-ride for yaw output, used by servo_test function
|
||||
float _servo1_out = 0.0f; // output value sent to motor
|
||||
float _servo2_out = 0.0f; // output value sent to motor
|
||||
float _servo3_out = 0.0f; // output value sent to motor
|
||||
float _servo4_out = 0.0f; // output value sent to motor
|
||||
|
||||
// parameters
|
||||
AP_Int16 _servo1_pos; // Angular location of swash servo #1
|
||||
|
|
Loading…
Reference in New Issue