act.c

Most satellite dish actuators send a series of pulses to indicate their position. By counting pulse

#define REALLY_SLOW_IO

#include <stdio.h>
#include <errno.h>
#include <sys/mman.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>

#include <rtai_lxrt.h>
#include <rtai_sem.h>
#include <rtai_usi.h>
#include <rtai_fifos.h>
#include <time.h>
#include <sys/io.h>

#define BASEPORT (unsigned short)0x3f8   // COM1
#define MOTORPORT BASEPORT+4      // Line control register
#define STATUSPORT BASEPORT+6     // Modem status register.

#define MOTOR_EAST 0x01
#define MOTOR_WEST 0x02
#define MOTOR_OFF 0x03
RT_TASK *maint;
int squarethread;


static volatile int exec_end = 1;
static volatile unsigned char motor_state=MOTOR_OFF;    //1=on, 0=off
static volatile int motor_direction=0; 

#define PERIOD     50000000            // in nanoseconds.


int read_dsr(void) 
{   
  return inb_p(STATUSPORT) & 0x20;
}


void write_status(int fifo, int count)
{
  // Maximum count for my actuator is 907.  I use 12 bits of positional 
  // accuracy allowing for 4097. Should be enough for the longest of 
  // actuators!
  unsigned short value=0;
  value = 0x0fff & count;
  value |= 0x3000 & (motor_state << 12);
  value |= 0x4000 & (motor_direction << 14);
  rtf_put(fifo, &value, sizeof(value));
}


// Periodic sampling
static void *position_handler(void *args) {
        RT_TASK *handler;
        RTIME period;
        unsigned char command;
        int count;
        int last_state, this_state;
        int outfifo,infifo;

        if (!(handler = rt_task_init_schmod(nam2num("ACHLR"), 
                 0, 0, 0, SCHED_FIFO, 0xF))) {
                printf("Unable to init timer handler task.\n");
                exit(1);
        }

        if (0!=rtf_create(1, 100)) {
          printf("Unable to access command FIFO.\n");
          rt_task_delete(handler);
          exit(1);
        }
        infifo = 1;

        if (0!=rtf_create(0, 100)) {
          printf("Unable to access output FIFO\n");
          rt_task_delete(handler);
          rtf_destroy(infifo);
          exit(1);
        }
        outfifo=0;

        // Ensure the motor control lines are both high
        // or there won't be power for the pulse counting circuit
        outb_p((unsigned char)motor_state,MOTORPORT);

        rt_allow_nonroot_hrt();
        // no swapping
        mlockall(MCL_CURRENT | MCL_FUTURE);

        rt_set_oneshot_mode();
        start_rt_timer(0);
        period = nano2count(PERIOD);
        rt_make_hard_real_time();
        exec_end = 0;
        rt_task_make_periodic(handler, rt_get_time() + period, period);
        rt_task_wait_period();
        // Note:  if we start count at 1000 we can tell if it goes negative...
        count = 1000;
        motor_direction = 1;
        last_state = this_state = read_dsr();
        while (!exec_end) {
                rt_task_wait_period();
                this_state = read_dsr();
                if (last_state!=this_state) {
                  if (motor_direction) {
                    count++;
                  } else {
                    count--;
                  }                  
                }
                
                // Check on the user-space command.
                if (1==rtf_get(infifo, &command, 1)) 
                {
                  switch (command) {
                    case 'w' :
                      // start counting down (motor driven west)
                      motor_direction = 0;
                      motor_state = MOTOR_WEST;
                      break;
                    case 'e' :
                      // start counting up (motor driven east)
                      motor_direction = 1;
                      motor_state = MOTOR_EAST;
                      break;
                    case 'h' :
                      // Halt the motor
                      motor_state = MOTOR_OFF;
                      break;
                  }
                  
                  // Update the port with the state of the motor.
                  outb_p((unsigned char)motor_state,MOTORPORT);

                  write_status(outfifo, count);
                }
                // Read ports, and start timer.
                last_state = this_state;
        }
        motor_state = MOTOR_OFF;
        outb_p((unsigned char)motor_state,MOTORPORT);
        
        stop_rt_timer();
        rt_make_soft_real_time();
        rt_task_delete(handler);
        rtf_destroy(infifo);
        rtf_destroy(outfifo);
        return 0;
}

void cleanup(int sig) {
        exec_end = 1;
        return;
}


int main(void) {
        
        signal(SIGTERM, cleanup);
        signal(SIGINT, cleanup);
        signal(SIGKILL, cleanup);
        
        if (!(maint = rt_task_init(nam2num("MAIN"), 1, 0, 0))) {
                printf("Cannot initialise main tastk.\n");
                exit(1);
        }
        // ask for permission to access the port from user-space
        ioperm(BASEPORT,8,1);
        ioperm(0x80,1,1);

        squarethread = rt_thread_create(position_handler, NULL, 10000);

        while (exec_end) {   
                usleep(100000);
        }
        printf("Actuator task is realtime\n");
        while (!exec_end) {
          usleep(200000);
        }

        rt_thread_join(squarethread);
        rt_task_delete(maint);
        return 0;
}