00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00036
00037
00038
00039 #ifndef CONTROL_H
00040 #define CONTROL_H
00041
00042 #define MAX_COMMANDS 10 // max number of commands that can be registered
00043
00044
00045
00046
00047
00048 #include "Utils.h"
00049 #include "QDOperations.h"
00050 #include "FilterAndRotations.h"
00051 #include "DataStorage.h"
00052
00053
00054
00055
00056
00059 struct VHzConfiguration
00060 {
00061 _iq twopidt;
00062 _iq PWMVCO;
00063 volatile struct sqddata vqds_s;
00064 volatile _iq voltage;
00065 volatile _iq theta;
00066 };
00067
00068 extern struct VHzConfiguration vhzConf;
00069
00072 typedef struct CommandsData
00073 {
00074 _iq vbegin;
00075 _iq vend;
00076 _iq tUS;
00077 Uint32 actualSample;
00078 Uint32 finalSample;
00079 volatile _iq* command;
00080 void* extra;
00081 void (*CommandFcn)(struct CommandsData* command);
00082 unsigned char start;
00083 } commandsData;
00084
00085
00086
00089 typedef struct CommandGeneratorCfg
00090 {
00091 unsigned int ncommand;
00092 _iq schedPeriodUS;
00093 _iq maxCommandError;
00094 } _commandGeneratorCfg;
00095
00096
00099 typedef struct CubicCommandsData
00100 {
00101 unsigned long count;
00102 _iq tSEnd;
00103 _iq tSAct;
00104 _iq schedPeriodS;
00105 _iq derBegin;
00106 _iq derEnd;
00107 _iq equConsta;
00108 _iq equConstb;
00109 _iq equConstc;
00110 _iq equConstd;
00111 } _cubiccommandsData;
00112
00113
00114
00115
00116 extern _commandGeneratorCfg commandGeneratorCfg;
00117 extern commandsData commands[MAX_COMMANDS];
00118
00119
00120
00121
00122
00127
00128 extern void InitVHz(_iq vbus, _iq fmax, _iq ts);
00129 static inline void VHz(_iq F, volatile _iq* vas, volatile _iq* vbs,
00130 volatile _iq* vcs);
00131 static inline void HomopolarInjection(volatile _iq *vas,
00132 volatile _iq *vbs, volatile _iq *vcs);
00133
00134
00135
00136
00137
00138 static inline int RegisterCommand(_iq vbegin, _iq vend, _iq tUS,
00139 volatile _iq* command, commandsData** commandData,
00140 void (*CommandFcn)(struct CommandsData* command),void* extra);
00141 static inline void StartCommands(void);
00142 static inline void StartCommand(struct CommandsData* command);
00143 static inline void SetCommand(struct CommandsData* command, _iq vbegin, _iq vend);
00144 static inline void ConfigureCommandGenerator(_iq schedPeriodUS, _iq error);
00145 static inline void StartCommandGenerator(void);
00146 static inline void UpdateCommands(void);
00147 static inline void UpdateCommand(struct CommandsData* command);
00148 static inline int IsValue(struct CommandsData* command, _iq value, _iq error);
00149 static inline int IsFinalValue(struct CommandsData* command, _iq error);
00150
00151
00152 static inline void RampCommandFcn(struct CommandsData* command);
00153
00154
00155 static inline void SCommandFcn(struct CommandsData* command);
00156 static inline void SConfigFcn(struct CommandsData* command, void* extra);
00157
00158 static inline void CubicCommandFcn(struct CommandsData* command);
00159 static inline void CubicConfigFcn(struct CommandsData* command, void* extra,_iq derBegin,_iq derEnd);
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00184
00185 static inline void VHz(_iq F, volatile _iq* vas, volatile _iq* vbs,
00186 volatile _iq* vcs)
00187 {
00188 vhzConf.theta+=_IQmpy(F,vhzConf.twopidt);
00189 WRAP2PI(vhzConf.theta);
00190 vhzConf.voltage=_IQmpy(F,vhzConf.PWMVCO);
00191 SSCALAR2SYNCVEC(vhzConf.voltage,vhzConf.vqds_s,vhzConf.theta);
00192 DQ2PH(vhzConf.vqds_s, *vas, *vbs, *vcs);
00193 }
00194
00195
00196
00197
00198
00206
00207 static inline void HomopolarInjection(volatile _iq *vas,
00208 volatile _iq *vbs, volatile _iq *vcs)
00209 {
00210 volatile _iq *vphase_max, *vphase_min;
00211 _iq homopolar;
00212
00213 GetMax(vas,vbs,&vphase_max);
00214 GetMax(vphase_max,vcs,&vphase_max);
00215 GetMin(vas,vbs,&vphase_min);
00216 GetMin(vphase_min,vcs,&vphase_min);
00217
00218
00219
00220 homopolar = -(long)(*vphase_max + *vphase_min) >> 1;
00221 *vas +=homopolar;
00222 *vbs +=homopolar;
00223 *vcs +=homopolar;
00224 }
00225
00226
00227
00228
00229
00241
00242 static inline int RegisterCommand(_iq vbegin, _iq vend, _iq tUS,
00243 volatile _iq* command, commandsData** commandData,
00244 void (*CommandFcn)(struct CommandsData* command), void* extra)
00245 {
00246 if(commandGeneratorCfg.ncommand > MAX_COMMANDS)
00247 return -1;
00248 commands[commandGeneratorCfg.ncommand].vbegin = vbegin;
00249 commands[commandGeneratorCfg.ncommand].vend = vend;
00250 commands[commandGeneratorCfg.ncommand].actualSample = 0;
00251 commands[commandGeneratorCfg.ncommand].finalSample = _IQtoF(_IQdiv(tUS,commandGeneratorCfg.schedPeriodUS));
00252 commands[commandGeneratorCfg.ncommand].tUS = tUS;
00253 commands[commandGeneratorCfg.ncommand].command = command;
00254 commands[commandGeneratorCfg.ncommand].extra = 0;
00255 commands[commandGeneratorCfg.ncommand].CommandFcn = CommandFcn;
00256 commands[commandGeneratorCfg.ncommand].extra = extra;
00257 commands[commandGeneratorCfg.ncommand].start = 0;
00258
00259 *commandData = &(commands[commandGeneratorCfg.ncommand]);
00260
00261 commandGeneratorCfg.ncommand++;
00262 return 0;
00263
00264 }
00265
00266
00267
00268
00270
00271 static inline void StartCommands(void)
00272 {
00273 int cc;
00274 for (cc=0;cc<commandGeneratorCfg.ncommand;cc++){
00275 commands[cc].start = 1;
00276 *(commands[cc].command) = commands[cc].vbegin;
00277 }
00278 }
00279
00280
00281
00282
00285
00286 static inline void StartCommand(struct CommandsData* command)
00287 {
00288 command->start = 1;
00289 *(command->command) = command->vbegin;
00290 }
00291
00292
00293
00294
00299
00300 static inline void SetCommand(struct CommandsData* command, _iq vbegin, _iq vend)
00301 {
00302 if( (IsValue(command,vend,commandGeneratorCfg.maxCommandError)) ||
00303 (command->start == 1) )
00304 return;
00305 command->start = 1;
00306 command->vbegin = vbegin;
00307 command->vend = vend;
00308 command->actualSample = 0;
00309 *(command->command) = vbegin;
00310 }
00311
00312
00313
00314
00318
00319 static inline void ConfigureCommandGenerator(_iq schedPeriodUS, _iq error)
00320 {
00321 commandGeneratorCfg.ncommand = 0;
00322 commandGeneratorCfg.schedPeriodUS = schedPeriodUS;
00323 commandGeneratorCfg.maxCommandError = error;
00324 }
00325
00326
00327
00328
00331
00332 static inline void StartCommandGenerator(void)
00333 {
00334 }
00335
00336
00337
00338
00340
00341 static inline void UpdateCommands(void)
00342 {
00343 int cc;
00344 for (cc=0;cc<MAX_COMMANDS;cc++){
00345 UpdateCommand(&commands[cc]);
00346 }
00347 }
00348
00349
00350
00351
00354
00355 static inline void UpdateCommand(struct CommandsData* command)
00356 {
00357 if (command->start == 1){
00358
00359 if((command->actualSample)++ != command->finalSample)
00360 command->CommandFcn(command);
00361 else{
00362 *command->command = command->vend;
00363 command->start = 0;
00364 }
00365 }
00366 }
00367
00368
00369
00370
00376
00377 static inline int IsValue(struct CommandsData* command, _iq value, _iq error)
00378 {
00379 return (_IQabs(*(command->command) - value) <= error);
00380 }
00381
00382
00383
00384
00389
00390 static inline int IsFinalValue(struct CommandsData* command, _iq error)
00391 {
00392 return IsValue(command, command->vend, error);
00393 }
00394
00395
00396
00397
00400
00401 static inline void RampCommandFcn(struct CommandsData* command)
00402 {
00407 *command->command += (command->vend-command->vbegin)/command->finalSample;
00408 }
00409
00410
00411
00412
00416
00417 static inline void SCommandFcn(struct CommandsData* command)
00418 {
00419 struct CubicCommandsData* sCommand = (struct CubicCommandsData*)(command->extra);
00420 sCommand->count++;
00421 sCommand->tSAct=sCommand->schedPeriodS*sCommand->count;
00422
00423
00424 *command->command =_IQmpy(sCommand->equConsta,_IQmpy(sCommand->tSAct,_IQmpy(sCommand->tSAct,sCommand->tSAct)))
00425 +_IQmpy(sCommand->equConstb,_IQmpy(sCommand->tSAct,sCommand->tSAct))
00426
00427 +sCommand->equConstd;
00428
00429
00430
00431
00432
00433
00434
00435
00436 }
00437
00438
00439
00440
00445
00446 static inline void SConfigFcn(struct CommandsData* command, void* extra)
00447 {
00448 _cubiccommandsData* sCommand = (_cubiccommandsData*)(extra);
00449 sCommand->count=0;
00450 sCommand->tSEnd=command->tUS/1000000;
00451 sCommand->schedPeriodS=commandGeneratorCfg.schedPeriodUS/1000000;
00452
00453 sCommand->equConstd=command->vbegin;
00454 sCommand->equConstc=0;
00455 sCommand->equConsta=_IQdiv((command->vend-command->vbegin),
00456 _IQmpy(_IQmpy(_IQmpy(
00457 sCommand->tSEnd,sCommand->tSEnd)
00458 ,sCommand->tSEnd),_IQ(-0.5)));
00459 sCommand->equConstb=_IQmpy(_IQ(-1.5),
00460 _IQmpy(sCommand->equConsta,sCommand->tSEnd));
00461 }
00462
00463
00464
00465
00468
00469
00470 static inline void CubicCommandFcn(struct CommandsData* command)
00471 {
00472 struct CubicCommandsData* cubicCommand = (struct CubicCommandsData*)(command->extra);
00473 cubicCommand->count++;
00474 cubicCommand->tSAct=cubicCommand->schedPeriodS*cubicCommand->count;
00475
00476 *command->command =_IQmpy(cubicCommand->equConsta,_IQmpy(cubicCommand->tSAct,_IQmpy(cubicCommand->tSAct,cubicCommand->tSAct)))
00477 +_IQmpy(cubicCommand->equConstb,_IQmpy(cubicCommand->tSAct,cubicCommand->tSAct))
00478 +_IQmpy(cubicCommand->equConstc,cubicCommand->tSAct)
00479 +cubicCommand->equConstd;
00480
00481
00482
00483
00484
00485
00486
00487 }
00488
00489
00490
00491
00498
00499 static inline void CubicConfigFcn(struct CommandsData* command, void* extra,_iq derBegin,_iq derEnd)
00500 {
00501 _cubiccommandsData* cubicCommand = (_cubiccommandsData*)(extra);
00502 cubicCommand->count=0;
00503 cubicCommand->tSEnd=command->tUS/1000000;
00504 cubicCommand->schedPeriodS=commandGeneratorCfg.schedPeriodUS/1000000;
00505 cubicCommand->derBegin=derBegin;
00506 cubicCommand->derEnd=derEnd;
00507 cubicCommand->equConstd=command->vbegin;
00508 cubicCommand->equConstc=cubicCommand->derBegin;
00509 cubicCommand->equConsta=_IQdiv(command->vend-command->vbegin-_IQmpy((long)(cubicCommand->derBegin+cubicCommand->derEnd)>>1,cubicCommand->tSEnd)
00510 ,_IQmpy(_IQmpy(_IQmpy(
00511 cubicCommand->tSEnd,cubicCommand->tSEnd)
00512 ,cubicCommand->tSEnd),_IQ(-0.5)));
00513 cubicCommand->equConstb=_IQdiv((cubicCommand->derEnd-cubicCommand->derBegin-
00514 _IQmpy(cubicCommand->equConsta*3,
00515 _IQmpy(cubicCommand->tSEnd,cubicCommand->tSEnd))),
00516 (long)cubicCommand->tSEnd<<1);
00517 }
00518
00519
00520
00521
00522
00523
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00564
00565 #define RealizableReference(ref, meass, error, controlAction, maxControlAction, kp, ki){\
00566 if(_IQabs(controlAction[N])>= maxControlAction ){\
00567 controlAction[N] = _IQmpy(maxControlAction,_IQ(SIGN(controlAction[N])));\
00568 INV_PI_REG(error,controlAction,kp,ki);\
00569 }\
00570 }
00571
00572
00573
00574
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00619 #define RealizableReferenceQD(ref, meass, error, controlAction, maxControlAction, kp, ki){\
00620 if(MAG_QD_NDATA(controlAction,N)>= maxControlAction ){\
00621 MODARG2QD_NDATA(maxControlAction,PHASE_QD_NDATA(controlAction,N),controlAction,N); \
00622 INV_PI_QD_REG(error,controlAction,kp,ki);\
00623 }\
00624 }
00625
00626
00627
00628 #endif
00629
00630
00631
00632
00633
00634