aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_MotorsMatrix: add normalise_rpy_factors

Browse Source
This commit is contained in:
Leonard Hall 2016-02-04 20:45:49 +09:00 committed by Randy Mackay
parent e7ba5ae451
commit 8f8eb7e214
2 changed files with 41 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -389,3 +389,41 @@ void AP_MotorsMatrix::remove_all_motors()
remove_motor(i);
}
}
// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
void AP_MotorsMatrix::normalise_rpy_factors()
{
float roll_fac = 0.0f;
float pitch_fac = 0.0f;
float yaw_fac = 0.0f;
// find maximum roll, pitch and yaw factors
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
if (roll_fac < fabsf(_roll_factor[i])) {
roll_fac = fabsf(_roll_factor[i]);
}
if (pitch_fac < fabsf(_pitch_factor[i])) {
pitch_fac = fabsf(_pitch_factor[i]);
}
if (yaw_fac < fabsf(_yaw_factor[i])) {
yaw_fac = fabsf(_yaw_factor[i]);
}
}
}
// scale factors back to -0.5 to +0.5 for each axis
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
if (!is_zero(roll_fac)) {
_roll_factor[i] = 0.5f*_roll_factor[i]/roll_fac;
}
if (!is_zero(pitch_fac)) {
_pitch_factor[i] = 0.5f*_pitch_factor[i]/pitch_fac;
}
if (!is_zero(yaw_fac)) {
_yaw_factor[i] = 0.5f*_yaw_factor[i]/yaw_fac;
}
}
}
}

3
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -71,6 +71,9 @@ protected:
// add_motor using raw roll, pitch, throttle and yaw factors
void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order);
// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
void normalise_rpy_factors();
float _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll
float _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch
float _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to yaw (normally 1 or -1)