Positron8 - LD06 LIDAR interface

[top204]

I have been experimenting with interfacing to an LD06 LIDAR unit, and have come up with a simple to use 'experimental' program that gives good results, and can be developed further for use with real projects or displays on a graphic LCD etc...

The LD06 unit uses a serial interface for its data packet transmission, and a PWM signal for a very silly motor drive mechanism, that probably looked OK to the designer, but certainly not to the user. :-)

Below is the code listing for the LD06 interface experiment:

Code Select Expand

'
'   /\\\\\\\\\
'  /\\\///////\\\
'  \/\\\     \/\\\                                                 /\\\          /\\\
'   \/\\\\\\\\\\\/        /\\\\\     /\\\\\\\\\\     /\\\\\\\\   /\\\\\\\\\\\  /\\\\\\\\\\\  /\\\\\\\\\
'    \/\\\//////\\\      /\\\///\\\  \/\\\//////    /\\\/////\\\ \////\\\////  \////\\\////  \////////\\\
'     \/\\\    \//\\\    /\\\  \//\\\ \/\\\\\\\\\\  /\\\\\\\\\\\     \/\\\         \/\\\        /\\\\\\\\\\
'      \/\\\     \//\\\  \//\\\  /\\\  \////////\\\ \//\\///////      \/\\\ /\\     \/\\\ /\\   /\\\/////\\\
'       \/\\\      \//\\\  \///\\\\\/    /\\\\\\\\\\  \//\\\\\\\\\\    \//\\\\\      \//\\\\\   \//\\\\\\\\/\\
'        \///        \///     \/////     \//////////    \//////////      \/////        \/////     \////////\//
'                                  Let's find out together what makes a PIC Tick!
'
' Interface with an LD06 LIDAR unit.
' For use on a PIC18Fx6K40 device, running at 5 volts, and operating at 64MHz.
'
' Written by Les Johnson for the Positron8 BASIC compiler.
' https://sites.google.com/view/rosetta-tech/home
'
    Device = 18F26K40                                                       ' Tell the compiler what device to compile for
    Declare Xtal = 64                                                       ' Tell the compiler what frequency the device is operating at (in MHz)
    On_Hardware_Interrupt GoTo ISR_Handler                                  ' Point to the interrupt handler
    Declare Float_Display_Type = Fast                                       ' Tell the compiler to use the fast, and more accurate, Floating Point to ASCII converter
    Declare Auto_Heap_Strings = On                                          ' Tell the compiler to always create Strings above the address of standard variables
    Declare Auto_Variable_Bank_Cross = On                                   ' Tell the compiler to makes sure multi-byte variables always reside in the same RAM bank
'
' Setup USART1 to transmit to the outside world
'
    Declare Hserial1_Baud = 115200
    Declare HRSOut1_Pin = PORTC.6
'
' Setup USART2 to receive data from the LD06 unit
'
$define LD06_TX_Pin PORTC.0                                                 ' The pin used for RX from the LD06 unit
    Declare Hserial2_Baud = 230400                                          ' The Baud rate used by the LD06 unit
    Declare HRSIn2_Pin = LD06_TX_Pin                                        ' Connects to the LD06 unit's TX pin
'
' Setup the HPWM pin connected to the LD06 unit
'
$define LD06_PWM_Pin PORTC.2                                                ' The pin used for PWM to the LD06 unit
    Declare HPWM1_Pin = LD06_PWM_Pin

    Include "Int_RX2_18FxxK40.inc"                                          ' Load the USART2 receive buffer routines into the program

$define cLD06_PACKET_SIZE       $2C                                         ' The size of the LD06 unit's serial packet
$define cLD06_HEADER            $54                                         ' The serial packet header value
$define cLD06_POINTS_PER_PACKET 12                                          ' The amount of '3-byte' data items in the distances section
'
' Create global variables here
'
    Dim LD06_bHeader        As Byte                                         ' Holds the 8-bit header value received from the LD06 unit
    Dim LD06_bDataLen       As Byte                                         ' Holds the 8-bit data lenght value received from the LD06 unit
    Dim LD06_wSpeed         As Word                                         ' Holds the 16-bit speed value received from the LD06 unit
    Dim LD06_wStartAngle    As Word                                         ' Holds the 16-bit start angle value received from the LD06 unit
    Dim LD06_wEndAngle      As Word                                         ' Holds the 16-bit end angle value received from the LD06 unit
    Dim LD06_wTimeStamp     As Word                                         ' Holds the 16-bit time stamp value received from the LD06 unit
    Dim LD06_bCRC8          As Byte                                         ' Holds the 8-bit CRC value received from the LD06 unit

    Dim LD06_wDistance0     As Word                                         ' Holds the 16-bit distance 0 value received from the LD06 unit
    Dim LD06_wDistance1     As Word                                         ' Holds the 16-bit distance 1 value received from the LD06 unit
    Dim LD06_wDistance2     As Word                                         ' Holds the 16-bit distance 2 value received from the LD06 unit
    Dim LD06_wDistance3     As Word                                         ' Holds the 16-bit distance 3 value received from the LD06 unit
    Dim LD06_wDistance4     As Word                                         ' Holds the 16-bit distance 4 value received from the LD06 unit
    Dim LD06_wDistance5     As Word                                         ' Holds the 16-bit distance 5 value received from the LD06 unit
    Dim LD06_wDistance6     As Word                                         ' Holds the 16-bit distance 6 value received from the LD06 unit
    Dim LD06_wDistance7     As Word                                         ' Holds the 16-bit distance 7 value received from the LD06 unit
    Dim LD06_wDistance8     As Word                                         ' Holds the 16-bit distance 8 value received from the LD06 unit
    Dim LD06_wDistance9     As Word                                         ' Holds the 16-bit distance 9 value received from the LD06 unit
    Dim LD06_wDistance10    As Word                                         ' Holds the 16-bit distance 10 value received from the LD06 unit
    Dim LD06_wDistance11    As Word                                         ' Holds the 16-bit distance 11 value received from the LD06 unit

    Dim LD06_bConfidence0   As Byte                                         ' Holds the 8-bit confidence 0 value received from the LD06 unit
    Dim LD06_bConfidence1   As Byte                                         ' Holds the 8-bit confidence 1 value received from the LD06 unit
    Dim LD06_bConfidence2   As Byte                                         ' Holds the 8-bit confidence 2 value received from the LD06 unit
    Dim LD06_bConfidence3   As Byte                                         ' Holds the 8-bit confidence 3 value received from the LD06 unit
    Dim LD06_bConfidence4   As Byte                                         ' Holds the 8-bit confidence 4 value received from the LD06 unit
    Dim LD06_bConfidence5   As Byte                                         ' Holds the 8-bit confidence 5 value received from the LD06 unit
    Dim LD06_bConfidence6   As Byte                                         ' Holds the 8-bit confidence 6 value received from the LD06 unit
    Dim LD06_bConfidence7   As Byte                                         ' Holds the 8-bit confidence 7 value received from the LD06 unit
    Dim LD06_bConfidence8   As Byte                                         ' Holds the 8-bit confidence 8 value received from the LD06 unit
    Dim LD06_bConfidence9   As Byte                                         ' Holds the 8-bit confidence 9 value received from the LD06 unit
    Dim LD06_bConfidence10  As Byte                                         ' Holds the 8-bit confidence 10 value received from the LD06 unit
    Dim LD06_bConfidence11  As Byte                                         ' Holds the 8-bit confidence 11 value received from the LD06 unit

    Dim LD06_wAngle         As Word                                         ' Holds the main angle value of the LD06_wStartAngle in the packet scan
    Dim LD06_wAngleDistArray[360] As Word Heap                              ' Holds the distance values per angle (0 to 259)
    Dim wDistanceElement    As Word                                         ' Holds the value for the LD06_wAngleDistArray
    Dim wDistance           As Word                                         ' Holds a value read from LD06_wAngleDistArray

'---------------------------------------------------------------------------------------------------
' The main program starts here
' Receive scans from an LD06 LIDAR, and transmit the results to a serial terminal
'
Main:
    Setup()                                                                 ' Setup the program and any peripherals
    Do                                                                      ' Create a loop
        If LD06_PacketReceive() = True Then                                 ' Receive a packet
            LD06_wAngle = LD06_wStartAngle / 100                            ' Calculate the main angle of the LD06_wStartAngle value
            LD06_LoadDistElements(LD06_wAngle)                              ' Load the elements of the LD06_wAngleDistArray array with the distances

            If LD06_wAngle >= 359 Then                                      ' Has the maximum angle been measured?
                HRSOut1 1                                                   ' Yes. So clear the serial terminal
                For wDistanceElement = 0 To 359 Step 6                      ' Create a loop to transmit the elements of the distance array
                    wDistance = LD06_wAngleDistArray[wDistanceElement]      ' Load wDistance with a value from the angle/distance array
                    If wDistance <> 0 Then
                        HRSOut1Ln Dec3 wDistanceElement, " deg = ",         ' \
                                  Dec3 wDistance / 10,                      ' | Transmit the angles and distances to a serial terminal
                                  " |  ", Rep "*"\wDistance / 100           ' / Transmit a bar of "*" charactersfor the distance
                    EndIf
                Next
            EndIf
        EndIf
    Loop                                                                    ' Do it forever

'---------------------------------------------------------------------------------------------------
' Receive a packet from the LD06 unit
' Input     : None
' Output    : Returns true if the packet was received
'           : Loads global variables: LD06_wSpeed, LD06_wStartAngle, LD06_wEndAngle, LD06_wTimeStamp, and LD06_bCRC8
'           : Loads Global variables: LD06_wDistance0 to LD06_wDistance11 with the distance values
'           : Loads Global variables: LD06_bConfidence0 to LD06_bConfidence11 with the cofidence values
' Notes     : None
'
Proc LD06_PacketReceive(), Bit
    Result = False                                                                      ' Default to a false result
    If USART2_tByteInBuffer = False Then                                                ' Has any data been received from the LD06?
        ExitProc                                                                        ' No. So exit the procedure
    EndIf
    HSerIn2 1024, TimedOut, [LD06_bHeader, LD06_bDataLen]                               ' Receive the, possible, header and data length bytes
    If LD06_bHeader = cLD06_HEADER Then                                                 ' Was the LD06_bHeader value correct?
        If LD06_bDataLen = cLD06_PACKET_SIZE Then                                       ' Yes. So was the LD06_bDataLen value correct?
            HSerIn2 255, TimedOut, [LD06_wSpeed.Byte0, LD06_wSpeed.Byte1,_                              ' Receive the speed value
                                    LD06_wStartAngle.Byte0, LD06_wStartAngle.Byte1,_                    ' Receive the start angle value
                                    LD06_wDistance0.Byte0, LD06_wDistance0.Byte1, LD06_bConfidence0,_   ' Receive the distance data point 0
                                    LD06_wDistance1.Byte0, LD06_wDistance1.Byte1, LD06_bConfidence1,_   ' Receive the distance data point 1
                                    LD06_wDistance2.Byte0, LD06_wDistance2.Byte1, LD06_bConfidence2,_   ' Receive the distance data point 2
                                    LD06_wDistance3.Byte0, LD06_wDistance3.Byte1, LD06_bConfidence3,_   ' Receive the distance data point 3
                                    LD06_wDistance4.Byte0, LD06_wDistance4.Byte1, LD06_bConfidence4,_   ' Receive the distance data point 4
                                    LD06_wDistance5.Byte0, LD06_wDistance5.Byte1, LD06_bConfidence5,_   ' Receive the distance data point 5
                                    LD06_wDistance6.Byte0, LD06_wDistance6.Byte1, LD06_bConfidence6,_   ' Receive the distance data point 6
                                    LD06_wDistance7.Byte0, LD06_wDistance7.Byte1, LD06_bConfidence7,_   ' Receive the distance data point 7
                                    LD06_wDistance8.Byte0, LD06_wDistance8.Byte1, LD06_bConfidence8,_   ' Receive the distance data point 8
                                    LD06_wDistance9.Byte0, LD06_wDistance9.Byte1, LD06_bConfidence9,_   ' Receive the distance data point 9
                                    LD06_wDistance10.Byte0, LD06_wDistance10.Byte1, LD06_bConfidence10,_ ' Receive the distance data point 10
                                    LD06_wDistance11.Byte0, LD06_wDistance11.Byte1, LD06_bConfidence11,_ ' Receive the distance data point 11
                                    LD06_wEndAngle.Byte0, LD06_wEndAngle.Byte1,_                        ' Receive the end angle value
                                    LD06_wTimeStamp.Byte0,LD06_wTimeStamp.Byte1,_                       ' Receive the time stamp value
                                    LD06_bCRC8]                                                         ' Receive the CRC value

            USART2_tByteInBuffer = False                                                ' Reset the USART2_tByteInBuffer flag
        Else                                                                            ' Otherwise... The data length value was not correct...
            ExitProc                                                                    ' So... Exit the procedure with a false result
        EndIf
    Else                                                                                ' Otherwise... The header value was not correct...
        ExitProc                                                                        ' So... Exit the procedure with a false result
    EndIf
    Result = True                                                                       ' Return a true result
'
' Jump here if the received data timed out, and the result will be the same as the last result
'
TimedOut:
EndProc

'---------------------------------------------------------------------------------------------------
' Load the array: LD06_wAngleDistArray with the distances measured in a scan
' Input     : pAngle holds the angle of the scan
'           : Variables: LD06_wDistanceX hold the distances measures
' Output    : Elements of the array: LD06_wAngleDistArray hold the distances
' Notes     : None
'
Proc LD06_LoadDistElements(pAngle As LD06_wAngle)
    If pAngle > 359 Then pAngle = 359
    LD06_wAngleDistArray[pAngle]     = LD06_wDistance0
    LD06_wAngleDistArray[pAngle + 1] = LD06_wDistance1
    LD06_wAngleDistArray[pAngle + 2] = LD06_wDistance3
    LD06_wAngleDistArray[pAngle + 3] = LD06_wDistance5
    LD06_wAngleDistArray[pAngle + 4] = LD06_wDistance7
    LD06_wAngleDistArray[pAngle + 5] = LD06_wDistance9
    LD06_wAngleDistArray[pAngle + 6] = LD06_wDistance11
EndProc

'---------------------------------------------------------------------------------------------------
' Calculate the amount steps between the start and end angle of a scan
' Input     : LD06_wStartAngle holds the 16-bit start angle value read from the LD06 unit's packet
'           : LD06_wEndAngle holds the 16-Bit end angle value read from the LD06 unit's packet
' Output    : Returns the amount of steps between each data sample
' Notes     : None
'
Proc LD06_GetStepSize(), Byte
    Dim wSAngle As Word = LD06_wStartAngle / 100                            ' Remove the angle value after a decimal point
    Dim wEAngle As Word = LD06_wEndAngle / 100                              ' Remove the angle value after a decimal point
    Result = wEAngle - wSAngle                                              ' Find the difference between the start and end angle values
    If Result.SByte < 0 Then                                                ' Is the value less than 0?
        Result = (wEAngle + 360) - wSAngle                                  ' Yes. So alter the calculation to make it a positive value
    EndIf
EndProc

'---------------------------------------------------------------------------------------------------
' Setup the program and any peripherals
' Input     : None
' Output    : None
' Notes     : None
'
Proc Setup()
    PinMode LD06_TX_Pin, Input_PullUp
    PinHigh LD06_PWM_Pin
    USART2_Buff_Init()                                                      ' Initialise the USART2 RX buffer interrupt
    HPWM 1, 65, 30000                                                       ' Set the PWM so that there are approx 6 angle steps per scan
    Clear LD06_wAngleDistArray
EndProc

'-------------------------------------------------------------------------------------------
' Interrupt Service Routine handler
' Input     : None
' Output    : USART2_tByteInBuffer is true if a byte has been received
' Notes     : Implements buffered replacement for HRsin2 and HSerin2
'
ISR_Handler:
    Context Save                                                            ' Save any system variables and SFRs used
'
' Service a USART2 RX interrupt
'
    If USART2_RX_Flag() = True Then                                         ' Was it a USART2 byte receive that triggered the interrupt?
        WREG = RC2STA & %00000110                                           ' Yes. So mask out unwanted bits and check for errors
        If STATUSbits_Z = 0 Then                                            ' Are there any bits set in RC2STA?
            WREG = RC2REG                                                   ' \ Yes. So empty the 2 byte USART2 buffer
            WREG = RC2REG                                                   ' /
            RC2STAbits_CREN = 0                                             ' Disable USART2's RX to clear flags
            USART2_RX_Flag() = 0                                            ' Clear the USART2 RX flag
            RC2STAbits_CREN = 1                                             ' Re-Enable USART2's RX
        Else                                                                ' Otherwise... No error so...
            USART2_bRXByte = RC2REG                                         ' Place the byte received into USART2_bRXByte
            Inc USART2_bRxIndexIn                                           ' Move up the buffer
            If USART2_bRxIndexIn >= cUSART2_RxBufferSize Then               ' End of buffer reached?
                USART2_bRxIndexIn = 0                                       ' Yes. So reset USART2_bRxIndexIn
            EndIf
            USART2_wFSR1 = AddressOf(USART2_bRxBuffer)                      ' Point FSR1L\H to the address of USART2_bRxBuffer
            USART2_wFSR1 = USART2_wFSR1 + USART2_bRxIndexIn                 ' Add the buffer position to FSR1L\H
            INDF1 = USART2_bRXByte                                          ' Place the received character into the buffer
            USART2_tByteInBuffer = True                                     ' Indicate that there is a byte in the buffer
        EndIf
    EndIf

    Context Restore                                                         ' Restore variables then exit the interrupt

'---------------------------------------------------------------------------------------------
' Setup the config fuses to use the internal oscillator on a PIC18F26K40 at 64MHz
' OSC pins are general purpose I/O lines
'
Config_Start
    RSTOSC = HFINTOSC_64MHZ                 ' Internal oscillator at 64MHz and CDIV = 1:1
    FEXTOSC = Off                           ' External oscillator not enabled
    CLKOUTEN = Off                          ' CLKOUT function is disabled
    CSWEN = On                              ' Writing to NOSC and NDIV is allowed
    FCMEN = Off                             ' Fail-Safe Clock Monitor disabled
    MCLRE = EXTMCLR                         ' MCLR pin is MCLR
    PWRTE = On                              ' Power up timer enabled
    LPBOREN = off                           ' LPBOREN disabled
    BOREN = On                              ' Brown-out Reset enabled and controlled by software (BORCONbits_SBOREN is enabled)
    BORV = VBOR_285                         ' Brown-out Reset Voltage set to 2.85V
    ZCD = Off                               ' ZCD disabled. ZCD can be enabled by setting the ZCDSEN bit of ZCDCON
    PPS1WAY = Off                           ' PPSLOCK bit can be set and cleared repeatedly
    STVREN = Off                            ' Stack full/underflow will not cause Reset
    Debug = Off                             ' Background debugger disabled
    XINST = Off                             ' Extended Instruction Set disabled
    SCANE = Off                             ' Scanner module is not available for use. SCANMD bit is ignored
    LVP = Off                               ' Low Voltage programming disabled
    WDTE = Off                              ' Watchdog timer disabled
    WDTCPS = WDTCPS_17                      ' Watchdog Divider ratio 1:4194304 (128 seconds)
    WDTCWS = WDTCWS_7                       ' Watchdog Window always open (100%). Software control. Keyed access not required
    WDTCCS = LFINTOSC                       ' Watchdog timer reference clock is the 31.2kHz HFINTOSC output
    WRT0 = Off                              ' Block 0 (000800-001FFF) not write-protected
    WRT1 = Off                              ' Block 1 (002000-003FFF) not write-protected
    WRTC = Off                              ' Configuration registers (300000-30000B) not write-protected
    WRTB = Off                              ' Boot Block (000000-0007FF) write-protected
    WRTD = Off                              ' Data EEPROM not write-protected
    Cp = Off                                ' User NVM code protection disabled
    CPD = Off                               ' Data NVM code protection disabled
    EBTR0 = Off                             ' Block 0 (000800-001FFF) not protected from table reads executed in other blocks
    EBTR1 = Off                             ' Block 1 (002000-003FFF) not protected from table reads executed in other blocks
    EBTRB = Off                             ' Boot Block (000000-0007FF) not protected from table reads executed in other blocks
Config_End

And below is the code listing for the USART2 buffered serial interface, so that no serial data is missed:

Code Select Expand

$ifndef _BUFFER_USART2_INC_
$define _BUFFER_USART2_INC_
'
'   /\\\\\\\\\
'  /\\\///////\\\
'  \/\\\     \/\\\                                                 /\\\          /\\\
'   \/\\\\\\\\\\\/        /\\\\\     /\\\\\\\\\\     /\\\\\\\\   /\\\\\\\\\\\  /\\\\\\\\\\\  /\\\\\\\\\
'    \/\\\//////\\\      /\\\///\\\  \/\\\//////    /\\\/////\\\ \////\\\////  \////\\\////  \////////\\\
'     \/\\\    \//\\\    /\\\  \//\\\ \/\\\\\\\\\\  /\\\\\\\\\\\     \/\\\         \/\\\        /\\\\\\\\\\
'      \/\\\     \//\\\  \//\\\  /\\\  \////////\\\ \//\\///////      \/\\\ /\\     \/\\\ /\\   /\\\/////\\\
'       \/\\\      \//\\\  \///\\\\\/    /\\\\\\\\\\  \//\\\\\\\\\\    \//\\\\\      \//\\\\\   \//\\\\\\\\/\\
'        \///        \///     \/////     \//////////    \//////////      \/////        \/////     \////////\//
'                                  Let's find out together what makes a PIC Tick!
'
' Interrupt driven buffered USART2 receive.
' Implements replacements for the compiler's HRsin2 and HSerin2 commands.
' Note. The interrupt handler routine must be placed in the main program listing.
'
' The code is for PIC18FxxK40 devices.
' Written for the Positron8 compiler by Les Johnson.
'
$if _device <> _18F27K40 And _device <> _18F47K40 And _device <> _18F26K40 And _device <> _18F46K40
    $error "The TX/RX buffer library is for PIC18F26K40, PIC18F27K40, PIC18F46K40, and PIC18F47K40 devices only"
$else
    #Disable HRSIn2, HRSIn2To, HRSIn2_RCREG_Read                    ' Disable the compiler's library subroutines for HRsin2 and HSerin2
'
' Create some Compiler system variables
'
    Dim GEN  As Byte System                                         ' \
    Dim GENH As Byte System                                         ' / Buffered Hrsin2 Timeout value storage
    Dim PP0  As Byte System                                         ' \
    Dim PP0H As Byte System                                         ' / Storage for FSR0L\H registers
    Dim PP1  As Byte System                                         ' \
    Dim PP1H As Byte System                                         ' / Buffered Hrsin2 Timeout inside loop counter
'
' Create variables for the interrupt buffered serial
'
    Dim USART2_bTimeoutInt     As GEN
    Dim USART2_bTimeoutIntH    As GENH
    Dim USART2_wTimeoutValue   As USART2_bTimeoutInt.Word           ' Alias the timeout value to GEN\H
    Dim USART2_wFSR0Save       As Word Heap                         ' Alias the FSR0L\H storage to PP0\H
    Dim USART2_bInsideLoopInt  As PP1
    Dim USART2_bInsideLoopIntH As PP1H
    Dim USART2_wInsideLoop     As USART2_bInsideLoopInt.Word        ' Alias the inside loop to PP1\H
    Dim USART2_wOutsideLoop    As PRODL.Word                        ' Alias the outside loop to PRODL\H registers
    Dim USART2_bRxIndexIn      As Byte Access                       ' Pointer to the next empty location in the buffer
    Dim USART2_bRxIndexOut     As Byte Access                       ' Pointer to the location of the oldest byte in the buffer
    Dim USART2_bRXByte         As Byte Access                       ' Holds the byte received from USART2
    Dim USART2_tByteInBuffer   As Bit                               ' Holds true if a byte is in the RX buffer
'
' Create the serial buffer in high RAM
'
$define cUSART2_RxBufferSize 128                                    ' The amount of RAM reserved for the receive buffer (Max 255)
    Dim USART2_bRxBuffer[cUSART2_RxBufferSize] As Byte Heap         ' Array for holding received bytes (in high RAM because it is used indirectly, so needs no RAM bank switching mnemonics)
'
' Alias some SFR's as 16-bit variables
'
    Dim USART2_wFSR0 As FSR0L.Word
    Dim USART2_wFSR1 As FSR1L.Word

$ifndef False
    $define False 0
$endif
$ifndef True
    $define True 1
$endif

'------------------------------------------------------------------------------------------------
' USART2 RX SFRs and bits for a PIC18F26K40 device
'
$define USART2_RX_Flag()       PIR3bits_RC2IF                   ' The bit that indicates a byte has been received
$define USART2_RX_ClearFlag()  USART2_RX_Flag() = 0             ' Reset the receive flag
$define USART2_RX_IntFlag()    PIE3bits_RC2IE                   ' The USART2 RX interrupt bit
$define USART2_RX_IntEnable()  USART2_RX_IntFlag() = 1          ' Enable a USART2 RX interrupt
$define USART2_RX_IntDisable() USART2_RX_IntFlag() = 0          ' Disable a USART2 RX interrupt

'------------------------------------------------------------------------------------------------
$define Global_Int_Enable()  INTCONbits_GIE = 1                 ' Enable global interrupts
$define Global_Int_Disable() INTCONbits_GIE = 0                 ' Disable global interrupts
$define Periph_Int_Enable()  INTCONbits_PEIE = 1                ' Enable peripheral interrupts
$define Periph_Int_Disable() INTCONbits_PEIE = 0                ' Disable peripheral interrupts

'-------------------------------------------------------------------------------------------
    GoTo _USART2_Buffer_Main_                                   ' Jump over the replacement subroutines

'-------------------------------------------------------------------------------------------
' Wait for a byte from the interrupt driven circular buffer with timeout
' Input     : GEN\GENH hold the timeout value in approx ms (0 to 65535)
' Output    : PP0 and WREG hold the value received
'           : CARRY Flag (STATUS.0) Clear if timeout out
' Notes     : Uses PRODL\H as temporary variable storage
'           : Uses FSR0L\H as buffer the pointer
'
#ifSym __SYSCOM_HRSIN2_TO_REQ_
__HRsin2_With_TimeOut__:
    USART2_wOutsideLoop = USART2_wTimeoutValue                  ' Save the timeout value so it doesn't get overwritten
    Clear USART2_wInsideLoop                                    ' Reset the inside loop counter
_USART2_OutsideLoop:
    DelayCS 2                                                   ' Delay for 2 cycles within the outside loop
_USART2_InsideLoop:
    DelayCS 1                                                   ' Delay for 1 cycle within the inside loop
    WREG = USART2_bRxIndexIn                                    ' \
    Subwf     USART2_bRxIndexOut,w                              ' / Is there anything in the serial buffer?
    Bnz       USART2_GetByte                                    ' Yes. So fetch it
    WREG = 255                                                  ' \
    Addwf     USART2_wInsideLoop,f                              ' |
    Addwfc    USART2_wInsideLoopH,f                             ' | Decrement the inside and outside loops
    Addwfc    USART2_wOutsideLoop,f                             ' |
    Addwfc    USART2_wOutsideLoopH,f                            ' /
    Btfss     STATUSbits_C
    Ret                                                         ' Return with the Carry flag clear to indicate timed out
    Infsnz    USART2_wInsideLoop,w
    Incfsz    USART2_wInsideLoopH,w
    GoTo _USART2_OutsideLoop
    USART2_wInsideLoop = ((59 * Xtal) / 4)                      ' Set the inside loop counter based upon the Xtal frequency
    GoTo _USART2_InsideLoop
#endIfSym   ' __SYSCOM_HRSIN2_TO_REQ_

'-------------------------------------------------------------------------------------------
' Wait for a byte from the interrupt driven circular buffer without timeout
' Input     : None
' Output    : WREG holds the value received
' Notes     : Uses FSR0L\H as buffer pointers
'
#ifSym __SYSCOM_HRSIN2_REQ_
__HRsin2__:
    While USART2_bRxIndexIn = USART2_bRxIndexOut                ' \ Wait for a byte to appear in the serial buffer
    Wend                                                        ' /
#endIfSym   ' __SYSCOM_HRSIN2_REQ_
'
' Fall through to USART2_GetByte
'-------------------------------------------------------------------------------------------
' Read a byte from the interrupt driven circular buffer
' Input     : None
' Output    : PP0 and WREG hold the value received
' Notes     : Uses FSR0L\H as buffer pointers
'
#ifSym __SYSCOM_HRSIN2_HELPERS_REQ_
USART2_GetByte:
    Inc USART2_bRxIndexOut                                      ' Increment USART2_bRxIndexOut pointer (0 to 255)
    If USART2_bRxIndexOut >= cUSART2_RxBufferSize Then          ' End of buffer reached?
        USART2_bRxIndexOut = 0                                  ' Yes. So clear USART2_bRxIndexOut.
    EndIf
    USART2_wFSR0Save = USART2_wFSR0                             ' Save FSR0L\H registers
    USART2_wFSR0 = AddressOf(USART2_bRxBuffer)                  ' Point FSR0L\H to USART2_bRxBuffer
    USART2_wFSR0 = USART2_wFSR0 + USART2_bRxIndexOut            ' Add the buffer position to FSR0L\H
    WREG = INDF0                                                ' Read buffer location (USART2_bRxIndexOut) into WREG
    PP0 = WREG                                                  ' Also place it into PP0
    USART2_wFSR0 = USART2_wFSR0Save                             ' Restore FSR0\H registers
    Set STATUSbits_C                                            ' Set the Carry flag to indicate a byte received
    Return
#endIfSym   ' __SYSCOM_HRSIN2_HELPERS_REQ_

'-------------------------------------------------------------------------------------------
' Clear the Serial Buffers
' Input     : None
' Output    : None
' Notes     : Also resets the index pointers to the serial buffer
'
Proc USART2_ClearSerialBuffers()
    WREG = RC2REG                                               ' \ Empty the USART's 2-byte serial buffer
    WREG = RC2REG                                               ' /
    USART2_RX_Flag() = False                                    ' Reset the byte received flag
    Clear USART2_bRxBuffer                                      ' Clear the serial RX buffer
    USART2_bRxIndexIn = 0                                       ' Reset the buffer internal pointer
    USART2_bRxIndexOut = 0                                      ' Reset the buffer external pointer
    USART2_tByteInBuffer = False                                ' Reset the byte in buffer flag
EndProc

'-------------------------------------------------------------------------------------------
' Initialise the serial buffers and interrupts
' Input     : None
' Output    : None
' Notes     : None
'
Proc USART2_Buff_Init()
    Global_Int_Disable()                                        ' Disable global interrupts (just in case)
    WREG = RC2REG                                               ' \ Empty the USART's 2-byte serial buffer
    WREG = RC2REG                                               ' /
    USART2_RX_Flag() = False                                    ' Reset the byte received flag
    Clear USART2_bRxBuffer                                      ' Clear the serial RX buffer
    USART2_bRxIndexIn = 0                                       ' Reset the buffer internal pointer
    USART2_bRxIndexOut = 0                                      ' Reset the buffer external pointer
    USART2_tByteInBuffer = False                                ' Reset the byte in buffer flag
    USART2_RX_IntEnable()                                       ' Enable an interrupt on USART2 receive
    Periph_Int_Enable()                                         ' Enable peripheral interrupts
    Global_Int_Enable()                                         ' Enable global interrupts
EndProc

'-------------------------------------------------------------------------------------------
_USART2_Buffer_Main_:

$endif  ' _device
$endif  ' _BUFFER_USART2_INC_

Name the above code listing as: "Int_RX2_18FxxK40.inc", and place it in the same folder as the main .bas code.

Below is a screenshot of the LD06 unit sitting on my bench and showing distances (in centimetres) of some of the angles, and bars to illustrate the distances. If I move my hands around the LD06 unit, the distance values change for the angle that my hands are at, and the closer I move my hand to the unit, the smaller the distance value is. The default distances are all the things sitting on and around my desk. Including me. :-)