|
|
|
@ -258,7 +258,7 @@ Vector3f Plane::getForce(float inputAileron, float inputElevator, float inputRud
@@ -258,7 +258,7 @@ Vector3f Plane::getForce(float inputAileron, float inputElevator, float inputRud
|
|
|
|
|
return Vector3f(ax, ay, az); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel) |
|
|
|
|
void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel) |
|
|
|
|
{ |
|
|
|
|
float aileron = filtered_servo_angle(input, 0); |
|
|
|
|
float elevator = filtered_servo_angle(input, 1); |
|
|
|
@ -380,7 +380,7 @@ void Plane::update(const struct sitl_input &input)
@@ -380,7 +380,7 @@ void Plane::update(const struct sitl_input &input)
|
|
|
|
|
|
|
|
|
|
update_wind(input); |
|
|
|
|
|
|
|
|
|
calculate_forces(input, rot_accel, accel_body); |
|
|
|
|
calculate_forces(input, rot_accel); |
|
|
|
|
|
|
|
|
|
update_dynamics(rot_accel); |
|
|
|
|
update_external_payload(input); |
|
|
|
|