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

Fix bug with incorrect thrust direction and update test for vel and x_cg

Browse source
This commit is contained in:
Alex Selimov 2026-08-12 20:11:16 -04:00
parent 3cba5f4781
commit 6862712551
3 changed files with 44 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

44
src/integration.zig
View file

@ -1,23 +1,24 @@
const drone = @import("./drone.zig");
const vec3 = @import("./vec3.zig");
const forces = @import("./forces.zig");
const std = @import("std");
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);
const prop_force, const prop_moment = forces.propThrust(s.q, 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.drag(d.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));
s.x_cg = vec3.vec3Add(s.x_cg, vec3.vec3Scale(s.v, dt));
const I_omega = vec3.vec3MulMat3(d.moment_of_inertia, s.omega);
const omega_dot = vec3.vec3MulMat3(
@ -37,3 +38,40 @@ pub fn timestep(dt: f64, d: drone.Drone, props: []const drone.Propeller, s: *dro
s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt));
return;
}
test "Validate timesteps" {
const d = drone.Drone{
.drag_coeff = 0.0,
.mass_kg = 0.5,
.moment_of_inertia = .{
.row1 = .init(2, 0, 0),
.row2 = .init(0, 0.5, 0),
.row3 = .init(0, 0, 2),
},
};
var state: drone.State = .{
.x_cg = .init(0, 0, 0),
.omega = .init(0, 0, 0),
.q = vec3.yawPitchRollToQuat(0, 0, 0),
.v = .init(0, 0, 0),
};
const prop: drone.Propeller = .{
.angular_velocity = 100,
.cg_to_prop = .init(1, 0, 0),
.moment_coefficient = 0.1,
.thrust_coefficient = 0.5,
};
const props: [1]drone.Propeller = .{prop};
timestep(0.005, d, &props, &state);
try std.testing.expectApproxEqAbs(0, state.v.x(), 1e-12);
try std.testing.expectApproxEqAbs(0, state.v.y(), 1e-12);
try std.testing.expectApproxEqAbs(-49.95096675, state.v.z(), 1e-8);
try std.testing.expectApproxEqAbs(0, state.x_cg.x(), 1e-12);
try std.testing.expectApproxEqAbs(0, state.x_cg.y(), 1e-12);
try std.testing.expectApproxEqAbs(-0.24975483375, state.x_cg.z(), 1e-11);
}