scopedemo.c

基于嵌入式实时开发系统Tornado 2的工具使用内部培训资料

/* vxdemo2 \- A (simple) demo for StethoScope. */

/* 
   modification history
   ------------ -------
   5.1e,02apr99,laf Provide Shutdown functionality to clean up.
   5.1a,26oct98,nm  ScopeInitServer() now takes two buffer sizes.
   5.1a,16oct98,nm  Set WtxOverride to 1, not to true.
   5.1a,12aug98,nm  Updated for scope 5.1 release.
   5.0b,24jun97,sas  Added position & velocity gains to signals.
   RTI,03oct94,sas  Changed to controls demo.
   RTI,20dec92,sas  Added ScopeIndex.  Removed support for VxWorks 4.x
   RTI,25nov92,sas  Ported to VxWorks 5.1
   RTI,06may92,sas  Added rebootHookAdd to insure reboot success.  
		    Made VxWorks 5.0 the default.
   RTI,03apr92,sas  Converted to mangen format.
   RTI,01sep89,sas  written.
*/

/*
   DESCRIPTION:
      This file contains code to start a simple synchronous sampler, and
      install a few demo signals.

      This is a complete VxWorks application.  It performs all the necessaary 
      setup.

   NOTES:
      This code normally works using taskDelay.  It can also work by attaching
      a semaphore (sampleSemaphore) to the Aux clock interrupt, the recommended
      means of executing periodic functions in VxWorks.  If your processor does
      have an aux-clock, set the "useAuxClock" parameter to 1.

   SEE ALSO:
      ScopeProbe(2), scope(1), vxdemo(3)
*/

/* (c) Copyright Real-Time Innovations, Inc., 1999.  All rights reserved. */

#include "vxWorks.h"
#include "stdioLib.h"
#include "math.h"
#include "rebootLib.h"
#include "taskLib.h"
#include "sysLib.h"
#include "semLib.h"
#include "scope/scope.h"


/* A very simple VxWorks "C" program to simulate a control system. */

#define STEPDELAY (900)

typedef struct {
    float pos;
    float vel;
    float acc;

    int stepCount;	/* Poor-man's trajectory generator. */
    float posDes;
    float velDes;

    SEM_ID sampleSemaphore;
    int scopeIndex;
} *ControlInfo, ControlInfoStorage;

/* Controller gains; accessible from the VxWorks shell. */
float Kp = 10;
float Kv = 4;

ControlInfo TheCI;
int tid = 0;

static
/* This "plant" is just an Euler double integrator.  */
void Plant(ControlInfo ci, float t, float dt)
{
    ci->vel += ci->acc * dt;		
    ci->pos += ci->vel * dt;
}

static
/* A simple PD controller. */
void Control(ControlInfo ci, float t)
{
    if(ci->stepCount-- <= 0) {
	ci->stepCount = STEPDELAY;
	ci->posDes = -ci->posDes;
    }
    ci->acc = Kp*(ci->posDes - ci->pos) + Kv*(ci->velDes - ci->vel);
}

static
/* This routine attaches a semaphore to the Aux clock iff useAuxClock is
 * non-zero.
 */
void ScopeDemoSampler(ControlInfo ci, int requestedSR, int useAuxClock)
{
    int nTicksBwSamples = 0;
    float actualSR;
    float time = 0.0;
    float dt;

    if (useAuxClock) {
	ci->sampleSemaphore = semBCreate(SEM_Q_PRIORITY, SEM_EMPTY);
    }

    /* Connect the semaphore to the auxillary clock interrupt. */
    if (useAuxClock &&
	(sysAuxClkConnect(semGive, (long)ci->sampleSemaphore) == OK)) {
        sysAuxClkEnable();
        sysAuxClkRateSet(requestedSR);
        /* hardware may not be exact */
        actualSR = sysAuxClkRateGet();
	rebootHookAdd((FUNCPTR) sysAuxClkDisable);
    } else {
        actualSR = sysClkRateGet();
        if (actualSR < requestedSR) {
            /* Requested rate (requestedSR) is higher than
             * maximum achievable rate.
             */
            nTicksBwSamples = 1;
        } else {
            nTicksBwSamples = actualSR / requestedSR;
            actualSR = requestedSR;
        }
    }

    /* Tell scope the actual sample rate (hardware may not be exact). */
    ScopeChangeSampleRate(actualSR, ci->scopeIndex);

    if((actualSR/requestedSR > 1.1) || (actualSR/requestedSR < 0.9)) {
        printf("ScopeDemo: requested rate (%f Hz) not \n"
	       "achievable, actual sampling rate is %f Hz\n",
	       (float) requestedSR, actualSR);
    }

    dt = 1./actualSR;
    while(1) {
	if (useAuxClock) {
	    semTake(ci->sampleSemaphore, WAIT_FOREVER);
	} else {
	    taskDelay(nTicksBwSamples);
	}
	time += dt;
	Plant(ci, time, dt);
	Control(ci, time);
	ScopeCollectSignals(ci->scopeIndex);
    }
}

void ScopeDemo(void)
{
    int dataBufSize, signalBufSize;
    int samplingRate = 60;	/* Hz */

    TheCI = (ControlInfo) calloc(1,sizeof(*TheCI));

    TheCI->posDes = 1.0;
    TheCI->scopeIndex = 0;

    /* Initialize */

    WtxOverrideGlobal = 0;
/*    WtxOverrideGlobal = 1;
*/
    dataBufSize = 4*8*1024; /* 8k data buffer (ints/floats) */;
    signalBufSize = 640; /* approx. 12 signals, assuming name & units for a
			  * signal is 25 characters (including two '\0'
			  * characters).
			  */
    if (ScopeInitServer(dataBufSize, signalBufSize, 1, 0)
	!= 0) {
	return;
    }

    /* Install signals to StethoScope. */
    if (!ScopeInstallSignal("stepCount", "count", &TheCI->stepCount,
			    "int", TheCI->scopeIndex)) {
        return;
    }
    if (!ScopeInstallSignal("Pos", "meters", &TheCI->pos, "float",
			    TheCI->scopeIndex)) {
	return;
    }
    if (!ScopeInstallSignal("PosDes", "meters", &TheCI->posDes,
			    "float", TheCI->scopeIndex)) {
        return;
    }
    if (!ScopeInstallSignal("Vel", "m/s", &TheCI->vel, "float",
			    TheCI->scopeIndex)) {
        return;
    }
    if (!ScopeInstallSignal("Acc", "m/s/s", &TheCI->acc, "float",
			    TheCI->scopeIndex)) {
        return;
    }
    if (!ScopeInstallSignal("PosGain", "N/m", &Kp, "float",
			    TheCI->scopeIndex)) {
        return;
    }
    if (!ScopeInstallSignal("VelGain", "N/m/s", &Kv, "float",
			    TheCI->scopeIndex)) {
        return;
    }

    tid = taskSpawn("ScopeDemo", 100, VX_FP_TASK, 0x4000, 
	      (FUNCPTR) ScopeDemoSampler,
	      (int)TheCI, samplingRate, 0,0,0,0,0,0,0,0);
}


void ScopeDemoShutdown(void)
{
    taskDelete(tid);
    ScopeShutdown(TheCI->scopeIndex);
    tid = 0;
    free(TheCI);
}