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

AP_Motors: Single and Coax Fixes

Browse Source
This commit is contained in:
Leonard Hall 2016-05-28 22:09:42 +09:30 committed by Andrew Tridgell
parent e5bdf0a0a2
commit d67f83559c
2 changed files with 16 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -233,7 +233,10 @@ void AP_MotorsCoax::output_armed_stabilizing()
_thrust_yt_ccw = thrust_out + 0.5f * yaw_thrust; _thrust_yt_ccw = thrust_out + 0.5f * yaw_thrust;
_thrust_yt_cw = thrust_out - 0.5f * yaw_thrust; _thrust_yt_cw = thrust_out - 0.5f * yaw_thrust;
if (is_zero(thrust_out)) { // limit thrust out for calculation of actuator gains
float thrust_out_actuator = MAX(throttle_thrust_hover*0.5,thrust_out);
if (is_zero(thrust_out_actuator)) {
limit.roll_pitch = true; limit.roll_pitch = true;
if (roll_thrust < 0.0f) { if (roll_thrust < 0.0f) {
_actuator_out[0] = -1.0f; _actuator_out[0] = -1.0f;
@ -254,8 +257,8 @@ void AP_MotorsCoax::output_armed_stabilizing()
// 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
// the angle of attack multiplied by the static thrust. // the angle of attack multiplied by the static thrust.
_actuator_out[0] = roll_thrust/thrust_out; _actuator_out[0] = roll_thrust/thrust_out_actuator;
_actuator_out[1] = pitch_thrust/thrust_out; _actuator_out[1] = pitch_thrust/thrust_out_actuator;
if (fabsf(_actuator_out[0]) > 1.0f) { if (fabsf(_actuator_out[0]) > 1.0f) {
limit.roll_pitch = true; limit.roll_pitch = true;
_actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f); _actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f);

15
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -223,13 +223,14 @@ void AP_MotorsSingle::output_armed_stabilizing()
// combine roll, pitch and yaw on each actuator // combine roll, pitch and yaw on each actuator
// front servo // front servo
actuator[0] = rpy_scale * roll_thrust + yaw_thrust;
actuator[0] = rpy_scale * roll_thrust - yaw_thrust;
// right servo // right servo
actuator[1] = rpy_scale * pitch_thrust + yaw_thrust; actuator[1] = rpy_scale * pitch_thrust - yaw_thrust;
// rear servo // rear servo
actuator[2] = -rpy_scale * roll_thrust + yaw_thrust; actuator[2] = -rpy_scale * roll_thrust - yaw_thrust;
// left servo // left servo
actuator[3] = -rpy_scale * pitch_thrust + yaw_thrust; actuator[3] = -rpy_scale * pitch_thrust - yaw_thrust;
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces // calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rp = MAX(MAX(fabsf(actuator[0]), fabsf(actuator[1])), MAX(fabsf(actuator[2]), fabsf(actuator[3]))); thrust_min_rp = MAX(MAX(fabsf(actuator[0]), fabsf(actuator[1])), MAX(fabsf(actuator[2]), fabsf(actuator[3])));
@ -272,12 +273,16 @@ void AP_MotorsSingle::output_armed_stabilizing()
} else { } else {
rpy_scale = 1.0f; rpy_scale = 1.0f;
} }
// limit thrust out for calculation of actuator gains
float thrust_out_actuator = MAX(throttle_thrust_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
// the angle of attack multiplied by the static thrust. // the angle of attack multiplied by the static thrust.
for (uint8_t i=0; i<NUM_ACTUATORS; i++) { for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
_actuator_out[i] = constrain_float(rpy_scale*actuator[i]/_thrust_out, -1.0f, 1.0f); _actuator_out[i] = constrain_float(rpy_scale*actuator[i]/thrust_out_actuator, -1.0f, 1.0f);
} }
} }
} }