Wednesday, July 16, 2014

How to Extract RC radio receiver pulse width data (all channels) with only ONE digital microcontroller pin (Part 1)

So you're running out of gpio ports on your microcontroller and you have an 8 channel receiver you'd like to use for your super cool Quadcopter project. What do you do?


There are several ways to go about this. The radio signal comes in through the antenna, passes through a series of low pass filters and low noise amplifiers, eventually reaching a series of flip flops that basically separate each of the pulses to their respective channels (with the help of a reference xtal oscillator).


PPM signal decomposition by internal Flip Flops

It's possible, and very easy to extract the signal right before it reaches these flip flops (PPM signal) by soldering a wire to the pin that contains the PPM signal, and simply parse the data yourself through code. The other way, which I will explain in more detail doesn't require soldering or opening your precious receiver and it's silly simple by using OR gates. Yes, remember OR gates from your logic design class in college? I sure do ^_^


Just connect the channel outputs of your RC receiver to a simple OR gate circuit (shown below) in order to recreate the PPM signal. Furthermore, it is very easy to cascade multiple OR gate ICs together if your application requires more than the 5 channels shown below.

Circuit diagram with single Quad OR gate IC

The PPM signal line (above) should be connected to any digital pin of your microcontroller, which you can then parse via Pin Change Interrupts in your software. I will explain the software part on a different post since it's rather long.

Tuesday, July 8, 2014

Arduino (atmega328) Interrupt-Based Maxbotix EZ Sonar Sensor

So your girlfriend (or boyfriend) gave you an EZ0 Maxbotix Sonar Module for your birthday. It's laying around collecting dust mites and you just don't know what to do with it. Well, you've come to the right place! But seriously, here is a picture of the damn thing.


In most blogs I've come across, they only explain how to interface to the module through the Analog pin, and that's fine. However, analog reads in the Arduino take a lot of CPU cycles, and it could potentially slow down the rest of your code. There is another pin in the module, however, that allows us to get useful readings from the sensor in a faster way: the PW (Pulse width) pin.


Theory

The PW pin essentially outputs a Pulse-width modulated signal whose pulse width is proportional to the distance to the object/s being detected. By using Pin Change Interrupts, we can measure the deltas in the pulse widths, and then convert these to distances without wasting time sampling analog voltages. The following diagram is a high level representation of the interaction between the sonar sensor and our program's logic.



Circuit Diagram

The following image shows a simple circuit setup with an Arduino UNO. 


I am connecting the PW pin from the ultrasonic module to digital pin 4 (PD4) on the Arduino, but you can use any other pin as long as you setup the appropriate PCINT register values. I highly recommend going over the atmega328 datasheet for more details. It may seem confusing at first, but keep at it, and it will start making sense over time. If it doesn't, drop a comment below and I will do my best to help clarify ^_^ 

These are the registers that we need to modify in order to configure Pin Change Interrupts on PD4. You would put these two lines of code somewhere in your initialization routine, before you start attempting to take any measurements from the sensor module. 

PCICR |= (1 << PCIE2) /* Allow PCINT interrupts on port D */
PCMSK2 |= (1 << PCINT20) /* Allow PCINT on pin PD4 */

I put these definitions somewhere in the code in order to organize things a bit. 

#define MICROSECONDS_PER_CM 373.38f
#define CM_PER_MICROSECONDS (1/MICROSECONDS_PER_CM)

#define SONAR_PIN 4 /* Digital pin 4 (PD4) */
#define SONAR_PIN_MASK (1 << SONAR_PIN)

typedef struct _sonar_state
{
  volatile uint32_t t_start;            /* Start of pulse */
  volatile uint32_t dt;                 /* Puse duration in microseconds */
  volatile boolean pulseStarted;        /* Flag used to ensure correct initial conditions */
  volatile boolean sonarPin_lastState;  /* Keep track of the last logical state of PD4 (1 or 0) */
} SONAR_STATE;

SONAR_STATE sonar_state;

I'm sure you've noticed the volatile modifiers above. If you don't know what it does, don't worry. Essentially, you would declare a variable as volatile, when its value may change unexpectedly, like in an interrupt, for instance. When an interrupt handler is called, the state of all variables used within the interrupt handler are saved to the stack, and when the program returns from the interrupt routine, these variables are then restored to their previous values, ignoring the modifications that may have occurred during the interrupt. You can see how this could be a problem. 

The following code is the essence of the interrupt mechanism. Upon any signal change on PD4, the ISR routine (below) will automagically execute, allowing us to record the signal transition times. Thus, being able to calculate the distance to nearby detected objects. In theory anyways ^_^

ISR( PCINT2_vect )
{
  /* If PD4 is high, then save pulse start time (micros() returns the time in microseconds since program started.     Overflow condition is ignored for the sake of simplicity). Otherwise, save pulse end */
  if( PIND & SONAR_PIN_MASK > 0 )
  {
    sonar_state.t_start = micros();
    sonar_state.pulseStarted = 1;
  }
  else if( sonar_state.pulseStarted )
  {
    sonar_state.dt = micros() - sonar_state.t_start;

    /* Calculate distance in centimeter */
    sonar_state.distance = CM_PER_MICROSECONDS * sonar_state.dt;

    /* Clear flag */
    sonar_state.pulseStarted = 0;
  }
}

Note: if we have several pins within port D that allow Pin Change Interrupts, the above ISR will execute on ANY pin change that has been configured in the PCMSK2 register (not only PD4). This would cause erroneous time deltas to be recorded. A way to fix this is to keep track of the state of PD4 and add a check within the interrupt routine to verify that PD4 indeed changed logical states (log-to-high or high-to-low) before recording signal-change times. Just keep in mind that the code above assumes there's no other port D pin with Pin Change Interrupts enabled, other than digital pin 4 (PD4) for the sake of simplicity.

Thursday, July 3, 2014

Arduino (atmega328) Interrupt-based BMP085 (Barometric Pressure Sensor)

The BMP085 is a neat little sensor to measure atmospheric pressure, and ultimately altitude (which is the reason why I am using the damn thing in the first place).


The problem I found with the BMP085 library was that every time you want to read the temperature or pressure, you have to add a delay in your code and wait for the sensor to sample and return a valid value (the time delay will depend on the resolution you are using). The code below is part of the BMP085 library to read raw pressure from the barometer. Notice how after we request a pressure measurement, we wait for some time before the data can be read from the pressure register.

int32_t BMP085::readRawPressure(void) 
{
  uint32_t raw;

  /* Request pressure measurement from the BMP085 */
  I2C::write8( DEV_ADDR, CONTROL_REG, READPRESSURE_CMD + (oversampling << 6) );
  
  /* Wait until BMP085 finishes sampling pressure for you */
  if ( oversampling == ULTRALOWPOWER ) 
    delay(5);
  else if ( oversampling == STANDARD ) 
    delay(8);
  else if ( oversampling == HIGHRES ) 
    delay(14);
  else 
    delay(26);

  /* Read registers 0xF6, 0xF7, and 0xF8
   * UP = data(0xF6) << 16 | data(0xF7) << 8 | data(0xF8)
   */
  raw = I2C::read16( DEV_ADDR, PRESSURE_REG );
  raw <<= 8; 
  raw |= I2C::read8( DEV_ADDR, PRESSURE_REG + 2 ); 
  raw >>= (8 - oversampling);

  return raw;
}

The added delay would kill the rest of your code for that time. So, I'm sitting here thinking... interrupts anyone?

On the image above (right), the EOC pin (End of Conversion) essentially tells you when a conversion is done and the requested value is ready for you. This means that we can just request a value, continue on with our lives (but keep an eye on the EOC pin), and when that value is ready for you, then go ahead and read (no unnecessary delays). I made some modifications to the original library in order to include an interrupt-polling mechanism without the headache of timers, just good ol' pin change interrupts (PCINT).

The image below shows my configuration for the circuit diagram.



I am using Digital pin 4 (PD4) on my arduino to monitor the EOC state via Pin Change Interrupts. When we submit a measurement request, EOC goes LOW, and when the measurement is ready for us, EOC goes HIGH. We are interested in capturing RISING signal changes on PD4, then simply read from the BMP085 whatever value we had requested. The image below shows a high level representation of the interrupt-polling mechanism.




To allow pin change interrupts on PD4, we have to configure some registers. By looking at the atmega328 datasheet, setting PCIE2 bit in the PCICR register, effectively allows PortD to have pin change interrupts. However, we need to mask which pin within PortD we want to enable PCINT in. Digital Pin 4 is mapped to PCINT20. By setting this bit in the PCMSK2 register, we fully enable pin change interrupts on pin 4 (see below).

PCICR |= (1 << PCIE2 ); /* Allow PortD to have Pin Change Interrupts */
PCMSK2 |= (1 << PCINT20 ); /* Enable Pin Change Interrupts on PD4 */

The following routine is the interrupt handler, which is where we advance to the next stage in the polling process.

ISR( PCINT2_vect )
{
  ... In here we process the EOC event everytime there is a LOW-to-HIGH transition.
}

The following code snippet shows the pressure request we send to the BMP085. We keep track of the request through a status structure. This way, when we receive an EOC event, we can read the appropriate register from the barometer. We also set a busy flag to 1, since we don't want to send another request while we are waiting for a reply. This is valid for all requests.

void BMP085::readRawTempReq( void )
{
  if( !status.busy )
  {
    status.dataState = GET_RAW_TEMP_STATE;
    status.busy = 1;
    I2C::write8( DEV_ADDR, CONTROL_REG, READTEMP_CMD );
  }
}

The following code snippet shows the temperature request we send to the BMP085.

void BMP085::readRawPressureReq( void )
{
  if( !status.busy )
  {
    status.dataState = GET_RAW_PRESSURE_STATE;
    status.busy = 1;
    I2C::write8( DEV_ADDR, CONTROL_REG, READPRESSURE_CMD + (oversampling << 6) );
 }
}

This is the data structure used to keep track of the state of the barometer.
typedef struct _bmp_status
{
  uint8_t dataState;

  boolean busy;
  boolean eocReceived;
  
  int32_t rawPressure;
  int32_t compensatedPressure;
  uint16_t rawTemperature;
} BMP085_STATUS;

When we get the EOC event, we have to process it accordingly. The code below shows the eoc handler.

/* After read request was submitted, End Of Conversion event will trigger */
void BMP085::eocEvent( void )
{
  int32_t val;
  int32_t p;

  switch( status.dataState )
  {
    case GET_RAW_TEMP_STATE:
      status.rawTemperature = I2C::read16( DEV_ADDR, TEMPERATURE_REG );
      break;
    case GET_RAW_PRESSURE_STATE:
      val = I2C::read16( DEV_ADDR, PRESSURE_REG );
      val <<= 8;
      val |= I2C::read8( DEV_ADDR, PRESSURE_REG + 2 );
      val >>= (8 - oversampling);
      status.rawPressure = val;

      /* Calculate compensated pressure */
      status.compensatedPressure = calculateRealPressure( status.rawTemperature, status.rawPressure );
      break;
  }

  /* Have to clear eoc flag */
  status.eocReceived = 0;
  status.busy = 0;
}

Click HERE for the code on Github