Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: output_motor_mask update

This commit is contained in:
Peter Hall 2020-01-03 00:12:59 +00:00 committed by Andrew Tridgell
parent 5da062b850
commit 4aa0dfa2aa
3 changed files with 5 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Motors/AP_MotorsMatrixTS.cpp
View File

@ -25,33 +25,6 @@ extern const AP_HAL::HAL& hal;
#define SERVO_OUTPUT_RANGE 4500
// output a thrust to all motors that match a given motor mask. This
// is used to control motors enabled for forward flight. Thrust is in
// the range 0 to 1
void AP_MotorsMatrixTS::output_motor_mask(float thrust, uint8_t mask, float rudder_dt)
{
const int16_t pwm_min = get_pwm_output_min();
const int16_t pwm_range = get_pwm_output_max() - pwm_min;
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
int16_t motor_out;
if (mask & (1U<<i)) {
/*
apply rudder mixing differential thrust
copter frame roll is plane frame yaw (this is only
used by tiltrotors and tailsitters)
*/
float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
motor_out = pwm_min + pwm_range * constrain_float(thrust + diff_thrust, 0.0f, 1.0f);
} else {
motor_out = pwm_min;
}
rc_write(i, motor_out);
}
}
}
void AP_MotorsMatrixTS::output_to_motors()
{
// calls calc_thrust_to_pwm(_thrust_rpyt_out[i]) for each enabled motor

2
libraries/AP_Motors/AP_MotorsMatrixTS.h
View File

@ -19,8 +19,6 @@ public:
virtual void output_to_motors() override;
virtual void output_motor_mask(float thrust, uint8_t mask, float rudder_dt) override;
protected:
bool enable_yaw_torque; // differential torque for yaw control

7
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -732,6 +732,9 @@ void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float th
// the range 0 to 1
void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float rudder_dt)
{
const int16_t pwm_min = get_pwm_output_min();
const int16_t pwm_range = get_pwm_output_max() - pwm_min;
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
if (mask & (1U << i)) {
@ -742,10 +745,10 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float r
*/
float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
set_actuator_with_slew(_actuator[i], thrust_to_actuator(thrust + diff_thrust));
int16_t pwm_output = get_pwm_output_min() + (get_pwm_output_max() - get_pwm_output_min()) * _actuator[i];
int16_t pwm_output = pwm_min + pwm_range * _actuator[i];
rc_write(i, pwm_output);
} else {
rc_write(i, get_pwm_output_min());
rc_write(i, pwm_min);
}
}
}