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

SITL: fixed rotations of motors by large angles

This commit is contained in:
Andrew Tridgell 2016-04-22 09:55:22 +10:00
parent 6165c42535
commit b4d24d8e03
1 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/SITL/SIM_Motor.cpp
View File

@ -35,18 +35,27 @@ void Motor::calculate_forces(const Aircraft::sitl_input &input,
rot_accel.z = yaw_factor * motor_speed * radians(400.0);
thrust(0, 0, motor_speed * thrust_scale); // newtons
if (roll_servo >= 0) {
float roll = constrain_float(roll_min + (input.servos[roll_servo]-1000)*0.001*(roll_max-roll_min), roll_min, roll_max);
float roll;
if (roll_min < roll_max) {
roll = constrain_float(roll_min + (input.servos[roll_servo]-1000)*0.001*(roll_max-roll_min), roll_min, roll_max);
} else {
roll = constrain_float(roll_max + (2000-input.servos[roll_servo])*0.001*(roll_min-roll_max), roll_max, roll_min);
}
Matrix3f rotation;
rotation.identity();
rotation.rotate(Vector3f(radians(roll), 0, 0));
rotation.from_euler(radians(roll), 0, 0);
rot_accel = rotation * rot_accel;
thrust = rotation * thrust;
}
if (pitch_servo >= 0) {
float pitch = constrain_float(pitch_min + (input.servos[pitch_servo]-1000)*0.001*(pitch_max-pitch_min), pitch_min, pitch_max);
float pitch;
if (pitch_min < pitch_max) {
pitch = constrain_float(pitch_min + (input.servos[pitch_servo]-1000)*0.001*(pitch_max-pitch_min), pitch_min, pitch_max);
} else {
pitch = constrain_float(pitch_max + (2000-input.servos[pitch_servo])*0.001*(pitch_min-pitch_max), pitch_max, pitch_min);
}
Matrix3f rotation;
rotation.identity();
rotation.rotate(Vector3f(0, radians(pitch), 0));
rotation.from_euler(0, radians(pitch), 0);
rot_accel = rotation * rot_accel;
thrust = rotation * thrust;
}