40 lines
1.4 KiB
Zig
40 lines
1.4 KiB
Zig
|
const drone = @import("./drone.zig");
|
||
|
|
const vec3 = @import("./vec3.zig");
|
||
|
|
const forces = @import("./forces.zig");
|
||
|
|
|
||
|
|
pub fn timestep(dt: f64, d: drone.Drone, props: []const drone.Propeller, s: *drone.State) void {
|
||
|
|
var force = vec3.Vec3F64.init(0, 0, 0);
|
||
|
|
var moment = vec3.Vec3F64.init(0, 0, 0);
|
||
|
|
|
||
|
|
for (props) |prop| {
|
||
|
|
const prop_force, const prop_moment = forces.propThrust(s.attitude, prop);
|
||
|
|
force = vec3.vec3Add(force, prop_force);
|
||
|
|
moment = vec3.vec3Add(moment, prop_moment);
|
||
|
|
}
|
||
|
|
|
||
|
|
force = vec3.vec3Add(force, forces.drag(drone.drag_coeff, s.v));
|
||
|
|
force = vec3.vec3Add(force, forces.gravity(d.mass_kg));
|
||
|
|
|
||
|
|
const accel = vec3.vec3Scale(force, 1.0 / d.mass_kg);
|
||
|
|
s.v = vec3.vec3Add(s.v, vec3.vec3Scale(accel, dt));
|
||
|
|
s.x_cg = vec3.vec3Add(s.x_cg, vec3.vec3Scale(accel, dt));
|
||
|
|
|
||
|
|
const I_omega = vec3.vec3MulMat3(d.moment_of_inertia, s.omega);
|
||
|
|
const omega_dot = vec3.vec3MulMat3(
|
||
|
|
vec3.mat3Inv(d.moment_of_inertia),
|
||
|
|
vec3.vec3Sub(moment, vec3.vec3Cross(s.omega, I_omega)),
|
||
|
|
);
|
||
|
|
|
||
|
|
s.omega = vec3.vec3Add(s.omega, vec3.vec3Scale(omega_dot, dt));
|
||
|
|
const half_omege_quat: vec3.QuatF64 = .init(
|
||
|
|
s.omega.x() * 0.5,
|
||
|
|
s.omega.y() * 0.5,
|
||
|
|
s.omega.z() * 0.5,
|
||
|
|
0,
|
||
|
|
);
|
||
|
|
|
||
|
|
const qdot = vec3.quatMul(half_omege_quat, s.q);
|
||
|
|
s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt));
|
||
|
|
return;
|
||
|
|
}
|