Copter: slow start motors from 0 to min throttle

This commit is contained in:
Randy Mackay 2013-10-20 14:51:35 +09:00
parent 5499c6ea6d
commit 943d7374f6
4 changed files with 34 additions and 15 deletions

View File

@ -150,16 +150,16 @@ void AP_MotorsMatrix::output_armed()
// if we are not sending a throttle output, we cut the motors
if (_rc_throttle->servo_out == 0) {
// range check spin_when_armed
if (_spin_when_armed < 0) {
_spin_when_armed = 0;
if (_spin_when_armed_ramped < 0) {
_spin_when_armed_ramped = 0;
}
if (_spin_when_armed > _min_throttle) {
_spin_when_armed = _min_throttle;
if (_spin_when_armed_ramped > _min_throttle) {
_spin_when_armed_ramped = _min_throttle;
}
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
// spin motors at minimum
if (motor_enabled[i]) {
motor_out[i] = _rc_throttle->radio_min + _spin_when_armed;
motor_out[i] = _rc_throttle->radio_min + _spin_when_armed_ramped;
}
}

View File

@ -102,15 +102,15 @@ void AP_MotorsTri::output_armed()
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle->servo_out == 0) {
// range check spin_when_armed
if (_spin_when_armed < 0) {
_spin_when_armed = 0;
if (_spin_when_armed_ramped < 0) {
_spin_when_armed_ramped = 0;
}
if (_spin_when_armed > _min_throttle) {
_spin_when_armed = _min_throttle;
if (_spin_when_armed_ramped > _min_throttle) {
_spin_when_armed_ramped = _min_throttle;
}
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min + _spin_when_armed;
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min + _spin_when_armed;
motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min + _spin_when_armed;
motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min + _spin_when_armed_ramped;
// Every thing is limited
limit.throttle_lower = true;

View File

@ -64,7 +64,8 @@ AP_Motors::AP_Motors( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_
_speed_hz(speed_hz),
_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
_max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE)
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
_spin_when_armed_ramped(0)
{
uint8_t i;
@ -77,6 +78,9 @@ AP_Motors::AP_Motors( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_
set_motor_to_channel_map(APM2_MOTOR_TO_CHANNEL_MAP);
#endif
// slow start motors from zero to min throttle
_flags.slow_start_low_end = true;
// clear output arrays
for(i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
motor_out[i] = 0;
@ -93,6 +97,9 @@ void AP_Motors::Init()
void AP_Motors::armed(bool arm)
{
_flags.armed = arm;
if (!_flags.armed) {
_flags.slow_start_low_end = true;
}
AP_Notify::flags.armed = arm;
};
@ -187,6 +194,15 @@ void AP_Motors::slow_start(bool true_false)
// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
void AP_Motors::update_max_throttle()
{
// ramp up minimum spin speed if necessary
if (_flags.slow_start_low_end) {
_spin_when_armed_ramped += AP_MOTOR_SLOW_START_LOW_END_INCREMENT;
if (_spin_when_armed_ramped >= _spin_when_armed) {
_spin_when_armed_ramped = _spin_when_armed;
_flags.slow_start_low_end = false;
}
}
// return max throttle if we're not slow_starting
if (!_flags.slow_start) {
return;

View File

@ -55,8 +55,9 @@
#define AP_MOTOR_THROTTLE_LIMIT 0x04
#define AP_MOTOR_ANY_LIMIT 0xFF
// slow start increment - max throttle increase per (100hz) iteration. i.e. 10 = full speed in 1 second
#define AP_MOTOR_SLOW_START_INCREMENT 10
// slow start increments - throttle increase per (100hz) iteration. i.e. 5 = full speed in 2 seconds
#define AP_MOTOR_SLOW_START_INCREMENT 10 // max throttle ramp speed (i.e. motors can reach full throttle in 2 seconds)
#define AP_MOTOR_SLOW_START_LOW_END_INCREMENT 2 // min throttle ramp speed (i.e. motors will speed up from zero to _spin_when_armed speed in about 1 second)
/// @class AP_Motors
class AP_Motors {
@ -150,6 +151,7 @@ protected:
uint8_t armed : 1; // 1 if the motors are armed, 0 if disarmed
uint8_t frame_orientation : 2; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2, H_FRAME 3
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
} _flags;
// parameters
@ -166,5 +168,6 @@ 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 in pwm (i.e. 1000 ~ 2000). calculated from the THR_MID parameter
int16_t _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero
};
#endif // __AP_MOTORS_CLASS_H__