May 22, 2020

Servo Hooks - part 1

The el_servo model is set up as an optional add-on to the mount code. Of course it is far from optional today, and has been in regular use for approximately 10 years, but that is the fundamental design.

The basic idea is that el_servo.o can get loaded after mount.o and then if a switch is set, the mount code calls one routine in el_servo which sets everything up. In theory this switch could not be set and the el_servo file could not be loaded, and we would fall back on some prior servo. It is not clear what that servo even is at this point, or if the hardware exists to support it in the current mount. But that is not the point, the point is to understand the connections between that servo and the mount code, and those connections are what I call hooks.

Dynamic call of rt_run()

Some vxworks trickery is used to allow code in mount.o to call a function in el_servo.o. The code is in main.c in function servo_task() and is as follows: 

    if ( start_simulink_servos ) {
	dynamic_call ( "rt_run" );
	start_simulink_servos = 0;
    }
So if this flag is set (as it is in the startup script), the function rt_run() is called once when the servo system starts up. This performs additional hookup and initialization, including the spawning of a task to run the servo at 1 kHz.

Scheduling the simulink servo to run at 1 kHz

There is some trickery here also, given that the servo module loads after the main mount code. The tricks are in timer.c. When rt_run() is called, it will make a call back to a function: 
void mmt_mount_connect ( SEM_ID sem, int rate );
This registers a semaphore with our timer code and instructs the timer at what rate to activate the servo using this semaphore. The code is: 
/* Called by the simulink/RTW wrapper to connect to our
 * timer, the rate argument is typically 1000 (when it
 * expects to be activated via this semaphore at 1000 Hz)
 */
void
mmt_mount_connect ( SEM_ID sem, int rate )
{
        int tics;

        simulink_sem = sem;

        simulink_count = 0;
        simulink_tics = 1;

        if ( rate > 0 ) {
            tics = CLOCK_RATE / rate;
            if ( tics * rate == CLOCK_RATE )
                simulink_tics = tics;
        }
}
The following code is part of the timer interrupt routine: 
/* If simulink model is active,
         * run it at 1000 Hz
         * (Actually we allow down count to slower rates)
         */
        if ( simulink_sem ) {
            if ( simulink_tics > 1 ) {
                if ( ++simulink_count > simulink_tics ) {
                    semGive ( simulink_sem );
                    simulink_count = 0;
                }
            } else {
                semGive ( simulink_sem );
            }
        }