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

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);
@@ -57,7 +57,7 @@ test "thrust is correct" {
         .moment_coefficient = 2,
     };
 
-    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,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);
+}

src/main.zig

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