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

Add moment calculation for propeller

Browse source
This commit is contained in:
Alex Selimov 2026-08-08 21:41:17 -04:00
parent f3d930405c
commit 923574ec48
2 changed files with 33 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/drone.zig
View file

@ -1,23 +1,23 @@
const vec3 = @import("./vec3.zig"); const vec3 = @import("./vec3.zig");
const std = @import("std"); const std = @import("std");
const Drone = struct { pub const Drone = struct {
mass_kg: f64, mass_kg: f64,
center_of_gravity_inertial: vec3.Vec3F64, center_of_gravity_inertial: vec3.Vec3F64,
moment_of_inertia: vec3.Mat3F64, moment_of_inertia: vec3.Mat3F64,
attitude: vec3.QuatF64, attitude: vec3.QuatF64,
}; };
const Propellers = struct { pub const Propeller = struct {
cg_to_prop: vec3.Vec3F64, cg_to_prop: vec3.Vec3F64,
angular_velocity: f64, angular_velocity: f64,
thrust_coefficient: f64, thrust_coefficient: f64,
torque_drag_coefficient: f64, moment_coefficient: f64,
}; };
const QuadCopterSim = struct { pub const QuadCopterSim = struct {
drone: Drone, drone: Drone,
propellers: [4]Propellers, propellers: [4]Propeller,
}; };
pub fn body_position_to_inertial( pub fn body_position_to_inertial(

39
src/forces.zig
View file

@ -1,23 +1,32 @@
const constants = @import("./constants.zig"); const constants = @import("./constants.zig");
const vec3 = @import("./vec3.zig"); const vec3 = @import("./vec3.zig");
const drone = @import("./drone.zig");
const std = @import("std"); const std = @import("std");
pub fn gravity(mass: f64) vec3.Vec3F64 { pub fn gravity(mass: f64) vec3.Vec3F64 {
return .init(0, 0, mass * constants.g); return .init(0, 0, mass * constants.g);
} }
pub fn propThrust( pub fn propThrust(attitude: vec3.QuatF64, p: drone.Propeller) struct { vec3.Vec3F64, vec3.Vec3F64 } {
attitude: vec3.QuatF64,
thrust_coefficient: f64,
angular_velocity: f64,
) vec3.Vec3F64 {
const body_thrust = vec3.Vec3F64.init( const body_thrust = vec3.Vec3F64.init(
0, 0,
0, 0,
thrust_coefficient * angular_velocity * angular_velocity, p.thrust_coefficient * p.angular_velocity * p.angular_velocity,
); );
return vec3.quatApply(attitude, body_thrust); 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);
// Yaw moment induced by roation of propeller
const drag_moment = vec3.Vec3F64.init(
0,
0,
p.moment_coefficient * p.angular_velocity * p.angular_velocity,
);
return .{ force, vec3.vec3Add(force_moment, drag_moment) };
} }
pub fn drag( pub fn drag(
@ -41,14 +50,22 @@ test "gravity is correct" {
test "thrust is correct" { test "thrust is correct" {
const attitude = vec3.yawPitchRollToQuat(0, 0, -std.math.pi / 2.0); const attitude = vec3.yawPitchRollToQuat(0, 0, -std.math.pi / 2.0);
const thrust_coefficient = 1; const prop: drone.Propeller = .{
const angular_velocity = 100; .cg_to_prop = vec3.Vec3F64.init(1, 1, 0),
.angular_velocity = 100,
.thrust_coefficient = 1,
.moment_coefficient = 2,
};
const thrust = propThrust(attitude, thrust_coefficient, angular_velocity); const expected_thrust_mag = 100 * 100;
const thrust, const momentum = propThrust(attitude, prop);
try std.testing.expectApproxEqAbs(0, thrust.x(), 1e-11); try std.testing.expectApproxEqAbs(0, thrust.x(), 1e-11);
try std.testing.expectApproxEqAbs(100 * 100, thrust.y(), 1e-11); try std.testing.expectApproxEqAbs(expected_thrust_mag, thrust.y(), 1e-11);
try std.testing.expectApproxEqAbs(0, thrust.z(), 1e-11); try std.testing.expectApproxEqAbs(0, thrust.z(), 1e-11);
try std.testing.expectApproxEqAbs(expected_thrust_mag, momentum.x(), 1e-11);
try std.testing.expectApproxEqAbs(-expected_thrust_mag, momentum.y(), 1e-11);
try std.testing.expectApproxEqAbs(100 * 100 * 2, momentum.z(), 1e-11);
} }
test "drag is correct" { test "drag is correct" {