Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: add throttle factor to Matrix

Browse Source
This commit is contained in:
Peter Hall 2021-05-16 23:45:22 +01:00 committed by Randy Mackay
parent fced5271af
commit 42538c7083
4 changed files with 44 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Motors/AP_Motors6DOF.h
View File

@ -69,7 +69,6 @@ protected:
AP_Int8 _motor_reverse[AP_MOTORS_MAX_NUM_MOTORS];
AP_Float _forwardVerticalCouplingFactor;
float _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to throttle (climb/descent)
float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward/backward
float _lateral_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to lateral (left/right)

58
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -91,6 +91,24 @@ bool AP_MotorsMatrix::init(uint8_t expected_num_motors)
return true;
}
// Set throttle factor from scripting
bool AP_MotorsMatrix::set_throttle_factor(int8_t motor_num, float throttle_factor)
{
if ((_active_frame_class != MOTOR_FRAME_SCRIPTING_MATRIX) ) {
// not the correct class
return false;
}
if (initialised_ok() || !motor_enabled[motor_num]) {
// Already setup or given motor is not enabled
return false;
}
_throttle_factor[motor_num] = throttle_factor;
return true;
}
#endif // ENABLE_SCRIPTING
// set update rate to motors - a value in hertz
@ -370,14 +388,14 @@ void AP_MotorsMatrix::output_armed_stabilizing()
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + (rpy_scale * _thrust_rpyt_out[i]);
_thrust_rpyt_out[i] = (throttle_thrust_best_plus_adj * _throttle_factor[i]) + (rpy_scale * _thrust_rpyt_out[i]);
}
}
// determine throttle thrust for harmonic notch
const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
// compensation_gain can never be zero
_throttle_out = throttle_thrust_best_plus_adj / compensation_gain;
@ -484,7 +502,7 @@ bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm)
}
// add_motor
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order)
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor)
{
if (initialised_ok()) {
// do not allow motors to be set if the current frame type has init correctly
@ -494,15 +512,14 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc
// ensure valid motor number is provided
if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
// increment number of motors if this motor is being newly motor_enabled
if (!motor_enabled[motor_num]) {
motor_enabled[motor_num] = true;
}
// enable motor
motor_enabled[motor_num] = true;
// set roll, pitch, thottle factors and opposite motor (for stability patch)
// set roll, pitch, yaw and throttle factors
_roll_factor[motor_num] = roll_fac;
_pitch_factor[motor_num] = pitch_fac;
_yaw_factor[motor_num] = yaw_fac;
_throttle_factor[motor_num] = throttle_factor;
// set order that motor appears in test
_test_order[motor_num] = testing_order;
@ -536,9 +553,10 @@ void AP_MotorsMatrix::remove_motor(int8_t motor_num)
if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
// disable the motor, set all factors to zero
motor_enabled[motor_num] = false;
_roll_factor[motor_num] = 0;
_pitch_factor[motor_num] = 0;
_yaw_factor[motor_num] = 0;
_roll_factor[motor_num] = 0.0f;
_pitch_factor[motor_num] = 0.0f;
_yaw_factor[motor_num] = 0.0f;
_throttle_factor[motor_num] = 0.0f;
}
}
@ -1040,24 +1058,21 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
}
// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
// normalizes throttle factors so max value is 1 and no value is less than 0
void AP_MotorsMatrix::normalise_rpy_factors()
{
float roll_fac = 0.0f;
float pitch_fac = 0.0f;
float yaw_fac = 0.0f;
float throttle_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]);
}
roll_fac = MAX(roll_fac,fabsf(_roll_factor[i]));
pitch_fac = MAX(pitch_fac,fabsf(_pitch_factor[i]));
yaw_fac = MAX(yaw_fac,fabsf(_yaw_factor[i]));
throttle_fac = MAX(throttle_fac,MAX(0.0f,_throttle_factor[i]));
}
}
@ -1073,6 +1088,9 @@ void AP_MotorsMatrix::normalise_rpy_factors()
if (!is_zero(yaw_fac)) {
_yaw_factor[i] = 0.5f * _yaw_factor[i] / yaw_fac;
}
if (!is_zero(throttle_fac)) {
_throttle_factor[i] = MAX(0.0f,_throttle_factor[i] / throttle_fac);
}
}
}
}

7
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -35,6 +35,10 @@ public:
#ifdef ENABLE_SCRIPTING
// Init to be called from scripting
virtual bool init(uint8_t expected_num_motors);
// Set throttle factor from scripting
bool set_throttle_factor(int8_t motor_num, float throttle_factor);
#endif // ENABLE_SCRIPTING
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
@ -78,7 +82,7 @@ public:
void disable_yaw_torque(void) override;
// 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);
void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor = 1.0f);
protected:
// output - sends commands to the motors
@ -108,6 +112,7 @@ protected:
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)
float _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to throttle 0~1
float _thrust_rpyt_out[AP_MOTORS_MAX_NUM_MOTORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
uint8_t _test_order[AP_MOTORS_MAX_NUM_MOTORS]; // order of the motors in the test sequence

1
libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h
View File

@ -46,7 +46,6 @@ protected:
// nothing to do for setup, scripting will mark as initalized when done
void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override {};
float _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to up thrust
float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward thrust
float _right_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to right thrust