aselimov/zigsdof
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Fix issue with drag calculation and update expected final value

Browse source
This commit is contained in:
Alex Selimov 2026-08-20 00:06:13 -04:00
parent 6332e3d770
commit 3a47ce9383
2 changed files with 27 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

23
src/integration.zig
View file

@ -28,14 +28,14 @@ pub fn timestep(dt: f64, d: drone.Drone, props: []const drone.Propeller, s: *dro
);
s.omega = vec3.vec3Add(s.omega, vec3.vec3Scale(omega_dot, dt));
const half_omege_quat: vec3.QuatF64 = .init(
const half_omega_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);
const qdot = vec3.quatMul(half_omega_quat, s.q);
s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt)).normalized();
return;
}
@ -85,4 +85,23 @@ test "Validate timesteps" {
try std.testing.expectApproxEqAbs(0.12403234937465502, state.q.y(), 1e-12);
try std.testing.expectApproxEqAbs(0.006201617468732751, state.q.z(), 1e-11);
try std.testing.expectApproxEqAbs(0.9922587949972401, state.q.w(), 1e-11);
timestep(0.005, d, &props, &state);
try std.testing.expectApproxEqAbs(-1.23072190e+01, state.v.x(), 1e-7);
try std.testing.expectApproxEqAbs(-7.69201185e-02, state.v.y(), 1e-8);
try std.testing.expectApproxEqAbs(-9.83635311e+01, state.v.z(), 1e-7);
try std.testing.expectApproxEqAbs(-6.15360950e-02, state.x_cg.x(), 1e-9);
try std.testing.expectApproxEqAbs(-3.84600592e-04, state.x_cg.y(), 1e-9);
try std.testing.expectApproxEqAbs(-7.41572489e-01, state.x_cg.z(), 1e-9);
try std.testing.expectApproxEqAbs(-0.46875, state.omega.x(), 1e-12);
try std.testing.expectApproxEqAbs(100, state.omega.y(), 1e-12);
try std.testing.expectApproxEqAbs(5, state.omega.z(), 1e-11);
try std.testing.expectApproxEqAbs(-0.0011280012096230429, state.q.x(), 1e-12);
try std.testing.expectApproxEqAbs(0.36096743708693396, state.q.y(), 1e-12);
try std.testing.expectApproxEqAbs(0.01790701920276581, state.q.z(), 1e-12);
try std.testing.expectApproxEqAbs(0.9324057998744074, state.q.w(), 1e-12);
}