Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsMulticopter: output_logic implements spooling

Browse Source
This commit is contained in:
Leonard Hall 2016-01-19 14:49:46 +09:00 committed by Randy Mackay
parent b7fe6aa16e
commit b26318c178
2 changed files with 196 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

178
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -143,21 +143,14 @@ void AP_MotorsMulticopter::output()
// calc filtered battery voltage and lift_max
update_lift_max_from_batt_voltage();
// move throttle_low_comp towards desired throttle low comp
update_throttle_rpy_mix();
// run spool logic
output_logic();
if (_flags.armed) {
if (!_flags.interlock) {
output_armed_zero_throttle();
} else if (_flags.stabilizing) {
output_armed_stabilizing();
} else {
output_armed_not_stabilizing();
}
} else {
_multicopter_flags.slow_start_low_end = true;
output_disarmed();
}
// calculate thrust
output_armed_stabilizing();
// convert rpy_thrust values to pwm
output_to_motors();
};
// update the throttle input filter
@ -381,6 +374,163 @@ void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t rad
_min_throttle = (float)min_throttle * _throttle_pwm_scalar;
}
void AP_MotorsMulticopter::output_logic()
{
// force desired and current spool mode if disarmed or not interlocked
if (!_flags.armed || !_flags.interlock) {
_multicopter_flags.spool_desired = DESIRED_SHUT_DOWN;
_multicopter_flags.spool_mode = SHUT_DOWN;
_multicopter_flags.slow_start_low_end = true;
}
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// make sure the motors are spooling in the correct direction
if(_multicopter_flags.spool_desired != DESIRED_SHUT_DOWN){
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 0.0f;
_throttle_thrust_max = 0.0f;
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
break;
case SPIN_WHEN_ARMED:{
// Motors should be stationary or at spin when armed.
// Servoes should be moving to correct the current attitude.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// set and increment ramp variables
float spool_step = 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
if(_multicopter_flags.spool_desired == DESIRED_SHUT_DOWN){
_throttle_low_end_pct -= spool_step;
} else if(_multicopter_flags.spool_desired == DESIRED_THROTTLE_UNLIMITED ){
_throttle_low_end_pct += spool_step;
}else{
_throttle_low_end_pct += constrain_float(spin_when_armed_low_end_pct()-_throttle_low_end_pct, -spool_step, spool_step);
}
_throttle_thrust_max = 0.0f;
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
// constrain ramp value and update mode
if (_throttle_low_end_pct <= 0.0f) {
_throttle_low_end_pct = 0.0f;
_multicopter_flags.spool_mode = SHUT_DOWN;
} else if (_throttle_low_end_pct >= 1.0f) {
_throttle_low_end_pct = 1.0f;
_multicopter_flags.spool_mode = SPOOL_UP;
}
break;
}
case SPOOL_UP:
// Maximum throttle should move from minimum to maximum.
// Servoes should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if(_multicopter_flags.spool_desired != DESIRED_THROTTLE_UNLIMITED ){
_multicopter_flags.spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max += 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
// constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
_throttle_thrust_max = get_current_limit_max_throttle();
_multicopter_flags.spool_mode = THROTTLE_UNLIMITED;
} else if (_throttle_thrust_max < 0.0f) {
_throttle_thrust_max = 0.0f;
}
break;
case THROTTLE_UNLIMITED:
// Throttle should exhibit normal flight behavior.
// Servoes should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if(_multicopter_flags.spool_desired != DESIRED_THROTTLE_UNLIMITED ){
_multicopter_flags.spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max = get_current_limit_max_throttle();
update_throttle_rpy_mix();
break;
case SPOOL_DOWN:
// Maximum throttle should move from maximum to minimum.
// Servoes should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if(_multicopter_flags.spool_desired == DESIRED_THROTTLE_UNLIMITED ){
_multicopter_flags.spool_mode = SPOOL_UP;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix_desired = _throttle_rpy_mix;
// constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f){
_throttle_thrust_max = 0.0f;
}
if (_throttle_rpy_mix <= 0.0f){
_throttle_rpy_mix = 0.0f;
}
if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max) && is_zero(_throttle_rpy_mix)) {
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
}
break;
}
}
// slow_start - set to true to slew motors from current speed to maximum
// Note: this must be set immediately before a step up in throttle
void AP_MotorsMulticopter::slow_start(bool true_false)

32
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -39,6 +39,9 @@
#define AP_MOTOR_SLOW_START_LOW_END_INCREMENT 1 // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 0.3 second)
#endif
// spool definition
#define AP_MOTORS_SPOOL_UP_TIME 0.5f // time (in seconds) for throttle to increase from zero to min throttle, and min throttle to full throttle.
/// @class AP_MotorsMulticopter
class AP_MotorsMulticopter : public AP_Motors {
public:
@ -49,6 +52,9 @@ public:
// output - sends commands to the motors
virtual void output();
// output_to_motors - sends commands to the motors
virtual void output_to_motors(){};
// set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm)
void set_yaw_headroom(int16_t pwm) { _yaw_headroom = pwm; }
@ -73,6 +79,26 @@ public:
// this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control
void set_hover_throttle(uint16_t hov_thr) { _hover_out = hov_thr; }
// spool up states
enum spool_up_down_mode {
SHUT_DOWN = 0, // all motors stop
SPIN_WHEN_ARMED = 1, // all motors at spin when armed
SPOOL_UP = 2, // increasing maximum throttle while stabilizing
THROTTLE_UNLIMITED = 3, // throttle is no longer constrained by start up procedure
SPOOL_DOWN = 4, // decreasing maximum throttle while stabilizing
};
// spool up states
enum spool_up_down_desired {
DESIRED_SHUT_DOWN = 0, // all motors stop
DESIRED_SPIN_WHEN_ARMED = 1, // all motors at spin when armed
DESIRED_SPIN_MIN_THROTTLE = 2, // all motors at min throttle
DESIRED_THROTTLE_UNLIMITED = 3, // throttle is no longer constrained by start up procedure
};
void set_desired_spool_state(enum spool_up_down_desired spool) { _multicopter_flags.spool_desired = spool;}
void output_logic();
// slow_start - set to true to slew motors from current speed to maximum
// Note: this must be set immediately before a step up in throttle
void slow_start(bool true_false);
@ -146,6 +172,8 @@ protected:
// flag bitmask
struct {
spool_up_down_mode spool_mode : 4; // motor's current spool mode
spool_up_down_desired spool_desired : 2; // caller's desired spool mode
uint8_t slow_start : 1; // 1 if slow start is active
uint8_t slow_start_low_end : 1; // 1 just after arming so we can ramp up the spin_when_armed value
} _multicopter_flags;
@ -170,6 +198,10 @@ protected:
int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start)
int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
float _throttle_thrust_max; // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max
// spool variables
float _throttle_low_end_pct; // throttle percentage (0 ~ 1) between zero and throttle_min
// battery voltage, current and air pressure compensation variables
float _batt_voltage_resting; // battery voltage reading at minimum throttle