365 lines
13 KiB
C
365 lines
13 KiB
C
/* 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 <ti/devices/msp/msp.h>
|
||
#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));
|
||
}
|
||
|