Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 01:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsMatrix: use ref for roll, pitch, yaw, thr channels

Browse Source
This commit is contained in:
Randy Mackay 2014-02-10 13:22:18 +09:00 committed by Andrew Tridgell
parent 2f4fe3e192
commit cf1d6854b9
7 changed files with 31 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Motors/AP_MotorsHexa.h
View File

@ -16,7 +16,7 @@ class AP_MotorsHexa : public AP_MotorsMatrix {
public: public:
/// Constructor /// Constructor
AP_MotorsHexa( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { AP_MotorsHexa( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
}; };
// setup_motors - configures the motors for a quad // setup_motors - configures the motors for a quad

50
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -100,7 +100,7 @@ void AP_MotorsMatrix::output_min()
// fill the motor_out[] array for HIL use and send minimum value to each motor // fill the motor_out[] array for HIL use and send minimum value to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) { for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) { if( motor_enabled[i] ) {
hal.rcout->write(_motor_to_channel_map[i], _rc_throttle->radio_min); hal.rcout->write(_motor_to_channel_map[i], _rc_throttle.radio_min);
} }
} }
} }
@ -110,8 +110,8 @@ void AP_MotorsMatrix::output_min()
void AP_MotorsMatrix::output_armed() void AP_MotorsMatrix::output_armed()
{ {
int8_t i; int8_t i;
int16_t out_min_pwm = _rc_throttle->radio_min + _min_throttle; // minimum pwm value we can send to the motors int16_t out_min_pwm = _rc_throttle.radio_min + _min_throttle; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _rc_throttle->radio_max; // maximum pwm value we can send to the motors int16_t out_max_pwm = _rc_throttle.radio_max; // maximum pwm value we can send to the motors
int16_t out_mid_pwm = (out_min_pwm+out_max_pwm)/2; // mid pwm value we can send to the motors int16_t out_mid_pwm = (out_min_pwm+out_max_pwm)/2; // mid pwm value we can send to the motors
int16_t out_best_thr_pwm; // the is the best throttle we can come up which provides good control without climbing int16_t out_best_thr_pwm; // the is the best throttle we can come up which provides good control without climbing
float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits
@ -132,23 +132,23 @@ void AP_MotorsMatrix::output_armed()
// Throttle is 0 to 1000 only // 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 // To-Do: we should not really be limiting this here because we don't "own" this _rc_throttle object
if (_rc_throttle->servo_out < 0) { if (_rc_throttle.servo_out < 0) {
_rc_throttle->servo_out = 0; _rc_throttle.servo_out = 0;
limit.throttle_lower = true; limit.throttle_lower = true;
} }
if (_rc_throttle->servo_out > _max_throttle) { if (_rc_throttle.servo_out > _max_throttle) {
_rc_throttle->servo_out = _max_throttle; _rc_throttle.servo_out = _max_throttle;
limit.throttle_upper = true; limit.throttle_upper = true;
} }
// capture desired roll, pitch, yaw and throttle from receiver // capture desired roll, pitch, yaw and throttle from receiver
_rc_roll->calc_pwm(); _rc_roll.calc_pwm();
_rc_pitch->calc_pwm(); _rc_pitch.calc_pwm();
_rc_throttle->calc_pwm(); _rc_throttle.calc_pwm();
_rc_yaw->calc_pwm(); _rc_yaw.calc_pwm();
// if we are not sending a throttle output, we cut the motors // if we are not sending a throttle output, we cut the motors
if (_rc_throttle->servo_out == 0) { if (_rc_throttle.servo_out == 0) {
// range check spin_when_armed // range check spin_when_armed
if (_spin_when_armed_ramped < 0) { if (_spin_when_armed_ramped < 0) {
_spin_when_armed_ramped = 0; _spin_when_armed_ramped = 0;
@ -159,7 +159,7 @@ void AP_MotorsMatrix::output_armed()
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
// spin motors at minimum // spin motors at minimum
if (motor_enabled[i]) { if (motor_enabled[i]) {
motor_out[i] = _rc_throttle->radio_min + _spin_when_armed_ramped; motor_out[i] = _rc_throttle.radio_min + _spin_when_armed_ramped;
} }
} }
@ -171,7 +171,7 @@ void AP_MotorsMatrix::output_armed()
} else { } else {
// check if throttle is below limit // check if throttle is below limit
if (_rc_throttle->radio_out <= out_min_pwm) { // perhaps being at min throttle itself is not a problem, only being under is if (_rc_throttle.radio_out <= out_min_pwm) { // perhaps being at min throttle itself is not a problem, only being under is
limit.throttle_lower = true; limit.throttle_lower = true;
} }
@ -179,8 +179,8 @@ void AP_MotorsMatrix::output_armed()
// set rpy_low and rpy_high to the lowest and highest values of the motors // set rpy_low and rpy_high to the lowest and highest values of the motors
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) { if (motor_enabled[i]) {
rpy_out[i] = _rc_roll->pwm_out * _roll_factor[i] + rpy_out[i] = _rc_roll.pwm_out * _roll_factor[i] +
_rc_pitch->pwm_out * _pitch_factor[i]; _rc_pitch.pwm_out * _pitch_factor[i];
// record lowest roll pitch command // record lowest roll pitch command
if (rpy_out[i] < rpy_low) { if (rpy_out[i] < rpy_low) {
@ -203,25 +203,25 @@ void AP_MotorsMatrix::output_armed()
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control) // We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favour reducing throttle instead of better yaw control because the pilot has commanded it // We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favour reducing throttle instead of better yaw control because the pilot has commanded it
int16_t motor_mid = (rpy_low+rpy_high)/2; int16_t motor_mid = (rpy_low+rpy_high)/2;
out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle->radio_out, (_rc_throttle->radio_out+_hover_out)/2)); out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, (_rc_throttle.radio_out+_hover_out)/2));
// calculate amount of yaw we can fit into the throttle range // calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller // this is always equal to or less than the requested yaw from the pilot or rate controller
yaw_allowed = min(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2; yaw_allowed = min(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2;
yaw_allowed = max(yaw_allowed, AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM); yaw_allowed = max(yaw_allowed, AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM);
if (_rc_yaw->pwm_out >= 0) { if (_rc_yaw.pwm_out >= 0) {
// if yawing right // if yawing right
if (yaw_allowed > _rc_yaw->pwm_out) { if (yaw_allowed > _rc_yaw.pwm_out) {
yaw_allowed = _rc_yaw->pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output yaw_allowed = _rc_yaw.pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{ }else{
limit.yaw = true; limit.yaw = true;
} }
}else{ }else{
// if yawing left // if yawing left
yaw_allowed = -yaw_allowed; yaw_allowed = -yaw_allowed;
if( yaw_allowed < _rc_yaw->pwm_out ) { if( yaw_allowed < _rc_yaw.pwm_out ) {
yaw_allowed = _rc_yaw->pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output yaw_allowed = _rc_yaw.pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{ }else{
limit.yaw = true; limit.yaw = true;
} }
@ -247,7 +247,7 @@ void AP_MotorsMatrix::output_armed()
} }
// check everything fits // check everything fits
thr_adj = _rc_throttle->radio_out - out_best_thr_pwm; thr_adj = _rc_throttle.radio_out - out_best_thr_pwm;
// calc upper and lower limits of thr_adj // calc upper and lower limits of thr_adj
int16_t thr_adj_max = out_max_pwm-(out_best_thr_pwm+rpy_high); int16_t thr_adj_max = out_max_pwm-(out_best_thr_pwm+rpy_high);
@ -356,9 +356,9 @@ void AP_MotorsMatrix::output_test()
for( j=0; j<AP_MOTORS_MAX_NUM_MOTORS; j++ ) { for( j=0; j<AP_MOTORS_MAX_NUM_MOTORS; j++ ) {
if( motor_enabled[j] && _test_order[j] == i ) { if( motor_enabled[j] && _test_order[j] == i ) {
// turn on this motor and wait 1/3rd of a second // turn on this motor and wait 1/3rd of a second
hal.rcout->write(_motor_to_channel_map[j], _rc_throttle->radio_min + _min_throttle); hal.rcout->write(_motor_to_channel_map[j], _rc_throttle.radio_min + _min_throttle);
hal.scheduler->delay(300); hal.scheduler->delay(300);
hal.rcout->write(_motor_to_channel_map[j], _rc_throttle->radio_min); hal.rcout->write(_motor_to_channel_map[j], _rc_throttle.radio_min);
hal.scheduler->delay(2000); hal.scheduler->delay(2000);
} }
} }

2
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -21,7 +21,7 @@ class AP_MotorsMatrix : public AP_Motors {
public: public:
/// Constructor /// Constructor
AP_MotorsMatrix( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz)
{}; {};

2
libraries/AP_Motors/AP_MotorsOcta.h
View File

@ -16,7 +16,7 @@ class AP_MotorsOcta : public AP_MotorsMatrix {
public: public:
/// Constructor /// Constructor
AP_MotorsOcta( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { AP_MotorsOcta( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
}; };
// setup_motors - configures the motors for a quad // setup_motors - configures the motors for a quad

2
libraries/AP_Motors/AP_MotorsOctaQuad.h
View File

@ -16,7 +16,7 @@ class AP_MotorsOctaQuad : public AP_MotorsMatrix {
public: public:
/// Constructor /// Constructor
AP_MotorsOctaQuad( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { AP_MotorsOctaQuad( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
}; };
// setup_motors - configures the motors for a quad // setup_motors - configures the motors for a quad

2
libraries/AP_Motors/AP_MotorsQuad.h
View File

@ -16,7 +16,7 @@ class AP_MotorsQuad : public AP_MotorsMatrix {
public: public:
/// Constructor /// Constructor
AP_MotorsQuad( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { AP_MotorsQuad( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
}; };
// setup_motors - configures the motors for a quad // setup_motors - configures the motors for a quad

2
libraries/AP_Motors/AP_MotorsY6.h
View File

@ -18,7 +18,7 @@ class AP_MotorsY6 : public AP_MotorsMatrix {
public: public:
/// Constructor /// Constructor
AP_MotorsY6( RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) { AP_MotorsY6( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
}; };
// setup_motors - configures the motors for a quad // setup_motors - configures the motors for a quad