Merge branch 'quaternion-fix' into 'main'

Add PD, RW and Quaternion noop conversion

See merge request acubesat/adcs/adcs-simulation-julia!7

Manifest.toml

@@ -2,7 +2,7 @@
 
 julia_version = "1.10.0-beta2"
 manifest_format = "2.0"
-project_hash = "889658496d6fa3bcb31badd691b361c0a40fa9fb"
+project_hash = "4903dd5d84faaf8c3eae7adbcef17ba971308241"
 
 [[deps.Accessors]]
 deps = ["CompositionsBase", "ConstructionBase", "Dates", "InverseFunctions", "LinearAlgebra", "MacroTools", "Requires", "Test"]

Project.toml

@@ -4,10 +4,12 @@ authors = ["Orestis Ousoultzoglou <[email protected]>", "Romanos Voulgar
 version = "0.0.1-DEV"
 
 [deps]
+Accessors = "7d9f7c33-5ae7-4f3b-8dc6-eff91059b697"
 ConcreteStructs = "2569d6c7-a4a2-43d3-a901-331e8e4be471"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Optim = "429524aa-4258-5aef-a3af-852621145aeb"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
+Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 SatelliteDynamics = "0e7c1a32-1b9f-5532-88a4-e668712d6a4c"
 SatelliteToolbox = "6ac157d9-b43d-51bb-8fab-48bf53814f4a"
 SatelliteToolboxGeomagneticField = "9fc549ba-b5d7-49a2-b268-8171e5fb6e89"
@@ -20,7 +22,6 @@ Plots = "1"
 SatelliteDynamics = "0.4"
 SatelliteToolbox = "0.12"
 SatelliteToolboxGeomagneticField = "0.1"
-
 julia = "1.9"
 
 [extras]

src/control.jl

@@ -0,0 +1,11 @@
+@concrete struct PDController
+    Kp
+    Kd
+end
+
+# TODO: quaternion frame?
+function calculate_torque(PD::PDController, qtarget, qestimated, w, wtarget)
+    qrel = qtarget * conj(qestimated) # TODO: should it be conj(qtarget)?
+    werr = w - wtarget
+    return -sign(scalar(qrel)) * PD.Kp * vector(qrel) - PD.Kd * werr
+end

src/quaternion.jl

@@ -26,6 +26,7 @@ function Base.convert(::Type{Quaternion{T}}, x::T) where {T}
     return Quaternion(x, x, x, x)
 end
 
+Base.convert(::Type{Quaternion}, Q::Quaternion) = Q
 function Base.convert(::Type{T}, Q::Quaternion) where {T}
     return Quaternion{T}(Q.q1, Q.q2, Q.q3, Q.q4)
 end

src/reaction_wheel.jl

@@ -0,0 +1,24 @@
+@kwdef @concrete struct ReactionWheel
+    J
+    w
+    wdeadzone = 52.359877
+    wmid = wdeadzone + 10
+    α
+    maxtorque = 0.001
+    Fc = 0.001
+    b = 0.0005
+    Fs = 0.0005
+    w0 = 1
+end
+
+function stribeck(RW::ReactionWheel)
+    τcoulomb = RW.Fc * sign(RW.w)
+    τviscous = RW.b * RW.w
+    τstribeck = RW.Fs * exp(-(RW.w / RW.w0)^2) * sign(RW.w)
+    return τcoulomb + τviscous + τstribeck
+end
+
+function deadzone_compensation(RW::ReactionWheel)
+    abs(RW.w) <= RW.wdeadzone || return zero(RW.w)
+    return RW.maxtorque / (1 + exp(-RW.α * (RW.w - RW.wmid)))
+end