diff --git a/src/drone.zig b/src/drone.zig index 039cb79..75c97e9 100644 --- a/src/drone.zig +++ b/src/drone.zig @@ -16,11 +16,21 @@ pub const State = struct { }; pub const Propeller = struct { - cg_to_prop: vec3.Vec3F64, + x_cg: vec3.Vec3F64, angular_velocity: f64, thrust_coefficient: f64, moment_coefficient: f64, direction: constants.PropSpinDirection, + + pub fn empty() Propeller { + return .{ + .x_cg = .init(0, 0, 0), + .angular_velocity = 0, + .thrust_coefficient = 0, + .moment_coefficient = 0, + .direction = .cw, + }; + } }; pub const QuadCopterSim = struct { diff --git a/src/forces.zig b/src/forces.zig index ca3ee49..df43b5c 100644 --- a/src/forces.zig +++ b/src/forces.zig @@ -17,7 +17,7 @@ pub fn propThrust(attitude: vec3.QuatF64, p: drone.Propeller) struct { vec3.Vec3 const force = vec3.quatApply(attitude, body_thrust); // pitching/rolling moment induced by thrust force - const force_moment = vec3.vec3Cross(p.cg_to_prop, body_thrust); + const force_moment = vec3.vec3Cross(p.x_cg, body_thrust); // Yaw moment induced by roation of propeller const drag_moment = vec3.Vec3F64.init( @@ -51,7 +51,7 @@ test "gravity is correct" { test "thrust is correct" { const attitude = vec3.yawPitchRollToQuat(0, 0, -std.math.pi / 2.0); const prop: drone.Propeller = .{ - .cg_to_prop = vec3.Vec3F64.init(1, 1, 0), + .x_cg = vec3.Vec3F64.init(1, 1, 0), .angular_velocity = 100, .thrust_coefficient = 1, .moment_coefficient = 2, diff --git a/src/integration.zig b/src/integration.zig index b6d53cd..0b90a18 100644 --- a/src/integration.zig +++ b/src/integration.zig @@ -60,7 +60,7 @@ test "Validate timesteps" { const prop: drone.Propeller = .{ .angular_velocity = 100, - .cg_to_prop = .init(1, 0, 0), + .x_cg = .init(1, 0, 0), .moment_coefficient = 0.1, .thrust_coefficient = 0.5, .direction = constants.PropSpinDirection.cw, diff --git a/src/io.zig b/src/io.zig new file mode 100644 index 0000000..3e74486 --- /dev/null +++ b/src/io.zig @@ -0,0 +1,149 @@ +const std = @import("std"); + +const constants = @import("./constants.zig"); +const drone = @import("./drone.zig"); +const vec3 = @import("./vec3.zig"); + +const IoError = error{InputParsingError}; + +const JsonProp = struct { + x_cg: [3]f64, + thrust_coefficient: f64, + moment_coefficient: f64, + direction: constants.PropSpinDirection, +}; +const JsonSim = struct { + mass_kg: f64, + moment_of_inertia: [9]f64, + drag_coeff: f64, + propellers: [4]JsonProp, +}; + +pub fn jsonFileToSimConfig( + file_path: []u8, + allocator: std.mem.Allocator, + io: std.Io, +) !drone.QuadCopterSim { + const cwd = std.Io.Dir.cwd(); + const file = try cwd.openFile(io, file_path, .{ .mode = .read }); + defer file.close(io); + + var file_reader = file.reader(io, &.{}); + const json = try file_reader.interface.allocRemaining(allocator, .unlimited); + + return jsonStringToSimConfig(json, allocator, io); +} + +pub fn jsonStringToSimConfig( + json_string: []const u8, + allocator: std.mem.Allocator, +) !drone.QuadCopterSim { + const parsed = try std.json.parseFromSlice(JsonSim, allocator, json_string, .{}); + defer parsed.deinit(); + + const d: drone.Drone = .{ + .drag_coeff = parsed.value.drag_coeff, + .moment_of_inertia = .{ + .row1 = .init( + parsed.value.moment_of_inertia[0], + parsed.value.moment_of_inertia[1], + parsed.value.moment_of_inertia[2], + ), + .row2 = .init( + parsed.value.moment_of_inertia[3], + parsed.value.moment_of_inertia[4], + parsed.value.moment_of_inertia[5], + ), + .row3 = .init( + parsed.value.moment_of_inertia[6], + parsed.value.moment_of_inertia[7], + parsed.value.moment_of_inertia[8], + ), + }, + .mass_kg = parsed.value.mass_kg, + }; + var propellers: [4]drone.Propeller = .{ .empty(), .empty(), .empty(), .empty() }; + for (parsed.value.propellers, 0..) |parsed_prop, i| { + propellers[i].x_cg = .init( + parsed_prop.x_cg[0], + parsed_prop.x_cg[1], + parsed_prop.x_cg[2], + ); + propellers[i].thrust_coefficient = parsed_prop.thrust_coefficient; + propellers[i].moment_coefficient = parsed_prop.moment_coefficient; + propellers[i].direction = parsed_prop.direction; + } + return .{ + .propellers = propellers, + .drone = d, + }; +} + +test "Verify parse from Json String works" { + const TEST_JSON_STRING = + \\ { + \\ "mass_kg": 0.5, + \\ "moment_of_inertia": [1,0,0,0,1,0,0,0,1], + \\ "drag_coeff": 0.25, + \\ "propellers": [ + \\ { "x_cg": [1,0,0], + \\ "thrust_coefficient": 1.0, + \\ "moment_coefficient": 2.0, + \\ "direction": "cw" + \\ }, + \\ { "x_cg": [-1,0,0], + \\ "thrust_coefficient": 2.0, + \\ "moment_coefficient": 1.0, + \\ "direction": "ccw" + \\ }, + \\ { "x_cg": [0,1,0], + \\ "thrust_coefficient": 1.0, + \\ "moment_coefficient": 2.0, + \\ "direction": "cw" + \\ }, + \\ { "x_cg": [0,-1,0], + \\ "thrust_coefficient": 2.0, + \\ "moment_coefficient": 1.0, + \\ "direction": "ccw" + \\ } + \\] + \\} + ; + const gpa = std.testing.allocator; + const sim_config = try jsonStringToSimConfig(TEST_JSON_STRING, gpa); + + try std.testing.expectApproxEqAbs(0.5, sim_config.drone.mass_kg, 1e-12); + try std.testing.expectApproxEqAbs(0.25, sim_config.drone.drag_coeff, 1e-12); + try std.testing.expect(vec3.mat3ApproxEq(sim_config.drone.moment_of_inertia, vec3.Mat3F64.identity(), 1e-12)); + + const expected_x: [4]vec3.Vec3F64 = .{ + .init(1, 0, 0), + .init(-1, 0, 0), + .init(0, 1, 0), + .init(0, -1, 0), + }; + + const expected_thrust: [4]f64 = .{ 1, 2, 1, 2 }; + const expected_moment: [4]f64 = .{ 2, 1, 2, 1 }; + const expected_direction: [4]constants.PropSpinDirection = .{ .cw, .ccw, .cw, .ccw }; + + var i: u8 = 0; + while (i < 4) : (i += 1) { + try std.testing.expectApproxEqAbs( + expected_thrust[i], + sim_config.propellers[i].thrust_coefficient, + 1e-12, + ); + try std.testing.expectApproxEqAbs( + expected_moment[i], + sim_config.propellers[i].moment_coefficient, + 1e-12, + ); + + try std.testing.expect( + vec3.vec3ApproxEq(sim_config.propellers[i].x_cg, expected_x[i], 1e-12), + ); + + try std.testing.expect(sim_config.propellers[i].direction == expected_direction[i]); + } +} diff --git a/src/main.zig b/src/main.zig index a1cdafd..ac66f52 100644 --- a/src/main.zig +++ b/src/main.zig @@ -5,4 +5,5 @@ test "main" { _ = @import("./vec3.zig"); _ = @import("./forces.zig"); _ = @import("./integration.zig"); + _ = @import("./io.zig"); } diff --git a/src/vec3.zig b/src/vec3.zig index b81124e..07f3af9 100644 --- a/src/vec3.zig +++ b/src/vec3.zig @@ -392,7 +392,7 @@ test "mat3approxEq works" { try std.testing.expect(mat3ApproxEq(a, b, 1e-12)); b.row2.data[1] += 1e-11; try std.testing.expect(!mat3ApproxEq(a, b, 1e-12)); - try std.testing.expect(mat3ApproxEq(a, b, 1e-12)); + try std.testing.expect(mat3ApproxEq(a, b, 1e-10)); } test "vec3 mul mat3 works" {