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

AP_Motors: Single and Coax fix flap gains

Browse Source
This commit is contained in:
Leonard Hall 2016-06-29 21:26:33 +09:30 committed by Randy Mackay
parent e15b1a8f2e
commit fe68fe65e2
2 changed files with 40 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -238,25 +238,11 @@ void AP_MotorsCoax::output_armed_stabilizing()
_thrust_yt_cw = thrust_out - 0.5f * yaw_thrust; _thrust_yt_cw = thrust_out - 0.5f * yaw_thrust;
// limit thrust out for calculation of actuator gains // limit thrust out for calculation of actuator gains
float thrust_out_actuator = MAX(_throttle_hover*0.5f,thrust_out); float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,thrust_out), 0.1f, 1.0f);
if (is_zero(thrust_out_actuator)) { if (is_zero(thrust_out)) {
limit.roll_pitch = true; limit.roll_pitch = true;
if (roll_thrust < 0.0f) {
_actuator_out[0] = -1.0f;
} else if (roll_thrust > 0.0f) {
_actuator_out[0] = 1.0f;
} else {
_actuator_out[0] = 0.0f;
} }
if (roll_thrust < 0.0f) {
_actuator_out[1] = -1.0f;
} else if (roll_thrust > 0.0f) {
_actuator_out[1] = 1.0f;
} else {
_actuator_out[1] = 0.0f;
}
} else {
// force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared // force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared
// static thrust is proportional to the airflow velocity squared // static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to // therefore the torque of the roll and pitch actuators should be approximately proportional to
@ -271,7 +257,6 @@ void AP_MotorsCoax::output_armed_stabilizing()
limit.roll_pitch = true; limit.roll_pitch = true;
_actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f); _actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f);
} }
}
_actuator_out[2] = -_actuator_out[0]; _actuator_out[2] = -_actuator_out[0];
_actuator_out[3] = -_actuator_out[1]; _actuator_out[3] = -_actuator_out[1];
} }

21
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -247,35 +247,27 @@ void AP_MotorsSingle::output_armed_stabilizing()
if (is_zero(_thrust_out)) { if (is_zero(_thrust_out)) {
limit.roll_pitch = true; limit.roll_pitch = true;
limit.yaw = true; limit.yaw = true;
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
if (actuator[i] < 0.0f) {
_actuator_out[i] = -1.0f;
} else if (actuator[i] > 0.0f) {
_actuator_out[i] = 1.0f;
} else {
_actuator_out[i] = 0.0f;
} }
}
} else { // limit thrust out for calculation of actuator gains
float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,_thrust_out), 0.1f, 1.0f);
// calculate the maximum allowed actuator output and maximum requested actuator output // calculate the maximum allowed actuator output and maximum requested actuator output
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
if (actuator_max > fabsf(actuator[i])) { if (actuator_max > fabsf(actuator[i])) {
actuator_max = fabsf(actuator[i]); actuator_max = fabsf(actuator[i]);
} }
} }
if (actuator_max > _thrust_out && !is_zero(actuator_max)) { if (actuator_max > thrust_out_actuator && !is_zero(actuator_max)) {
// roll, pitch and yaw request can not be achieved at full servo defection // roll, pitch and yaw request can not be achieved at full servo defection
// reduce roll, pitch and yaw to reduce the requested defection to maximum // reduce roll, pitch and yaw to reduce the requested defection to maximum
limit.roll_pitch = true; limit.roll_pitch = true;
limit.yaw = true; limit.yaw = true;
rp_scale = _thrust_out/actuator_max; rp_scale = thrust_out_actuator/actuator_max;
} else { } else {
rp_scale = 1.0f; rp_scale = 1.0f;
} }
// limit thrust out for calculation of actuator gains
float thrust_out_actuator = MAX(_throttle_hover*0.5,_thrust_out);
// force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared // force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared
// static thrust is proportional to the airflow velocity squared // static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to // therefore the torque of the roll and pitch actuators should be approximately proportional to
@ -283,7 +275,6 @@ void AP_MotorsSingle::output_armed_stabilizing()
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
_actuator_out[i] = constrain_float(rp_scale*actuator[i]/thrust_out_actuator, -1.0f, 1.0f); _actuator_out[i] = constrain_float(rp_scale*actuator[i]/thrust_out_actuator, -1.0f, 1.0f);
} }
}
} }
// output_test - spin a motor at the pwm value specified // output_test - spin a motor at the pwm value specified