Add RK4 for RW, example control loop

src/dynamics.jl

@@ -25,3 +25,23 @@ function rk4_filter(w, q, dt)
     new_q = normalize(q + (dt / 6.0) * (k1_q + 2 * k2_q + 2 * k3_q + k4_q))
     return new_q
 end
+
+function rk4_rw(J, w, τ, dt)
+    k1_w = wderiv(J, w, τ)
+    k2_w = wderiv(J, w + 0.5 * dt * k1_w, τ)
+    k3_w = wderiv(J, w + 0.5 * dt * k2_w, τ)
+    k4_w = wderiv(J, w + dt * k3_w, τ)
+    return w + (dt / 6.0) * (k1_w + 2 * k2_w + 2 * k3_w + k4_w)
+end
+
+# TODO: what if saturation compensation is smaller than the cubesat w from control
+# function control_loop(sensors, target_vectors, w, RW::ReactionWheel, I, dt = 0.001)
+#     q = estimateq(sensors, target_vectors, w)
+#     τ = calculate_torque(PDController(), qtarget, q, w, wtarget)
+#     τw, τsm, mtrue = decompose_torque(τ, b, msaturation) # FIXME: implement
+#     compensation = deadzone_compensation(RW) + saturation_compensation(RW)
+#     τw = clamp(τw + compensation, -RW.maxtorque, RW.maxtorque)
+#     rwfriction = stribeck(RW)
+#     @reset RW.w = rk4_rw(RW.J, RW.w, -τw + rwfriction, dt)
+#     return rk4(I, w, τw - rwfriction, q, dt) # TODO: update cubesat state with τw + τsm + disturbances
+# end

src/plots.jl

@@ -27,7 +27,7 @@ function plot_difference(ŷ, y)
     yqs = [s.q for s in y]
     for i in 1:len
         q_rel = ŷqs[i] * conj(yqs[i])
-        euler_angles_deg = quat_to_euler_deg(q_rel)
+        euler_angles_deg = toeuler(q_rel)
         difference_array[1:3, i] = euler_angles_deg
     end
 

src/quaternion.jl

@@ -83,7 +83,7 @@ function rotvec(v::Vector, Q::Quaternion)
     return vector(conj(Q) * Quaternion([0; v]) * Q)
 end
 
-function quat_to_euler_deg(Q::Quaternion)
+function toeuler(Q::Quaternion)
     roll = atan(2 * (Q.q1 * Q.q2 + Q.q3 * Q.q4), 1 - 2 * (Q.q2^2 + Q.q3^2))
     pitch = asin(2 * (Q.q1 * Q.q3 - Q.q4 * Q.q2))
     yaw = atan(2 * (Q.q1 * Q.q4 + Q.q2 * Q.q3), 1 - 2 * (Q.q3^2 + Q.q4^2))