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

Copter Motors: add slow start feature

Browse Source 
Combined armed, frame orientation and slow_start into flags bitmask
Removed ability to set max throttle because it was never used
Re-ordered class variables
This commit is contained in:
Randy Mackay 2013-09-12 22:27:44 +09:00
parent ac98644405
commit 62cb5c172b
7 changed files with 95 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Motors/AP_MotorsHexa.cpp
View File

@ -29,7 +29,7 @@ void AP_MotorsHexa::setup_motors()
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);

11
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -59,7 +59,7 @@ void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
void AP_MotorsMatrix::set_frame_orientation( uint8_t new_orientation )
{
// return if nothing has changed
if( new_orientation == _frame_orientation ) {
if( new_orientation == _flags.frame_orientation ) {
return;
}
@ -132,7 +132,14 @@ void AP_MotorsMatrix::output_armed()
// Throttle is 0 to 1000 only
// To-Do: we should not really be limiting this here because we don't "own" this _rc_throttle object
_rc_throttle->servo_out = constrain_int16(_rc_throttle->servo_out, 0, _max_throttle);
if (_rc_throttle->servo_out < 0) {
_rc_throttle->servo_out = 0;
limit.throttle_lower = true;
}
if (_rc_throttle->servo_out > _max_throttle) {
_rc_throttle->servo_out = _max_throttle;
limit.throttle_upper = true;
}
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll->calc_pwm();

4
libraries/AP_Motors/AP_MotorsOcta.cpp
View File

@ -29,7 +29,7 @@ void AP_MotorsOcta::setup_motors()
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
@ -40,7 +40,7 @@ void AP_MotorsOcta::setup_motors()
add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
}else if( _frame_orientation == AP_MOTORS_V_FRAME ) {
}else if( _flags.frame_orientation == AP_MOTORS_V_FRAME ) {
// V frame set-up
add_motor_raw(AP_MOTORS_MOT_1, 1.0, 0.34, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
add_motor_raw(AP_MOTORS_MOT_2, -1.0, -0.32, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);

2
libraries/AP_Motors/AP_MotorsOctaQuad.cpp
View File

@ -29,7 +29,7 @@ void AP_MotorsOctaQuad::setup_motors()
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);

6
libraries/AP_Motors/AP_MotorsQuad.cpp
View File

@ -29,21 +29,21 @@ void AP_MotorsQuad::setup_motors()
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _frame_orientation == AP_MOTORS_PLUS_FRAME ) {
if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
}else if( _frame_orientation == AP_MOTORS_V_FRAME ) {
}else if( _flags.frame_orientation == AP_MOTORS_V_FRAME ) {
// V frame set-up
add_motor(AP_MOTORS_MOT_1, 45, 0.7981, 1);
add_motor(AP_MOTORS_MOT_2, -135, 1.0000, 3);
add_motor(AP_MOTORS_MOT_3, -45, -0.7981, 4);
add_motor(AP_MOTORS_MOT_4, 135, -1.0000, 2);
}else if( _frame_orientation == AP_MOTORS_H_FRAME ) {
}else if( _flags.frame_orientation == AP_MOTORS_H_FRAME ) {
// H frame set-up - same as X but motors spin in opposite directiSons
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);

49
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -62,8 +62,6 @@ AP_Motors::AP_Motors( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_
_rc_throttle(rc_throttle),
_rc_yaw(rc_yaw),
_speed_hz(speed_hz),
_armed(false),
_frame_orientation(0),
_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
_max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE)
@ -94,8 +92,8 @@ void AP_Motors::Init()
void AP_Motors::armed(bool arm)
{
_armed = arm;
AP_Notify::flags.armed = _armed;
_flags.armed = arm;
AP_Notify::flags.armed = arm;
};
// set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
@ -124,6 +122,20 @@ void AP_Motors::throttle_pass_through()
}
}
// output - sends commands to the motors
void AP_Motors::output()
{
// update max throttle
update_max_throttle();
// output to motors
if (_flags.armed ) {
output_armed();
}else{
output_disarmed();
}
};
// setup_throttle_curve - used to linearlise thrust output by motors
// returns true if set up successfully
bool AP_Motors::setup_throttle_curve()
@ -160,3 +172,32 @@ bool AP_Motors::setup_throttle_curve()
return retval;
}
// 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_Motors::slow_start(bool true_false)
{
// set slow_start flag
_flags.slow_start = true;
// initialise maximum throttle to current throttle
_max_throttle = constrain_int16(_rc_throttle->servo_out, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
}
// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
void AP_Motors::update_max_throttle()
{
// return max throttle if we're not slow_starting
if (!_flags.slow_start) {
return;
}
// increase slow start throttle
_max_throttle += AP_MOTOR_SLOW_START_INCREMENT;
// turn off slow start if we've reached max throttle
if (_max_throttle >= _rc_throttle->servo_out) {
_max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
_flags.slow_start = false;
}
}

58
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -55,6 +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
/// @class AP_Motors
class AP_Motors {
public:
@ -81,26 +84,23 @@ public:
virtual void set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; };
// set frame orientation (normally + or X)
virtual void set_frame_orientation( uint8_t new_orientation ) { _frame_orientation = new_orientation; };
virtual void set_frame_orientation( uint8_t new_orientation ) { _flags.frame_orientation = new_orientation; };
// enable - starts allowing signals to be sent to motors
virtual void enable() = 0;
// arm, disarm or check status status of motors
bool armed() { return _armed; };
bool armed() { return _flags.armed; };
void armed(bool arm);
// set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
void set_min_throttle(uint16_t min_throttle);
void set_max_throttle(uint16_t max_throttle) { _max_throttle = max_throttle; };
// set_mid_throttle - sets the mid throttle which is close to the hover throttle of the copter
// 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_mid_throttle(uint16_t mid_throttle);
// output - sends commands to the motors
void output() {
if( _armed ) { output_armed(); }else{ output_disarmed(); }
};
void output();
// output_min - sends minimum values out to the motors
virtual void output_min() = 0;
@ -118,6 +118,10 @@ public:
// 1 if motor is enabled, 0 otherwise
bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS];
// 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);
// final output values sent to the motors. public (for now) so that they can be access for logging
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS];
@ -135,26 +139,32 @@ public:
protected:
// output functions that should be overloaded by child classes
virtual void output_armed() {
};
virtual void output_disarmed() {
};
virtual void output_armed() {};
virtual void output_disarmed() {};
// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
void update_max_throttle();
// flag bitmask
struct AP_Motors_flags {
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
} _flags;
// parameters
AP_CurveInt16_Size4 _throttle_curve; // curve used to linearize the pwm->thrust
AP_Int8 _throttle_curve_enabled; // enable throttle curve
AP_Int8 _throttle_curve_mid; // throttle which produces 1/2 the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
AP_Int8 _throttle_curve_max; // throttle which produces the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
AP_Int8 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min
// internal variables
RC_Channel* _rc_roll, *_rc_pitch, *_rc_throttle, *_rc_yaw; // input in from users
uint8_t _motor_to_channel_map[AP_MOTORS_MAX_NUM_MOTORS]; // mapping of motor number (as received from upper APM code) to RC channel output - used to account for differences between APM1 and APM2
uint16_t _speed_hz; // speed in hz to send updates to motors
bool _armed; // true if motors are armed
uint8_t _frame_orientation; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2
int16_t _min_throttle; // the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle)
int16_t _max_throttle; // the minimum throttle to be sent to the engines when they're on (prevents issues with some motors on while other off at very low throttle)
AP_CurveInt16_Size4 _throttle_curve; // curve used to linearize the pwm->thrust
AP_Int8 _throttle_curve_enabled; // enable throttle curve
AP_Int8 _throttle_curve_mid; // throttle which produces 1/2 the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
AP_Int8 _throttle_curve_max; // throttle which produces the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
// for new stability patch
int16_t _hover_out; // the estimated hover throttle in pwm (i.e. 1000 ~ 2000). calculated from the THR_MID parameter
AP_Int8 _spin_when_armed; // used to control whether the motors always spin when armed. pwm value above radio_min
uint16_t _speed_hz; // speed in hz to send updates to motors
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
};
#endif // __AP_MOTORS_CLASS_H__