-
Notifications
You must be signed in to change notification settings - Fork 54
/
simulateDSM.c
executable file
·373 lines (350 loc) · 10.3 KB
/
simulateDSM.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
/* simulateDSM.c - A MEX-file for simulating a delta-sigma modulator
%[v,xn,xmax,y] = simulateDSM( u, ABCD, nlev[2], x0[0] )
% or
%[v,xn,xmax,y] = simulateDSM( u, ntf, nlev[2], x0[0] )
%
%Compute the output of a general delta-sigma modulator with input u,
%a structure described by ABCD, an initial state x0 (default zero) and
%a quantizer with a number of levels specified by nlev.
%Multiple quantizers are implied by making nlev an array,
% and multiple inputs are implied by the number of rows in u.
%
%Alternatively, the modulator may be described by an NTF.
%The NTF is zpk object. (The STF is assumed to be 1.)
%The structure that is simulated is the block-diagional structure used by
%zp2ss.m.
%
%The obsolete NTF style is
%a struct with fields 'zeros' and 'poles' ('k' is assumed to be 1).
%the STF is assumed to be 1.
%The structure that is simulated is the block-diagional structure used by
%zp2ss.m.
*/
#include <stdio.h>
#include <math.h>
#include "mex.h"
/* Global variables */
/* In an effort to make the code more readable and to cut down on the overhead
associated with function calls, I have made many variables global. */
char *cmdName = "simulateDSM";
int
order, /* The order of the modulator. */
nu, /* The number of inputs, inferred from size(u,1). */
nq, /* The number of quantizers, inferred from nlev */
N, /* The number of time steps. */
ABCD_rows, /* The number of rows in ABCD */
saveState; /* Flag: keep track of the states. */
double
*u, /* Points into the input array. */
*v, /* Points into the output array. */
*x, /* The current state. */
*xn, /* Points (in)to the (output) state array. */
*xMax, /* Points to the state maxima output array. */
*py, /* Points to the quantizer input output array. */
*ABCD, /* The ABCD array (col-wise) description of the moduator. */
*nlev, /* The number of quantizer levels. */
default_nlev=2;
#ifdef __STDC__
double quantize(double yy, int nLevels)
#else
double quantize(yy, nLevels)
double yy;
int nLevels;
#endif
{
double vv;
if(nLevels%2) { /* Mid-tread quantizer */
vv = 2*floor(0.5*(yy+1));
if( vv > nLevels )
vv = nLevels-1;
else if( vv < -nLevels )
vv = 1-nLevels;
}
else { /* Mid-rise quantizer */
vv = 2*floor(0.5*yy)+1;
if( vv > nLevels )
vv = nLevels-1;
else if( vv < -nLevels )
vv = 1-nLevels;
}
return vv;
}
/* The following function is for debugging purposes only */
#ifdef __STDC__
void printMatrix(double *x, int m, int n)
#else
printMatrix(x, m, n)
double *x;
int m, n;
#endif
{
int i,j;
for(i=0; i<m; ++i){
for(j=0; j<n; ++j)
mexPrintf("%8.3f ",x[i+m*j]);
mexPrintf("\n");
}
}
#ifdef __STDC__
void fatalError(char *s)
#else
fatalError(s)
char *s;
#endif
{
char msg[1024];
sprintf(msg, "%s: %s", cmdName, s);
mexErrMsgTxt(msg);
}
#ifdef __STDC__
void initializeX(const mxArray *M_x0)
#else
initializeX(M_x0)
mxArray *M_x0;
#endif
{
int i;
double *x0 = mxGetPr(M_x0);
if( mxGetM(M_x0)!=order || mxGetN(M_x0)!=1 )
fatalError("x0 must be an order x 1 column vector.");
for(i=0; i<order; ++i)
x[i] = *x0++;
}
#ifdef __STDC__
void checkArgs(int nlhs, mxArray **plhs, int nrhs, const mxArray **prhs)
#else
checkArgs(nlhs, plhs, nrhs, prhs)
int nlhs, nrhs;
mxArray *plhs[], *prhs[];
#endif
{
int i;
int form;
double *pABCD;
const mxArray *arg2=prhs[1];
mxArray *zeros, *poles;
/* Verify the rhs (input) arguments */
if( nrhs < 2 )
fatalError("At least two input arguments are needed.");
if( !mxIsDouble(prhs[0]) )
fatalError("The input vector does not contain double-precision data.");
u = mxGetPr(prhs[0]);
nu = mxGetM(prhs[0]);
N = mxGetN(prhs[0]);
nq = 1;
nlev = &default_nlev;
if(nrhs>=3)
if( !( mxIsEmpty(prhs[2]) || mxIsNaN(*mxGetPr(prhs[2])) ) ){
nq = mxGetM(prhs[2]) * mxGetN(prhs[2]);
nlev = mxGetPr(prhs[2]);
}
/* Determine the form of the modulator */
if( mxIsClass(arg2,"zpk") ){ /* NTF in zpk form */
/* Matlab code: [z,p,k] = zpkdata(ntf); zeros = z{1}; poles=p{1} */
mxArray *lhs[3];
form = 0;
mexCallMATLAB(3,lhs,1,&arg2,"zpkdata");
zeros = mxGetCell(lhs[0],0);
poles = mxGetCell(lhs[1],0);
if( (order=mxGetNumberOfElements(zeros)) != mxGetNumberOfElements(poles) )
fatalError("The number of poles must equal the number of zeros.");
}
else if( mxIsStruct(arg2) ){ /* Obsolete NTF form */
if( (zeros=mxGetField(arg2,0,"zeros"))==0 )
fatalError("No zeros field in the NTF struct.");
if( !mxIsNumeric(zeros) )
fatalError("The zeros field is not numeric.");
if( (poles=mxGetField(arg2,0,"poles"))==0 )
fatalError("No poles field in the NTF struct.");
if( !mxIsNumeric(poles) )
fatalError("The poles field is not numeric.");
if( (order=mxGetNumberOfElements(zeros)) != mxGetNumberOfElements(poles) )
fatalError("The number of poles must equal the number of zeros.");
mexWarnMsgTxt("You appear to be using an old-style form of NTF specification.\nAutomatic converstion to the new form will be done for this release only.");
form = 2;
}
else if( mxIsNumeric(arg2) ){
if( mxGetN(arg2)==mxGetM(arg2)+nu && mxIsDouble(arg2) ){
form = 1; /* ABCD form */
order = mxGetM(arg2)-nq;
}
else if( mxGetN(arg2)==2 ){
mexWarnMsgTxt("You appear to be using the old-style form of NTF specification.\nAutomatic converstion to the new form will be done for this release only.");
form = 3; /* old NTF form */
order = mxGetM(arg2);
}
else
fatalError("ABCD must be an order+nq by order+nu+nq matrix.");
}
else
fatalError("The second argument is neither a proper ABCD matrix nor an NTF.");
if( form==1 ){ /* ABCD form */
ABCD = mxGetPr(arg2);
}
else if( form==0 || form==2 || form==3 ){ /* NTF form */
/* MATLAB code for computing ABCD
[A,B2,C,D2] = zp2ss(ntf.poles,ntf.zeros,-1);
D2 = 0;
% !!!! Assume stf=1
B1 = -B2;
D1 = 1;
ABCD = [ A B1 B2; C D1 D2]
*/
mxArray *lhs[4],*pntf[3];
double *p1,*p2;
if( nu != 1 )
fatalError("Fatal error. Number of inputs must be 1 for a modulator specified by its NTF\n");
if( nq != 1)
fatalError("Fatal error. Number of quantizers must be 1 for a modulator specified by its NTF\n");
/* Copy the ntf (arg2) into the temporary pntf[] matrices. */
/* This could be accomplished more efficiently (but more dangerously)
by directly setting the elements of the mxArray data structure. */
pntf[0] = mxCreateDoubleMatrix(order,1,mxCOMPLEX);
pntf[1] = mxCreateDoubleMatrix(order,1,mxCOMPLEX);
pntf[2] = mxCreateDoubleMatrix(1,1,mxREAL);
p2 = mxGetPr((form==0||form==2) ? zeros:arg2);
for( p1=mxGetPr(pntf[1]), i=0; i<order; ++i)
*p1++ = *p2++;
if( form!=3 )
p2 = mxGetPr(poles);
for( p1=mxGetPr(pntf[0]), i=0; i<order; ++i)
*p1++ = *p2++;
p2 = mxGetPi((form==0||form==2) ? zeros:arg2);
if( p2 ) /* Non-null imaginary part. */
for( p1=mxGetPi(pntf[1]), i=0; i<order; ++i)
*p1++ = *p2++;
if( form!=3 )
p2 = mxGetPi(poles);
if( p2 ) /* Non-null imaginary part. */
for( p1=mxGetPi(pntf[0]), i=0; i<order; ++i)
*p1++ = *p2++;
*mxGetPr(pntf[2]) = -1;
mexCallMATLAB(4,lhs,3,pntf,"zp2ss");
p1 = mxGetPr(lhs[0]);
p2 = mxGetPr(lhs[2]);
ABCD = (double *)mxCalloc((order+nu)*(order+nu+nq),sizeof(double));
pABCD = ABCD;
for( i=0; i<order; ++i ){
int j;
for( j=0; j<order; ++j )
*pABCD++ = *p1++;
*pABCD++ = *p2++;
}
p1 = mxGetPr(lhs[1]); /* B1 = -B2 */
for( i=0; i<order; ++i )
*pABCD++ = -*p1++;
*pABCD++ = 1; /* D1 */
p1 = mxGetPr(lhs[1]); /* B2 */
for( i=0; i<order; ++i )
*pABCD++ = *p1++;
*pABCD = 0; /* D2 */
for(i=0;i<4;++i)
mxDestroyArray(lhs[i]);
for(i=0;i<2;++i)
mxDestroyArray(pntf[i]);
}
else
fatalError("Internal error. form != 0, 1, 2 or 3!");
ABCD_rows = order + nq;
plhs[0] = mxCreateDoubleMatrix(nq,N,mxREAL);
v = mxGetPr(plhs[0]);
x = (double *)mxCalloc(order,sizeof(double));
if(nrhs>=4){
if( !( mxIsEmpty(prhs[3]) || mxIsNaN(*mxGetPr(prhs[3])) ) )
initializeX(prhs[3]);
}
/* Verify the lhs (output) arguments */
saveState=0;
py=0;
xMax=0;
switch(nlhs){
case 4:
plhs[3] = mxCreateDoubleMatrix(nq,N,mxREAL);
py = mxGetPr(plhs[3]);
case 3:
plhs[2] = mxCreateDoubleMatrix(order,1,mxREAL);
xMax = mxGetPr(plhs[2]);
case 2:
plhs[1] = mxCreateDoubleMatrix(order,N,mxREAL);
xn = mxGetPr(plhs[1]);
saveState=1;
break;
case 1:
break;
default:
fatalError("Incorrect number of output arguments.");
}
if( !saveState )
xn = (double *)mxCalloc(order,sizeof(double));
}
/* Simulate the modulator using the difference equations. */
/* For efficiency, store the state in xn and compute from x. */
/* (These variables may be recycled internally, depending
on the output variables requested.) */
#ifdef __STDC__
void simulateDSM()
#else
simulateDSM()
#endif
{
int i,j,t, qi;
double *pABCD, *ptr, *pxn, tmp;
for( t=0; t<N; ++t ){ /* [xn;y] = ABCD*[x;u;v]; x=xn;*/
/* Compute y = C*x + D1*u and thence v for each quantizer */
for( qi=0; qi<nq; ++qi){
tmp = 0;
for(i=0, pABCD=ABCD+order+qi, ptr=x; i<order; ++i, pABCD+=ABCD_rows)
tmp += (*pABCD) * *ptr++;
for(i=0, ptr=u; i<nu; ++i, pABCD+=ABCD_rows)
tmp += (*pABCD) * *ptr++;
if( py!=0 )
*py++ = tmp;
v[qi] = quantize(tmp, nlev[qi]);
}
/* Next compute xn = A*x + B*[u;v], */
for( i=0, pxn=xn; i<order; ++i ){
tmp=0;
pABCD=ABCD+i;
for( ptr=x, j=0; j<order; ++j, pABCD += ABCD_rows )
tmp += *pABCD * *ptr++;
for( ptr=u, j=0; j<nu; ++j, pABCD += ABCD_rows )
tmp += *pABCD * *ptr++;
for( ptr=v, j=0; j<nq; ++j, pABCD += ABCD_rows )
tmp += *pABCD * *ptr++;
*pxn++ = tmp;
}
u += nu;
v += nq;
if(xMax!=0){
for( i=0; i<order; ++i){
double abs=fabs(xn[i]);
if( abs > xMax[i] )
xMax[i] = abs;
}
}
if(saveState){
x = xn;
xn += order;
}
else { /* swap x and xn */
double *xtmp = x;
x = xn;
xn = xtmp;
} /* if(saveState) */
} /* for( t=0 .... ) */
}
#ifdef __STDC__
void mexFunction(int nlhs, mxArray **plhs, int nrhs, const mxArray **prhs)
#else
mexFunction(nlhs, plhs, nrhs, prhs)
int nlhs, nrhs;
mxArray *plhs[], *prhs[];
#endif
{
checkArgs(nlhs, plhs, nrhs, prhs);
/* Print the variables being used
mexPrintf("x=\n"); printMatrix(x,order,1);
mexPrintf("\nABCD=\n"); printMatrix(ABCD,order+nq,order+nu+nq);
*/
simulateDSM();
}