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

AP_Motors: Single and Coax fix flap gains

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

47
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -238,39 +238,24 @@ void AP_MotorsCoax::output_armed_stabilizing()
_thrust_yt_cw = thrust_out - 0.5f * yaw_thrust;
// 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;
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
// static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
_actuator_out[0] = roll_thrust/thrust_out_actuator;
_actuator_out[1] = pitch_thrust/thrust_out_actuator;
if (fabsf(_actuator_out[0]) > 1.0f) {
limit.roll_pitch = true;
_actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f);
}
if (fabsf(_actuator_out[1]) > 1.0f) {
limit.roll_pitch = true;
_actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f);
}
}
// 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
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
_actuator_out[0] = roll_thrust/thrust_out_actuator;
_actuator_out[1] = pitch_thrust/thrust_out_actuator;
if (fabsf(_actuator_out[0]) > 1.0f) {
limit.roll_pitch = true;
_actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f);
}
if (fabsf(_actuator_out[1]) > 1.0f) {
limit.roll_pitch = true;
_actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f);
}
_actuator_out[2] = -_actuator_out[0];
_actuator_out[3] = -_actuator_out[1];

57
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -247,42 +247,33 @@ void AP_MotorsSingle::output_armed_stabilizing()
if (is_zero(_thrust_out)) {
limit.roll_pitch = 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;
}
}
// 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
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
if (actuator_max > fabsf(actuator[i])) {
actuator_max = fabsf(actuator[i]);
}
}
if (actuator_max > thrust_out_actuator && !is_zero(actuator_max)) {
// 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
limit.roll_pitch = true;
limit.yaw = true;
rp_scale = thrust_out_actuator/actuator_max;
} else {
// calculate the maximum allowed actuator output and maximum requested actuator output
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
if (actuator_max > fabsf(actuator[i])) {
actuator_max = fabsf(actuator[i]);
}
}
if (actuator_max > _thrust_out && !is_zero(actuator_max)) {
// 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
limit.roll_pitch = true;
limit.yaw = true;
rp_scale = _thrust_out/actuator_max;
} 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
// static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
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);
}
// 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
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
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);
}
}