/* LD19.c * Jonathan Valvano * Date: Nov 5, 2025 LiDAR Sensor LD19 distance sensor The LD19 consists mainly of a laser ranging core, a wireless power transmission unit, a wireless communication unit, an angle measuring unit, a motor drive unit and a mechanical housing Range 20mm to 12m Sampling rate 10 Hz Accuracy: 45mm Standard deviation 10mm Resolution: 15mm Angular resolution: 0.8deg (540 measurements per rotation) */ #include #include "../RTOS_Labs_common/LD19.h" #include "../inc/Clock.h" #include "../inc/LaunchPad.h" // There are four possible UART3 Rx pins that could be used // Sensor board uses PB13 (UART3) and PB12=0 (GPIO output) // LiDAR Sensor LD19 on RTOS sensor board // Pin LD19 MSPM0 // 1 Tx PB13 RxD: is UART3 Rx (LD19 to MSPM0) baud=230400 bps // 2 PWM PB12 GPIO write a 0 to run fastest (MSPM0 to LD19) // 3 GND ground // 4 P5V 5V // UART3 is shared between LD19 and TFLuna3 (can have either but not both) uint32_t LostDataLD19; #define LD19_SIZE 64 // usable size is 63 #define LD19_MESSAGESIZE 47 // LD19 packet is 47 bytes, assuming length remains 12 // prototypes for private functions void LD19Fifo_Init(void); uint32_t LD19Fifo_Put(uint8_t data); uint32_t LD19Fifo_Get(uint8_t *datapt); int LD19Index; uint8_t LD19LastByte; uint32_t StartAngle,EndAngle,Speed,Distance[12],Intensity[12],Previous; // 0 for looking for 54 2C // 2-47 filling the TFLunaDataMessage with 47-byte message //int LD19Mode; // 0 for FIFO and 1 for 540 array uint8_t LD19DataMessage[64]; // 47 should be enough //uint16_t Starts[256],Stops[256],Times[256],II=0; // number of angles LD19NUM 540 //uint16_t Distances[540]; uint16_t *Distances; int BadCnt=0; // index error int BadDistance=0; // too low uint32_t BadIntensity; /* Scope Measurements Continuous packets Baud = 230400 bps Packet Length = 47 bytes, 2.034ms long (470bits/230400bps = 2.039ms) Packet period = 2.447ms Packet frequency = 409 packets/sec Packet payload = 12 measurements Measurement frequency =12 measurements/2.447ms = 4904measurements/sec 540 measurements per rotation 4904 measurements/sec / 540 measurements/rotation = 9.08 rotations/sec Overhead: Interrupts every three frames, 130.4us (30bits/230400 bps) 15 interrupts take 3.55us (overhead 2.7%) 1 interrupt takes 13.4us (overhead 10%) average overhead = (15*2.7%+10%)/16 = 3.2% Header：The length is 1 byte, and the value is fixed at 0x54, indicating the beginning of the data packet; VerLen：The length is 1 byte, the upper three bits indicate the packet type, which is currently fixed at 1, and the lower five bits indicate the number of measurement points in a packet, which is currently fixed at 12, so the byte value is fixed at 0x2C Speed：The length is 2 bytes (little endian), the unit is degrees per second, indicating the speed of the lidar, e.g., 2152 degrees per second; Start angle: The length is 2 bytes (little endian), and the unit is 0.01 degrees, indicating the starting angle of the data packet point; e.g., 32427, or 324.27 degrees Data：Indicates measurement data, a measurement data length is 3 bytes, 12 measurements Distance: 2 bytes (little endian) in mm, e.g., 1234 means 1234mm Intensity: 1 byte, the typical value of the signal strength value is around 200 End angle: The length is 2 bytes (little endian), and the unit is 0.01 degrees, indicating the end angle of the data packet point; e.g., 33470, or 334.7 degrees; Timestamp：The length is 2 bytes, the unit is milliseconds, and the maximum is 30000. When it reaches 30000, it will be counted again CRC check：The length is 1 byte, obtained from the verification of all the previous data except itself. */ // UART3 is in power Domain PD1 // for 32MHz bus clock, UART3 clock is 32MHz // for 40MHz bus clock, UART3 clock is MCLK 40MHz // for 80MHz bus clock, UART3 clock is MCLK 80MHz //------------LD19_Init------------ // Initialize the UART3 for 230400 baud rate (assuming 80 MHz clock), // 8 bit word length, no parity bits, one stop bit, FIFOs enabled // Input: function 0 for debug, 1 for real time // Output: none void LD19_Init(uint16_t *d){ // do not reset or activate PortA, already done in LaunchPad_Init // RSTCLR to GPIOA and UART3 peripherals // bits 31-24 unlock key 0xB1 // bit 1 is Clear reset sticky bit // bit 0 is reset gpio port // GPIOA->GPRCM.RSTCTL = (uint32_t)0xB1000003; // called previously UART3->GPRCM.RSTCTL = 0xB1000003; // Enable power to GPIOA and UART3 peripherals // PWREN // bits 31-24 unlock key 0x26 // bit 0 is Enable Power // GPIOA->GPRCM.PWREN = (uint32_t)0x26000001; // called previously UART3->GPRCM.PWREN = 0x26000001; Clock_Delay(24); // time for uart to power up // the following code selects PB13 to use UART3 IOMUX->SECCFG.PINCM[PB13INDEX] = 0x00040082; //bit 18 INENA input enable //bit 7 PC connected //bits 5-0=2 for UART3_Rx // configure PB12 to be GPIO output IOMUX->SECCFG.PINCM[PB12INDEX] = 0x00000081; //bit 7 PC connected //bits 5-0=1 for GPIO GPIOB->DOE31_0 |= (1<<12); // enable output GPIOB->DOUTCLR31_0 = (1<<12); // PWM=0, fastest sampling UART3->CLKSEL = 0x08; // bus clock UART3->CLKDIV = 0x00; // no divide UART3->CTL0 &= ~0x01; // disable UART3 UART3->CTL0 = 0x00020018; // bit 17 FEN=1 enable FIFO // bits 16-15 HSE=00 16x oversampling // bit 14 CTSEN=0 no CTS hardware // bit 13 RTSEN=0 no RTS hardware // bit 12 RTS=0 not RTS // bits 10-8 MODE=000 normal // bits 6-4 TXE=001 enable TxD // bit 3 RXE=1 enable TxD // bit 2 LBE=0 no loop back // bit 0 ENABLE 0 is disable, 1 to enable if(Clock_Freq() == 40000000){ // 40000000/16 = 2,500,000 Hz // Baud = 230400 // 2,500,000/230400 = 10.8506944444 // divider = 10+54/64 = 10.84375 UART3->IBRD = 10; UART3->FBRD = 54; // baud =2,500,000/10.84375 = 230,547.55 }else if (Clock_Freq() == 32000000){ // 32000000/16 = 2,000,000 // Baud = 230400 // 2,000,000/230400 = 8.6805555 // divider = 8+43/64 = 8.671875 UART3->IBRD = 8; UART3->FBRD = 43; // 230,630.63 }else if (Clock_Freq() == 80000000){ // 80000000/16 = 5,000,000 Hz // Baud = 230400 // 5,000,000/230400 = 21.701388 // divider = 21+45/64 = 21.703125 UART3->IBRD = 21; UART3->FBRD = 45; // baud =5,000,000/21.703125 = 230,381.569 }else return; //LD19Mode = mode; Distances = d; LD19Index = 0; // looking for 0x54 0x2C LD19LastByte = 0; LD19DataMessage[0] = 0x54; LD19DataMessage[1] = 0x2C; LD19Fifo_Init(); LostDataLD19 = 0; UART3->LCRH = 0x00000030; // bits 5-4 WLEN=11 8 bits // bit 3 STP2=0 1 stop // bit 2 EPS=0 parity select // bit 1 PEN=0 no parity // bit 0 BRK=0 no break UART3->CPU_INT.IMASK = 0x0401; // bit 11 TXINT no // bit 10 RXINT yes // bit 0 Receive timeout, yes UART3->IFLS = 0x0232; // bits 11-8 RXTOSEL receiver timeout select 2 (0xF highest) // bits 6-4 RXIFLSEL 3 is greater than or equal to 3/4 full (three elements) // bits 2-0 TXIFLSEL 2 is less than or equal to half NVIC->ICPR[0] = 1<<3; // UART3 is IRQ 3 NVIC->ISER[0] = 1<<3; NVIC->IP[0] = (NVIC->IP[0]&(~0xFF000000))|(1<<30); // set priority (bits 31-30) IRQ 3 UART3->CTL0 |= 0x01; // enable UART3 } // pointer to most recent array of 540 measurements //uint16_t *LD19_GetDistances(void){ // return Distances; //} // copy from hardware RX FIFO to software LD19Fifo // stop when hardware RX FIFO is empty // data are lost if software LD19Fifo is full void static copyHardwareToSoftwareLD19(void){ uint8_t letter; if(Distances==0){ // raw data mode while((UART3->STAT&0x04) == 0){// empty RX hardware FIFO letter = UART3->RXDATA; if((LD19Fifo_Put(letter))==0){ LostDataLD19++; } } }else{ while((UART3->STAT&0x04)==0){ // empty RX hardware FIFO letter = UART3->RXDATA; if(LD19Index == 0){ // looking for sequence 0x54,0x2C if((letter == 0x2C)&&(LD19LastByte == 0x54)){ LD19Index = 2; // looking for message } LD19LastByte = letter; }else{ LD19DataMessage[LD19Index] = letter; LD19Index++; if(LD19Index == LD19_MESSAGESIZE){ // GPIOB->DOUTTGL31_0 = BLUE; // PB22 // Speed = LD19DataMessage[2]+(LD19DataMessage[3]<<8); StartAngle = LD19DataMessage[4]+(LD19DataMessage[5]<<8); //EndAngle = LD19DataMessage[42]+(LD19DataMessage[43]<<8); // StartAngle goes from 0 to 35999 // index goes from 0 to 540-1 (539() //uint32_t index = (540*StartAngle)/36000; // 0 to 539 uint32_t findex = (983*StartAngle)>>16; if(findex>=540) { findex = 540-1; BadCnt++; // never happens } for(int i=0;i<12;i++){ uint16_t d = LD19DataMessage[6+3*i]+(LD19DataMessage[7+3*i]<<8); if(d < 10){ BadDistance++; // happens a lot, might be low intensity BadIntensity = LD19DataMessage[8+3*i]; if(findex==0){ d = Distances[539]; if(d > 10){ Distances[findex+i] = d; } }else{ d = Distances[findex+i-1]; if(d > 10){ Distances[findex+i] = Distances[findex+i-1]; } } }else{ Distances[findex+i] = d; } // if(findex > 270){ // GPIOA->DOUTSET31_0 = 1; // GPIOB->DOUTSET31_0 = BLUE; // PB22 // } // if(findex < 200){ // GPIOA->DOUTCLR31_0 = 1; // GPIOB->DOUTCLR31_0 = BLUE; // PB22 // } // Distance[i] = d; // Intensity[i] = LD19DataMessage[8+3*i]; } /* Starts[II] = StartAngle; Stops[II] = EndAngle; Times[II] = LD19DataMessage[44]+(LD19DataMessage[45]<<8); II = (II+1)&0xFF; */ LD19LastByte = 0; // get ready for next message LD19Index = 0; } } } } } //------------LD19_InChar------------ // Wait for new serial port input // Input: none // Output: ASCII code for key typed uint8_t LD19_InChar(void){uint32_t status; uint8_t letter; do{ status = LD19Fifo_Get(&letter); }while(status==0); return(letter); } void UART3_IRQHandler(void){ uint32_t status; status = UART3->CPU_INT.IIDX; // reading clears bit in RIS // GPIOA->DOUTTGL31_0 = 1; GPIOB->DOUTSET31_0 = BLUE; // PB22 if(status == 0x01){ // 0x01 receive timeout copyHardwareToSoftwareLD19(); }else if(status == 0x0B){ // 0x0B receive copyHardwareToSoftwareLD19(); } GPIOB->DOUTCLR31_0 = BLUE; // PB22 } // Declare state variables for FiFo // size, buffer, put and get indexes int32_t static LD19PutI; // Index to put new int32_t static LD19GetI; // Index of oldest uint8_t static LD19Fifo[LD19_SIZE]; // *********** LD19Fifo_Init********** // Initializes a software LD19Fifo of a // fixed size and sets up indexes for // put and get operations void LD19Fifo_Init(void){ LD19PutI = LD19GetI = 0; } // *********** LD19Fifo_Put********** // Adds an element to the LD19FIFO // Input: data is character to be inserted // Output: 1 for success, data properly saved // 0 for failure, LD19Fifo is full uint32_t LD19Fifo_Put(uint8_t data){ if(((LD19PutI+1)&(LD19_SIZE-1)) == LD19GetI){ return 0; } LD19Fifo[LD19PutI] = data; LD19PutI = (LD19PutI+1)&(LD19_SIZE-1); return 1; } // *********** LD19Fifo_Get********** // Gets an element from the LD19FIFO // Input: pointer to empty 8-bit variable // Output: If the LD19FIFO is empty return 0 // If the LD19FIFO has data, remove it, and put in *datapt, return 1 uint32_t LD19Fifo_Get(uint8_t *datapt){ if(LD19GetI == LD19PutI){ return 0; } *datapt = LD19Fifo[LD19GetI]; LD19GetI = (LD19GetI+1)&(LD19_SIZE-1); return 1; } //------------LD19_InStatus------------ // Returns how much data available for reading from LD19 FIFO // Input: none // Output: number of elements in receive FIFO uint32_t LD19_InStatus(void){ return ((LD19PutI - LD19GetI)&(LD19_SIZE-1)); }