Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsHeli: add spool logic support 

Also moved heli init_target_on_arming flag in from vehicle code
This commit is contained in:
bnsgeyer 2018-12-28 17:02:05 +10:30 committed by Randy Mackay
parent ac87b3e1e5
commit 28f4c68f2f
8 changed files with 252 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -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

10
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -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

52
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -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()

4
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -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

45
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -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()
{

4
libraries/AP_Motors/AP_MotorsHeli_Quad.h
View File

@ -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];
};

82
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -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);
_servo4_out = yaw_out;
}
void AP_MotorsHeli_Single::output_to_motors()
{
if (!_flags.initialised_ok) {
return;
}
} else {
rc_write_angle(AP_MOTORS_MOT_4, yaw_out * YAW_SERVO_MAX_ANGLE);
// 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

7
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -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