diff --git a/src/constants.zig b/src/constants.zig index 043b397..e574c41 100644 --- a/src/constants.zig +++ b/src/constants.zig @@ -1,6 +1 @@ pub const g: f64 = 9.80665; - -pub const PropSpinDirection = enum(i8) { - cw = 1, - ccw = -1, -}; diff --git a/src/drone.zig b/src/drone.zig index 039cb79..10e8002 100644 --- a/src/drone.zig +++ b/src/drone.zig @@ -1,6 +1,5 @@ const vec3 = @import("./vec3.zig"); const std = @import("std"); -const constants = @import("./constants.zig"); pub const Drone = struct { mass_kg: f64, @@ -20,7 +19,6 @@ pub const Propeller = struct { angular_velocity: f64, thrust_coefficient: f64, moment_coefficient: f64, - direction: constants.PropSpinDirection, }; pub const QuadCopterSim = struct { diff --git a/src/forces.zig b/src/forces.zig index 86171ea..f9bf191 100644 --- a/src/forces.zig +++ b/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, - @intFromEnum(p.direction) * p.moment_coefficient * p.angular_velocity * p.angular_velocity, + p.moment_coefficient * p.angular_velocity * p.angular_velocity, ); return .{ force, vec3.vec3Add(force_moment, drag_moment) }; @@ -55,10 +55,9 @@ 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); diff --git a/src/integration.zig b/src/integration.zig index 1e7578c..e655f27 100644 --- a/src/integration.zig +++ b/src/integration.zig @@ -1,25 +1,23 @@ 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.q, 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(d.drag_coeff, s.v)); + 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(s.v, 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( @@ -36,53 +34,6 @@ 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)).normalized(); + 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, - .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); -} diff --git a/src/main.zig b/src/main.zig index a1cdafd..e6e2016 100644 --- a/src/main.zig +++ b/src/main.zig @@ -4,5 +4,4 @@ test "main" { _ = @import("./drone.zig"); _ = @import("./vec3.zig"); _ = @import("./forces.zig"); - _ = @import("./integration.zig"); }