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 std = @import("std");
const Drone = struct {
pub const Drone = struct {
mass_kg: f64,
center_of_gravity_inertial: vec3.Vec3F64,
moment_of_inertia: vec3.Mat3F64,
attitude: vec3.QuatF64,
};
const Propellers = struct {
pub const Propeller = struct {
cg_to_prop: vec3.Vec3F64,
angular_velocity: f64,
thrust_coefficient: f64,
torque_drag_coefficient: f64,
moment_coefficient: f64,
};
const QuadCopterSim = struct {
pub const QuadCopterSim = struct {
drone: Drone,
propellers: [4]Propellers,
propellers: [4]Propeller,
};
pub fn body_position_to_inertial(

39
src/forces.zig
View file

@ -1,23 +1,32 @@
const constants = @import("./constants.zig");
const vec3 = @import("./vec3.zig");
const drone = @import("./drone.zig");
const std = @import("std");
pub fn gravity(mass: f64) vec3.Vec3F64 {
return .init(0, 0, mass * constants.g);
}
pub fn propThrust(
attitude: vec3.QuatF64,
thrust_coefficient: f64,
angular_velocity: f64,
) vec3.Vec3F64 {
pub fn propThrust(attitude: vec3.QuatF64, p: drone.Propeller) struct { vec3.Vec3F64, vec3.Vec3F64 } {
const body_thrust = vec3.Vec3F64.init(
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(
@ -41,14 +50,22 @@ test "gravity is correct" {
test "thrust is correct" {
const attitude = vec3.yawPitchRollToQuat(0, 0, -std.math.pi / 2.0);
const thrust_coefficient = 1;
const angular_velocity = 100;
const prop: drone.Propeller = .{
.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(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(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" {