FRDM-KL25Z & ARDUINO - Codewarrior Tutorial : Ultrasonic Ranging with the Freescale Freedom Board

This tutorial was extracted from Erich Styger blog http://mcuoneclipse.wordpress.com with his agreement.

 

Freescale.bmp Kinetis-L.jpg

 

 

Question: What makes 8 times ‘beep’, but I cannot hear it?

 

 

Answer: My ultrasonic range finder :-)

 

 

 

frdm-kl25z-with-hc-sr04.png

FRDM-KL25Z with HC-SR04

 

 

 

 

What I have added to my FRDM-KL25Z board is an ultrasonic distance sensor, measuring distances up to 4 meters.

 

 

 

 

HC-SR04 Ultrasonic Sensor

 

 

The HC-SR04 sensor is a 4 pin sensor, at an incredible price point. I have mine from Play-Zone, but I have seen offers in the internet for around $6 too.

 

 

hc-sr04-front-side.png

HC-SR04 Front Side

 

 

 

 

 

The backside features all the electronics to which simply usage of the sensor:

 

 

 

hc-sr04-back-side.png

HC-SR04 Back Side

 

 

 

 

 

With simple I mean, it only needs 4 pins:

  1. Vcc (+5V DC supply)
  2. Trig (TTL input, to trigger a measurement)
  3. Echo (TTL output, pulse proportional to distance)
  4. GND (ground)

:idea: There is a similar module on the market: the Parallax Ping))) or Seedstudio one which only use 3 pins (Vcc, Trig+Echo, GND). While this saves a pin, it makes usage a big more complicated as I would have to switch between output and input on a single pin. So I prefer the 4 pin variant. Using the sensor is simple: hc-sr04-timing-diagram.pngHC-SR04 Timing Diagram

  1. Send a 10 us pulse on Trig
  2. In reaction to this, the sensor will send 8 sonic bursts to measure the distance
  3. After sending the burst, the sensor will raise the Echo signal until it receives the echo back. That means the length of the Echo signal corresponds to time the burst was travelling forward and echoed back.

Below is a snapshot with a logic analyzer: hc-sr04-timing.pngHC-SR04 Timing After the 10 us Trigger signal, the sensor sends the burst during ’1′ and ’2′ (around 456 us), followed by the Echo signal duration of 756 us. The 756 us corresponds to the time it took for the burst to come back. The speed of sound is about 340 m per second (depends on temperature and air relative humidity). A good approximation is 29 us per cm (or 58 us per centimeter as it is for back and forward). With the above example, the 756 us would result in 756/29/2 = 13 cm distance. :idea: For decimal system challenged engineers: instead of a divider of 58, use a divider of 148 to get the result in inch instead of centimeter.

Hardware Connection

I’m using a breadboard for easy wiring and connection. breadboard-wiring1.pngBreadboard Wiring with Freedom Board, Sensor, LCD and Logic analyzer To visualize the measurement, I’m using the 2×16 Character display I have used in my earlier post. Of course a LCD is optional. I used the same wiring as before: signals-with-labels.pngSignals with Labels =========================================================================SIGNAL LIST-------------------------------------------------------------------------SIGNAL-NAME [DIR]        => PIN-NAME [PIN-NUMBER]-------------------------------------------------------------------------DB4_D4 [I/O]             => TSI0_CH5/PTA4/I2C1_SDA/TPM0_CH1/NMI_b [30]DB5_D5 [I/O]             => PTA5/USB_CLKIN/TPM0_CH2 [31]DB6_D6 [I/O]             => CMP0_IN2/PTC8/I2C0_SCL/TPM0_CH4 [65]DB7_D7 [I/O]             => CMP0_IN3/PTC9/I2C0_SDA/TPM0_CH5 [66]E_D10 [Output]           => PTD0/SPI0_PCS0/TPM0_CH0 [73]LED_BLUE [Output]        => ADC0_SE5b/PTD1/SPI0_SCK/TPM0_CH1 [74]LED_GREEN [Output]       => TSI0_CH12/PTB19/TPM2_CH1 [54]LED_RED [Output]         => TSI0_CH11/PTB18/TPM2_CH0 [53]RS_D8 [Output]           => PTA13/TPM1_CH1 [33]RW_D9 [Output]           => ADC0_SE6b/PTD5/SPI1_SCK/UART2_TX/TPM0_CH5 [78]US_Echo_D2 [Input]       => PTD4/LLWU_P14/SPI1_PCS0/UART2_RX/TPM0_CH4 [77]US_Trig_D3 [Output]      => PTA12/TPM1_CH0 [32]========================================================================== What I need is the supply voltage, ground, an output pin (Trig) and an input pin to measure the signal (Echo).The HC-SR04 uses TTL (5V) supply voltage and logic levels. The FRDM-KL25Z processor is a 3.3V one, but the board provides 5V on the header. The HC-SR04 can use 3.3V levels on the Trig signal, but provides a 5V signal on the Echo pin. To get the signal to the 3.3V level, I use a simple voltage divider with a 27k Ohm and 15k Ohm resistor: voltage-divider.pngVoltage Divider with two resistors voltage-divider-detail.pngVoltage Divider Detail

CodeWarrior Project

With the wizard (File > New Bareboard Project) I create a new project for the KL25Z and Processor Expert. I need two things:

  1. Create a 10 us pulse on the Trig (Trigger) pin
  2. Measure the echo on the Echo pin

 

Trigger

For the Trigger pin I add a BitIO_LDD component to my project: bitio_ldd-component1.pngBitIO_LDD Component I configure it as Output pin on PTA12: trigger-properties.pngTrigger Properties Additionally I add the Wait component to the project: wait-component.pngWait Component Generating the 10 us pulse now is trivial:
========================
TRIG_SetVal(trigDevice);WAIT1_Waitus(10);TRIG_ClrVal(trigDevice);======================== Wait! There is some more infrastructure needed, such as this trigDevice handle. More on this below.

Echo Pulse Measurement

To measure the echo, I add a TimerUnit_LDD component: timerunit_ldd-added.pngTimerUnit_LDD added That component shows errors for now, because I need to configure it for my needs. What I want is a timer measuring the echo signal. For this I need to set up a timer which starts counting on the echo raising edge, and stops at the falling edge. For this I need a timer channel. I add a new channel using the ‘+’ icon: adding-timer-unit-channel.pngAdding Timer Unit Channel For this I’m selecting the TPM0_CNT as counter and configure the channel for ‘capture’: :idea: You will understand later on why I have selected this particular counter. using-tpm0_cnt-and-capture-mode1.pngUsing TPM0_CNT and Capture Mode As I have connected the Echo signal on PTD4, I need to use that pin as ‘Capture input pin’: ptd4-as-input-capture-pin.pngPTD4 as input capture pin As hinted by the pin name (PTD4/LLWU_P14/SPI1_PCS0/UART2_RX/TPM0_CH4), this pin is available on TPM0_CH4, so I need to configure this as capture device: selected-capture-device.pngselected capture device :idea:  Now you see why I have selected TPM0_CNT above as Counter device. I admit: that’s not something easy to know about, because this KL25Z has mirades of pin and mapping configuration (called Pin Muxing) :-( . One way is to guess what is behind from the pin names (this is what I try first). Otherwise you need to read through the device reference manual (which is very time-consuming). I wish these devices would be much less complicated and easier to use…. I want to get an interrupt for both the raising edge and falling edge of the Echo signal: I enable interrupts and configure it for both edges. And I give the signal a nice name: interrupt-settings.pngInterrupt Settings That’s not enough: I need to enable interrupts for the counter too: enabled-interrupts-for-counter.pngEnabled Interrupts for Counter

Counter Frequency

I still have errors showing up for my Timer/Counter: I need to configure the counter frequency: This is the clock which is used for my counter. In principle: the higher the frequency, the more precise the measurement will be. As always: a good balance is required. First, I configure the counter frequency: configuring-the-counter-frequency1.pngConfiguring the counter frequency The first thing to do is to disable ‘Auto select timing’: disabling-auto-select-timing.pngDisabling Auto Select timing :idea: Processor Expert usually chooses good default values, but fails (for whatever reason?) to do this properly for timers on the ARM platform. So as solution I always disable auto-select-timing and explicitly make my choice. Maybe better that way anyway :-) I make a gut guess and select a 2.x MHz clock from the list (double-click to assign it): selected-clock-frequency.pngSelected clock frequency

Counter Overrun

There one thing to consider: when will the counter overflow? I keep in mind that with the speed of sound, around 4 meters distance means 400*58us makes 23.5ms Echo signal pulse. So I need to make sure that for this distance, my counter does *not* overflow. I can check this in the ‘Overrun period’ settings: overrun-period.pngOverrun period Deselecting ‘auto select timing’ again will show the overrun period time: overrun-period1.pngOverrun period So my timer will overrun after 25 ms, so I’m fine :-) Next, I need to enable two methods I want to use. I need to read the counter value, and I need to reset it. As such, I enable the two methods (right-click on the method name and select ‘Toggle Enable/Disable’): enabled-timerunit_ldd-methods.pngEnabled TimerUnit_LDD Methods I notice the two events called:

  • OnCounterRestart() is called when there is a restart/overflow of the counter
  • OnChannel() is called for every interrupt of my channel. Remember that I have it configured for creating an interrupt/event for both raising and falling edge.

 

State Machine

I’m using a state machine to go through the different measurement phases: state-diagram.pngState Diagram

  • Idle: Sensor is idle
  • Triggered: we sent the trigger signal to the sensor
  • Measure: we received raising edge of echo signal and are measuring
  • Overflow: Counter overflow happened (see above)
  • Finished: received falling echo signal edge and we have captured the echo signal time

Another way is to show the different states on the timing diagram (without the Overflow case): timing-and-state-diagram.pngTiming and State Diagram With this in mind, the implementation is pretty straight forward. First, an enumeration for the state machine states: 1 typedef enum {2 ECHO_IDLE, /* device not used */3 ECHO_TRIGGERED, /* started trigger pulse */4 ECHO_MEASURE, /* measuring echo pulse */5 ECHO_OVERFLOW, /* measurement took too long */6 ECHO_FINISHED /* measurement finished */7 } US_EchoState; Next the data structure to keep all my data: 1 typedef struct {2 LDD_TDeviceData *trigDevice; /* device handle for the Trigger pin */3 LDD_TDeviceData *echoDevice; /* input capture device handle (echo pin) */4 volatile US_EchoState state; /* state */5 TU1_TValueType capture; /* input capture value */6 } US_DeviceType;7 8 static US_DeviceType usDevice; /* device handle for the ultrasonic device */ And here is how it gets initialized: 1 void US_Init(void) {2 usDevice.state = ECHO_IDLE;3 usDevice.capture = 0;4 usDevice.trigDevice = TRIG_Init(NULL);5 usDevice.echoDevice = TU1_Init(&usDevice);6 }

Event Handlers

Notice the call to TU1_Init() where I pass a pointer to my data structure: That way I get a nice handle passed from the Processor Expert event routines (in events.c) I can use for my own routines.Remember that I get interrupts for raising and falling edge, and for overflow. I handle that in Events.c: 01 /*02 ** ===================================================================03 ** Event : TU1_OnChannel0 (module Events)04 **05 ** Component : TU1 [TimerUnit_LDD]06 ** Description :07 ** Called if compare register match the counter registers or08 ** capture register has a new content. OnChannel0 event and09 ** Timer unit must be enabled. See and10 ** methods. This event is available only if a11 ** is enabled.12 ** Parameters :13 ** NAME - DESCRIPTION14 ** * UserDataPtr - Pointer to the user or15 ** RTOS specific data. The pointer passed as16 ** the parameter of Init method.17 ** Returns : Nothing18 ** ===================================================================19 */20 void TU1_OnChannel0(LDD_TUserData *UserDataPtr)21 {22 US_EventEchoCapture(UserDataPtr);23 }24 25 /*26 ** ===================================================================27 ** Event : TU1_OnCounterRestart (module Events)28 **29 ** Component : TU1 [TimerUnit_LDD]30 ** Description :31 ** Called if counter overflow/underflow or counter is32 ** reinitialized by modulo or compare register matching.33 ** OnCounterRestart event and Timer unit must be enabled. See34 ** and methods. This event is35 ** available only if a is enabled.36 ** Parameters :37 ** NAME - DESCRIPTION38 ** * UserDataPtr - Pointer to the user or39 ** RTOS specific data. The pointer passed as40 ** the parameter of Init method.41 ** Returns : Nothing42 ** ===================================================================43 */44 void TU1_OnCounterRestart(LDD_TUserData *UserDataPtr)45 {46 US_EventEchoOverflow(UserDataPtr);47 } In case of Overflow I simply set the state machine to the overflow state:

1 void US_EventEchoOverflow(LDD_TUserData *UserDataPtr) {2 US_DeviceType *ptr = (US_DeviceType*)UserDataPtr;3 4 ptr->state = ECHO_OVERFLOW;5 }

While in the interrupt/event for raising or falling edge, I reset the counter value at raising edge, or read out the counter value at falling edge:

1 US_DeviceType *ptr = (US_DeviceType*)UserDataPtr;2 3 if (ptr->state==ECHO_TRIGGERED) { /* 1st edge, this is the raising edge, start measurement */4 TU1_ResetCounter(ptr->echoDevice);5 ptr->state = ECHO_MEASURE;6 } else if (ptr->state==ECHO_MEASURE) { /* 2nd edge, this is the falling edge: use measurement */7 (void)TU1_GetCaptureValue(ptr->echoDevice, 0, &ptr->capture);8 ptr->state = ECHO_FINISHED;9 }

With this I reset the counter on the raising edge, and get the counter value at the falling edge. Then I set the state to ‘finished’: this will be the state I wait for (polling) we will see later.

Measure!

I have implemented a function which sends the trigger, and then waits until the measurement has been finished: 01 uint16_t US_Measure_us(void) {02 uint16_t us;03 04 /* send 10us pulse on TRIG line. */05 TRIG_SetVal(usDevice.trigDevice);06 WAIT1_Waitus(10);07 usDevice.state = ECHO_TRIGGERED;08 TRIG_ClrVal(usDevice.trigDevice);09 while(usDevice.state!=ECHO_FINISHED) {10 /* measure echo pulse */11 if (usDevice.state==ECHO_OVERFLOW) { /* measurement took too long? */12 usDevice.state = ECHO_IDLE;13 return 0; /* no echo, error case */14 }15 }16 us = (usDevice.capture*1000UL)/(TU1_CNT_INP_FREQ_U_0/1000);17 return us;18 } It sends the 10 us trigger and then waits for the finished state. In case of overflow (no echo received), I simply return a value of zero. The function returns the measured echo time in microseconds. To deal with different timer frequencies, the macroTU1_CNT_INP_FREQ_U_0 is used which is generated by Processor Expert.

Calculating to Distance

Usually I’m not interested in the echo time, but rather in the distance itself. For this I have created a utility function to transform the echo time (microsecond) into centimeter distance:

 

01 static uint16_t calcAirspeed_dms(uint8_t temperatureCelsius) {02 /* Return the airspeed depending on the temperature, in deci-meter per second */03 unsigned int airspeed; /* decimeters per second */04 05 airspeed = 3313 + (6 * temperatureCelsius); /* dry air, 0% humidity, seehttp://en.wikipedia.org/wiki/Speed_of_sound */06 airspeed -= (airspeed/100)*15; /* factor in ~15% for a relative humidity of ~40% */07 return airspeed;08 }09 10 uint16_t US_usToCentimeters(uint16_t microseconds, uint8_t temperatureCelsius) {11 return (microseconds*100UL)/calcAirspeed_dms(temperatureCelsius)/2; /* 2 because of two way */12 }

:idea: Speed of sound depends on factors like temperature, relative humidity and above sea level (media density). The above function is not counting in everything, but is accurate enough for me.

Example Application

Now using it is pretty simple: from my main routine I intialize my drivers, and then periodically call the function to measure the distance. To make things visual, I show values on a LCD plus show with the RGB LED the distance: 01 static void Measure(void) {02 uint16_t us, cm;03 uint8_t buf[8];04 05 us = US_Measure_us();06 UTIL1_Num16uToStrFormatted(buf, sizeof(buf), us, ' ', 5);07 LCD1_GotoXY(1,5);08 LCD1_WriteString((char*)buf);09 10 cm = US_usToCentimeters(us, 22);11 UTIL1_Num16uToStrFormatted(buf, sizeof(buf), cm, ' ', 5);12 LCD1_GotoXY(2,5);13 LCD1_WriteString((char*)buf);14 15 LEDR_Put(cm<10); /* red LED if object closer than 10 cm */16 LEDB_Put(cm>=10&&cm<=100); /* blue LED if object is in 10..100 cm range */17 LEDG_Put(cm>100); /* blue LED if object is in 10..100 cm range */18 }19 20 /*lint -save -e970 Disable MISRA rule (6.3) checking. */21 int main(void)22 /*lint -restore Enable MISRA rule (6.3) checking. */23 {24 /* Write your local variable definition here */25 26 /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/27 PE_low_level_init();28 /*** End of Processor Expert internal initialization. ***/29 30 /* Write your code here */31 US_Init();32 LCD1_Clear();33 LCD1_WriteLineStr(1, "us: ");34 LCD1_WriteLineStr(2, "cm: ");35 for(;;) {36 Measure();37 WAIT1_Waitms(50); /* wait at least for 50 ms until the next measurement to avoid echos */38 }39 40 /*** Don't write any code pass this line, or it will be deleted during code generation. ***/41 /*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/42 #ifdef PEX_RTOS_START43 PEX_RTOS_START(); /* Startup of the selected RTOS. Macro is defined by the RTOS component. */44 #endif45 /*** End of RTOS startup code. ***/46 /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/47 for(;;){}48 /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/49 } /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/   :idea: If you do not have a LCD, simply strip off the LCD code :-) .  The other thing to consider: inside the main() loop I use a delay of 50 ms: In case of an echo it will take about 25 ms for the echo to arrive: with 50 ms I will be calm for some time until I send another ‘ping’. And here is a short video to see everything in action:

Summary

Once getting through the concept and principles, it is rather easy to use an ultrasonic range sensor like the HC-SR04. Setting up the input capture/counter is the most tricky part. But I think with the concepts and steps outlined above, this should work out pretty good for any other microcontroller, especially if used with CodeWarrior and Processor Expert. The project shown above is available here. If you are looking for improvements, here are some ideas:

  • properly calculate in the humidity and air density.
  • Using a temperature sensor to factor in the current air temperature.
  • Averaging the samples.
  • Dynamic measurement time, adaptive to the distance found to save battery energy.
  • Using an RTOS like FreeRTOS. Especially to reduce the ‘busy waiting’ time and to do something useful during that time.
  • Shell support (e.g. FSShell) to show values on a console, including setting of calibration values.
  • Create a Processor Expert Component for it.
  • As the ultrasonic beam is rather focused: add a pan a tilt servo to it to ‘explore’ more.
  • Adding more sensors and actuators to use it for a robot application :-)

 

 

Happy Ultrasonic-Sounding :-)