Add single timestep test to verify math

src/constants.zig

@@ -1 +1,6 @@
 pub const g: f64 = 9.80665;
+
+pub const PropSpinDirection = enum(i8) {
+    cw = 1,
+    ccw = -1,
+};

src/drone.zig

@@ -1,5 +1,6 @@
 const vec3 = @import("./vec3.zig");
 const std = @import("std");
+const constants = @import("./constants.zig");
 
 pub const Drone = struct {
     mass_kg: f64,
@@ -19,6 +20,7 @@ pub const Propeller = struct {
     angular_velocity: f64,
     thrust_coefficient: f64,
     moment_coefficient: f64,
+    direction: constants.PropSpinDirection,
 };
 
 pub const QuadCopterSim = struct {

src/forces.zig

@@ -11,7 +11,7 @@ pub fn propThrust(attitude: vec3.QuatF64, p: drone.Propeller) struct { vec3.Vec3
     const body_thrust = vec3.Vec3F64.init(
         0,
         0,
-        p.thrust_coefficient * p.angular_velocity * p.angular_velocity,
+        -p.thrust_coefficient * p.angular_velocity * p.angular_velocity,
     );
 
     const force = vec3.quatApply(attitude, body_thrust);
@@ -23,7 +23,7 @@ pub fn propThrust(attitude: vec3.QuatF64, p: drone.Propeller) struct { vec3.Vec3
     const drag_moment = vec3.Vec3F64.init(
         0,
         0,
-        p.moment_coefficient * p.angular_velocity * p.angular_velocity,
+        @intFromEnum(p.direction) * p.moment_coefficient * p.angular_velocity * p.angular_velocity,
     );
 
     return .{ force, vec3.vec3Add(force_moment, drag_moment) };
@@ -55,9 +55,10 @@ test "thrust is correct" {
         .angular_velocity = 100,
         .thrust_coefficient = 1,
         .moment_coefficient = 2,
+        .direction = constants.PropSpinDirection.cw,
     };
 
-    const expected_thrust_mag = 100 * 100;
+    const expected_thrust_mag = -100 * 100;
     const thrust, const momentum = propThrust(attitude, prop);
 
     try std.testing.expectApproxEqAbs(0, thrust.x(), 1e-11);

src/integration.zig

@@ -1,23 +1,25 @@
 const drone = @import("./drone.zig");
 const vec3 = @import("./vec3.zig");
 const forces = @import("./forces.zig");
+const constants = @import("constants.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(
@@ -34,6 +36,53 @@ pub fn timestep(dt: f64, d: drone.Drone, props: []const drone.Propeller, s: *dro
     );
 
     const qdot = vec3.quatMul(half_omege_quat, s.q);
-    s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt));
+    s.q = vec3.quatAdd(s.q, vec3.quatScale(qdot, dt)).normalized();
     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,
+        .direction = constants.PropSpinDirection.cw,
+    };
+    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);
+
+    try std.testing.expectApproxEqAbs(0, state.omega.x(), 1e-12);
+    try std.testing.expectApproxEqAbs(50, state.omega.y(), 1e-12);
+    try std.testing.expectApproxEqAbs(2.5, state.omega.z(), 1e-11);
+
+    try std.testing.expectApproxEqAbs(0, state.q.x(), 1e-12);
+    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);
+}

src/main.zig

@@ -4,4 +4,5 @@ test "main" {
     _ = @import("./drone.zig");
     _ = @import("./vec3.zig");
     _ = @import("./forces.zig");
+    _ = @import("./integration.zig");
 }