add support for multiple inputs in C-code generation

Author: baggepinnen

src/SymbolicControlSystems.jl

@@ -131,6 +131,24 @@ end
 
 sym2num(P::TransferFunction, args...) = sym2num(Sym(P), args...)
 
+function numeric(x::Sym, pairs::Pair...)
+    for (sym, val) in pairs
+        x = subs(x, (sym, val))
+    end
+    float(x)
+end
+
+"""
+    sym2num(P::AbstractStateSpace, pairs::Pair...)
+
+For statespace systems, `sym2num` does not take the sample time `h`.
+"""
+function sym2num(P::AbstractStateSpace, pairs::Pair...)
+    A,B,C,D = ssdata(P)
+    A,B,C,D = (numeric.(x, pairs...) for x in (A,B,C,D))
+    ss(A,B,C,D,P.timeevol)
+end
+
 """
     sym2num(G, h::Real, pairs::Pair...)
 
@@ -309,10 +327,10 @@ double transfer_function(double ui$(var_str)) {
 end
 
 function ccode(sys::StateSpace{<:Discrete}; cse = true, function_name = "transfer_function")
-    sys.nu == 1 || throw(ArgumentError("Multiple input not yet supported"))
     nx = sys.nx
+    nu = sys.nu
     ny = sys.ny
-    u = Sym("u")
+    u = nu == 1 ? Sym("u") : [Sym("u[$(i-1)]") for i = 1:nu]
     x = [Sym("x[$(i-1)]") for i = 1:nx]
     # @show P
     if ControlSystems.numeric_type(sys) <: SymPy.Sym
@@ -334,10 +352,12 @@ function ccode(sys::StateSpace{<:Discrete}; cse = true, function_name = "transfe
     @show y = mul!(y, sys.C, x) + sys.D * u
     # @show y = sp.collect.(y, x)
 
+    u_str = nu == 1 ? "double u" : "double *u"
+
     code = """
 #include <stdio.h>\n
 #include <math.h>\n
-void $(function_name)(double *y, double u$(var_str)) {
+void $(function_name)(double *y, $(u_str)$(var_str)) {
     static double x[$(nx)] = {0};  // Current state
     double xp[$(nx)] = {0};        // Next state
     int i;

test/runtests.jl

@@ -182,6 +182,18 @@ end
             end
         end
 
+        function c_lsim_ss_multiinput(u, T, d, w)
+            y = zeros(1)
+            Y = Libc.Libdl.dlopen(outname) do lib
+                fn = Libc.Libdl.dlsym(lib, :transfer_function)
+                map(eachcol(u)) do u
+                    @ccall $(fn)(y::Ref{Cdouble}, u::Ref{Cdouble}, T::Float64, d::Float64, w::Float64)::Cvoid
+                    y[]
+                end
+            end
+            Y'
+        end
+
         function c_lsim_ss(u)
             Y = Libc.Libdl.dlopen(outname) do lib
                 fn = Libc.Libdl.dlsym(lib, :transfer_function)
@@ -213,6 +225,22 @@ end
         y_,_ = lsim(c2d(ss(G_), h, :tustin), u);
         @test norm(y-y_)/norm(y_) < 1e-10 # TODO: figure out why this is more sensitive
 
+        ## Multiple inputs
+        @vars w T d # Define symbolic variables
+        h = 0.01
+        G = tf([w^2], [1, 2*d*w, w^2]) * tf(1, [T, 1])
+        Gd = tustin(G, h) # Discretize 
+        Gd = [Gd 2Gd]
+        u = randn(2,1000); # Random input signal 
+        T_, d_, w_ = 0.03, 0.2, 2.0 # Define system parameters            
+
+        code = SymbolicControlSystems.ccode(ss(Gd), cse=true)
+        write(joinpath(path, filename), code)
+        run(`gcc $filename -lm -shared -o $outname`)
+        y = c_lsim_ss_multiinput( u,  T_,  d_,  w_); # Filter u through the 
+        Gd_ = sym2num(ss(Gd), Pair.((T, d, w), (T_, d_, w_))...) # Replace symbols with numeric constants
+        y_,_ = lsim(ss(Gd_), u);
+        @test norm(y-y_)/norm(y_) < 1e-10 # TODO: figure out why this is more sensitive
 
 
         # test without symbols