[ECOS] Servo Control Problems on the ARM AEB1

[louis l legrand <legrand at u dot washington dot edu>]

Greetings,

I realize this is sort of a general question.  I was hoping to isolate the
problem more, so that I could ask more specific questions.  Unfortunately
this is the best I could isolate the problem.  I have been working on it
for quite a while, and I thought maybe there is something obvious that you
could see.  First, a little background.......

One sub goal of our project requires control of up to eight RC-type
servos.  As you probably know, these servos are controlled via pulse width
modulation (PWM).  The frequency of the PWM is not important (within a
range of 30 to 360 Hz), and the servo position is determined by the
duration of the duty cycle (1 ms to 2 ms).  The AEB1 only supports three
PWM channels in hardware.  In order to support more PWM channels, we
developed a software driven PWM scheme that utilizes one hardware PWM
channel, one hardware timer, and one I/O port as outlined below.

For one channel of software PWM (which can easily be expanded to support
up to eight channels):

- The hardware PWM channel is set up to trigger an interrupt each time it
goes high.  I call this PWM interrupt the heartbeat.

- At each heartbeat, the 0'th bit of the I/O port is set to 1, and the
timer is set to expire after a certain time (between 1 and 2 ms)

- When the timer expires, it triggers an interrupt that sets the 0th bit
of the I/O port to 0.

-Note that both of these interrupt handlers are set up using eCos and
both run only the ISR so there should be no latency to to DSR queing.

The 0th bit of the I/O port is one channel of PWM.  To add channels, the
heartbeat must be sped up, and  multiple bits of the I/O port must be
cycled through, toggling on and off in succession.

In order for this scheme to work, the interrupt latencies must be small
(or at least consistent).  If there is an inconsistent latency, the duty
cycle will vary, and the servo position will not be consistent.  In the
grand scheme of the project, accurate and consistent servo positioning is
most important, and the performance of the other threads are of secondary
importance.

Attached is the (simplified) code we have developed to do one channel of
software PWM.  It works for the most part, but we have been plagued by a
periodic slight servo jitter.  The appearance of the jitter depends on the
settings for the PWM heartbeat.  Some heartbeat frequencies cause a jitter
and others don't.

Table of PWM heartbeat settings and the resulting behavior (24 MHz ARM
AEB1)


                 heartbeat 
div      tc      frequency     servo behavior
--------------------------------------------- 
50      0x257F    50.000       No jitter
 
8       0xEA5F    50.000       No jitter 

8       0xF423    48.000       No jitter 

50      0x2710    47.9952      3 little jumps about 0.5 seconds 
                               apart every 8.5 seconds
 
8       0xF410    48.0146      a little jiggle every 2.7 seconds
 
8       0xEA50    50.0125      a slow ramp up in error for about 
                               2 seconds then a quick return to 
                               nominal every 40 seconds
 
8      0xEA5E     50.00083     two saw tooth error bumps over 
                               about 14 seconds every 600 seconds


The question is, what is causing the jitter? Even though there is no
jitter at 50.00 and 48.00 Hz, I am uncomfortable using these settings
without understanding why the jitter occurs at the other settings.  I
don't want this to be a problem later.  The jitter direction indicates
that the PWM pulse is erroneously staying on too long, which suggests that
the timer interrupt latency is inconsistent.  Is it possible that there is
some hidden process, which periodically out prioritizes the timer
interrupt?  I believe I have set the interrupt priorities as high as
possible.  Is there any way to up their priorities even more?

Things I have tried: 

To verify that the jitter isn't caused by the sevo itself, I used the
hardware PWM to drive the servo at frequencies where the software PWM
causes a jitter.  The hardare PWM causes no jitter, ever.

Played with the interrupt priorities.  No change for any of the many
combinations I tried.

With the PWM heartbeat at 50.00 Hz, I launched four comptationally
intensive threads to see if I could cause a jitter by loading up the
processor.  No jitter.

Any help you can offer would be greatly appreciated!  Thanks in advance.

Louis LeGrand
/*
 * Filename: simple_oneservo.c
 * Compiler: arm-elf-gcc
 *
 * Programmers: Louis LeGrand/Keith Ebner
 * Date completed: 8/20/00
 *
 * Purpose: Drives one servo in a way that is expandable to drive 
 * up to eight servos.  It uses PWM2 to create a heartbeat at a 
 * specified freqency.   Each pulse of the heartbeat causes an
 * interupt that drives the servo signal high, and sets a timer
 * for when it should go low again.  Thus, the servo signal's
 * freqency is controled by PWM2, and it's duty cycle is controlled
 * by timer1.  
 *
 * The hardware needs to be set up as follows:
 *  JP3 pin 1 to servo ground
 *  JP3 pin 2 to servo V++
 *  JP3 pin 38 to servo signal
 *  JP3 pin 10 to JP3 pin 14 (jumps PWM2 to INT0)
 */

/* Includes */
#include <stdio.h>                      /* printf ,etc.*/
#include <cyg/kernel/kapi.h>            /* All the kernel specific stuff */
#include <cyg/hal/hal_intr.h>           /* All the interrupt channel names */

/* Programmable Peripheral Interface Registers */
// LH77790A/B Embedded Microcontroller User's Guide (Chap 13) 
// Port A Bus
#define ARM_PPI_PA   ((volatile char *)0xFFFF1C00)
// PPI Control Register
#define ARM_PPI_CTLR ((volatile char *)0xFFFF1C0C)

/* Pulse Width Modulator Registers */
// LH77790A/B Embedded Microcontroller User's Guide (Chap 9)
// PWM Channel 2 Terminal Count Register
#define ARM_PWM2_TC  ((volatile unsigned int *)0xFFFF1040)
// PWM Channel 2 Duty Cycle Register
#define ARM_PWM2_DC  ((volatile unsigned int *)0xFFFF1044)
// PWM Channel 2 Enable Register
#define ARM_PWM2_ENB ((volatile char *)0xFFFF1048)
// PWM Channel 2 Clock Divider Register
#define ARM_PWM2_DIV ((volatile char *)0xFFFF104C)

/* Clock Managment Registers */
// LH77790A/B Embedded Microcontroller User's Guide (Chap 6)
// Peripheral Clock Select Register
#define ARM_PCSR     ((volatile unsigned int *)0xFFFFAC04)
// Counter/Timer1 Clock Control Register
#define ARM_CT1CCR   ((volatile unsigned int *)0xFFFFAC1C)

/* Counter/Timer Registers */
// LH77790A/B Embedded Microcontroller User's Guide (Chap 11)
// Timer1 Count Register
#define ARM_CT_CNTR1 ((volatile char *)0xFFFF1804)
// Control Word Register
#define ARM_CT_CWR   ((volatile char *)0xFFFF180C)

/* I/O Configuration Registers */
// LH77790A/B Embedded Microcontroller User's Guide (Chap 15)
// Input/Output Configuration Register
#define ARM_IOCR    ((volatile unsigned int *)0xFFFFA410)

/* Function Prototypes */
cyg_uint32 int_0_isr(cyg_vector_t vector,cyg_addrword_t data);
void int_0_dsr(cyg_vector_t vector,cyg_uint32 count,cyg_addrword_t data);
cyg_uint32 int_7_isr(cyg_vector_t vector,cyg_addrword_t data);
void int_7_dsr(cyg_vector_t vector,cyg_uint32 count,cyg_addrword_t data);

/* Interrupt Globals */
// a handle to external INT0 ISR
cyg_handle_t int_0_ISR_H;
// and a storage place for whatever it needs
cyg_interrupt intr0;
// a handle to external INT7 ISR
cyg_handle_t int_7_ISR_H;
// and a storage place for whatever it needs
cyg_interrupt intr7;

/* Servo Position Globals */
char current_channel_duty_LSB=0x94; // 0x1194 is for 1.5 ms default
char current_channel_duty_MSB=0x11;




void cyg_user_start(void)
{
   
   int temp_div;

   // Brute force setup of Programmable Peripheral Interface 
   // Purpose:  Set up port A whose  bit 0 is the servo pwm signal output
   //           LH77790A/B Embedded Microcontroller User's Guide (Chap 13)
   //
   // set Overall Mode to "Mode Selection/Data Direction" ( bit[7] = 1 )
   // set Group A  to Simple I/O  "Mode 0" ( bit[5,6] = 0 )  
   // set Group A, Port A      to "Output" ( bit[4] = 0 )
   // set Group A, Port C[7:4] to "Output" ( bit[3] = 0 )
   // set Group B  to Simple I/O  "Mode 0" ( bit[2] = 0 )
   // set Group B, Port B   to    "Output" ( bit[1] = 0 )
   // set Group B, Port C[3:0] to "Output" ( bit[0] = 0 )
   *ARM_PPI_CTLR = 0X80;
   // To start out, drive all bits of port A to 0
   *ARM_PPI_PA = 0x00;

   // Configure the PWM2 heartbeat 
   // Each heartbeat will cause an interupt that will drive 
   // port A, bit[0] to high and set Timer1 to expire at a
   // specified time
   //
   // Make sure PWM2 is disabled (pg 9.12)
   *ARM_PWM2_ENB = 0x00;
   // Enable PWM clock to all PWM's (pg 6.6)
   *ARM_PCSR = *ARM_PCSR|0x80; 
   // tc = CPU_FREQ/(div*PWM_FREQ)-1 so....
   *ARM_PWM2_DIV = 8;
   *ARM_PWM2_TC  = 0xEA5F; // 50 Hz heartbeat 
   *ARM_PWM2_DC  = 0x000A; // A few clicks for a good signal 
   // create the interrupt object for the INT0=PWM2 heartbeat 
   cyg_interrupt_create(CYGNUM_HAL_INTERRUPT_EXT0,0,0,
                      &int_0_isr, &int_0_dsr ,
                      &int_0_ISR_H, &intr0);
   // activate the INT0=PWM2 interrupt handler
   cyg_interrupt_attach(int_0_ISR_H);
   cyg_interrupt_unmask(CYGNUM_HAL_INTERRUPT_EXT0);
  
   // Configure Timer1 which, when it expires, will cause an interupt
   // that will drive Port A, bit[0] low
   //
   // Configure the peripheral clock select register so
   // the counter/timer1 source is provided internally
   // by the CPMU by setting bit[4] = 0 (pg 6.6)
   *ARM_PCSR = *ARM_PCSR&0x1EF;
   // Configure the Counter/Timer1 Clock Control Register such that
   // timer1's clock tick frequency  is XCLK/8, i.e. 33.3 micro seconds
   // for a 24MHz XCLK (p 6.10) 
   *ARM_CT1CCR = 8; 
   // create the interrupt object for timer1
   cyg_interrupt_create(CYGNUM_HAL_INTERRUPT_TIMER1,0,0,
                        &int_7_isr, &int_7_dsr,
                        &int_7_ISR_H, &intr7);
   // activate the interrupt handler
   cyg_interrupt_attach(int_7_ISR_H);
   cyg_interrupt_unmask(CYGNUM_HAL_INTERRUPT_TIMER1);

   // Start the whole thing by firing off PWM2
   *ARM_PWM2_ENB = 1;

}

// INT0 ISR 
// Do this stuff every time there is a heartbeat ......
cyg_uint32 int_0_isr(cyg_vector_t vector,cyg_addrword_t data)
{
  // Set up timer1 but don't gate it, yet
  //
  // Set Control Word Register to:
  //   Binary Counter, 16-bit ( bit[0]=0 )
  //   Counter Mode 2: Rate generator ( bit[3:1] = 010 )
  //   Read/Write LEAST Significant Bit Only ( bit[5:4] = 01 )
  //   Select Counter 1 ( bit[7:6] = 01 )
  *ARM_CT_CWR = 0x54;
  // Write LSB to timer1
  *ARM_CT_CNTR1 = current_channel_duty_LSB; 
  // Set Control Word Register to:
  //   Binary Counter, 16-bit ( bit[0]=0 )
  //   Counter Mode 2: Rate generator ( bit[3:1] = 010 )
  //   Read/Write MOST Significant Bit Only ( bit[5:4] = 10 )
  //   Select Counter 1 ( bit[7:6] = 01 )
  *ARM_CT_CWR = 0x64;
  // Write MSB to timer1
  *ARM_CT_CNTR1 = current_channel_duty_MSB; 

  // Drive PortA, bit[0] high 
  *ARM_PPI_PA=0x01;

  // Set Counter/Timer1 Gate Source to Logic 1
  // bit[12:11] = 11 (pg 15.2) 
  *ARM_IOCR=*ARM_IOCR|0x1800;

  // Acknowledge the interrupt
  cyg_interrupt_acknowledge(vector);
  // inform eCos that ISR doesn't need DSR to be called
  return CYG_ISR_HANDLED;
}

// INT0 DSR
void int_0_dsr(cyg_vector_t vector,cyg_uint32 count,cyg_addrword_t data)
{
  // This is a place holder in case we need it later.
  // Right now, the ISR handles everything, since we're time
  // critical.  Maybe we could put prep for next channel stuff
  // in here.  But right now, we're only one channel.
  return;
}

// INT7 ISR 
// Do this every time timer1 expires
cyg_uint32 int_7_isr(cyg_vector_t vector,cyg_addrword_t data)
{
  // Drive PortA, bit[0] low 
  *ARM_PPI_PA=0x00;
 
  // Set Counter/Timer1 Gate Source to Logic 0
  // i.e. Turn off the gate to the duty timer to prevent another
  // firing of this interrupt
  // bit[12:11] = 10 (pg 15.2) 
  *ARM_IOCR=*ARM_IOCR&0xE7FF;
  *ARM_IOCR=*ARM_IOCR|0x1000;
 
  // Acknowledge the interrupt
  cyg_interrupt_acknowledge(vector);
  // inform eCos that ISR doesn't need DSR to be called
  return CYG_ISR_HANDLED;
}

// INT7 DSR
void int_7_dsr(cyg_vector_t vector,cyg_uint32 count,cyg_addrword_t data)
{
  // This is a place holder in case we need it later.
  // Right now, the ISR handles everything, since we're time
  // critical.  Maybe we could put prep for next channel stuff
  // in here.  But right now, we're only one channel.
  return;
}