用于自动制动系统的激光雷达传感器的速度测量
Speed measurement from lidar sensor for automatic braking system
为了一个学校项目，我一直在研究一个系统，该系统会在出现危险时自动刹车。只是要求比仅使用 ping 传感器并检测固定距离更严格（安全）。我也将实施传入 object 的速度。
在互联网上我发现了这个很棒的 100Hz 样品激光雷达模块，它非常便宜。它使用带有 2 header 帧 (0x59) 的 UART，然后是一个 dataLowbyte 和一个 dataHighbyte。这里是 datasheet for the module.
所以我开始在 atmega328P (Arduino) 上输入咖啡并输出代码（C 语言）。
如您所见，我已经设置了代码来读取传感器并处理数据。问题是它会返回奇怪的读数。就像我远离时的距离差和靠近时的，即使我说不是。有没有人有使用这个模块的经验？或者我可能误读了数据表的某些内容。
//Includes
//============================================================================================//
#include <avr/io.h>
#include <avr/interrupt.h>
#include <math.h>
//============================================================================================//
//Defines
//============================================================================================//
#define F_CPU 16000000 //CPU frequency
#define Baudrate 115200 //UART Baudrate
#define UBRR_VALUE (((F_CPU / (Baudrate * 8))) - 1) //UART Baudrate set in Arduino
// #define SafetyRating 8
// #define sampleTime 0x65 //Time between 2 samples in 10^-4. This value comes from testing (its 1,01ms)
//============================================================================================//
//Enums
//============================================================================================//
enum {Safe, Warning, Danger}State; //Possible States
//============================================================================================//
//Global variables
//============================================================================================//
uint8_t distanceL; //Distance (low value) data taken from the UART buffer coming from Lidar
uint8_t distanceH; //Distance (high value) data taken from the UART buffer coming from Lidar
uint16_t Distance; //Distance combined
uint8_t frameByteCount = 0; //To count which byte in the frame is current
uint16_t Speed; //Speed variable
//volatile uint8_t DangerLevel; //Danger indicator
//Function prototypes
//============================================================================================//
void calcSpeed(uint16_t Measurement); //Function to calculate the speed out of distance readings
void UART_Transmit(uint8_t data); //Funtion to send data via uart
//============================================================================================//
//Interrupts
//============================================================================================//
/*UART receive complete interrupt*/
ISR(USART_RX_vect) //Data taken from the UART buffer
{
uint8_t sample;
sample = UDR0; //Read a sample from the receiver buffer
if(frameByteCount == 3) //If its the 4th byte of the frame
{
frameByteCount = 0; //Reset the counter
distanceH = sample; //Read the distance data high byte
Distance = (8 << distanceH); //Combine the data low and data high
Distance |= distanceL;
calcSpeed(Distance); //Send the data to laptop for debugging purposes
}
if(frameByteCount == 2) //If its the 3rd byte in the frame read the distance data low byte
{
distanceL = sample;
frameByteCount++; //Increment the counter
}
if (sample == 0x59) //If a sample is a header increment a counter
{
frameByteCount++;
}
else if (frameByteCount != 2 && frameByteCount != 3) //If its not a header or distance data byte reset counter
{
frameByteCount = 0;
}
}
//============================================================================================//
//Timers and Counters
//============================================================================================//
/*Timer0 Counter, this decreases the danger level periodically so a really slow approach will not keep incrementing the danger level*/
ISR(TIMER0_OVF_vect)
{
// if (DangerLevel > 0)
// {
// DangerLevel--;
// }
}
//============================================================================================//
//Main
//============================================================================================//
int main(void)
{
DDRB |= 0x02; //For debugging purposes LED pin PB1 (pin 9) as output
UBRR0H = (UBRR_VALUE >> 8); //Setting the Baudrate register high value
UBRR0L = UBRR_VALUE; //Setting the Baudrate register low value
UCSR0A |= (1<<U2X0); //Setting the data sample to double speed to make Baudrate error less
UCSR0C |= (1<<UCSZ01) | (1<<UCSZ00); //Initializing UART, setting frame to 8 data bits
UCSR0B |= (1<<RXCIE0) | (1<<TXEN0) | (1<<RXEN0); //Setting receiver interrupt enable, transmitter enable and receiver enable... Transmitter for debugging purposes
TCCR0A = 0x00; //Timer0 Setup
TCCR0B |= 0x05; //Timer0 clock prescaler to 1024
TIMSK0 |= 0x01; //Timer0 overflow interrupt enable
sei(); //Set interrupts
State = Safe; //Set state to safe
while (1)
{
//checkState(DangerLevel);
}
}
//============================================================================================//
//Functions
//============================================================================================//
/*Calculate the danger level out of distance readings*/
void calcSpeed(uint16_t Measurement)
{
static uint8_t samplenumber = 0; //Sample tracker
static uint16_t value0 = 0; //First sample
static uint16_t value1 = 0; //Second sample
switch(samplenumber) //To store the measurements alternately
{
case 0:
value0 = Measurement; //Store first measurement
samplenumber = 1; //So next measurement goes to a different variable
break;
case 1:
value1 = Measurement; //Store 2nd measurement
samplenumber = 0; //So next measurement goes to a different variable
break;
default:
break;
}
if (samplenumber == 0 && value1 < value0) //When value0 is first and value1 is second and the object is closing in
{
Speed = value0 - value1;
}
else if (samplenumber == 1 && value0 < value1) //When value1 is first and value0 is second and the object is closing in
{
Speed = value1 - value0;
}
else
{
Speed = 0;
}
UART_Transmit(Speed); //I think sending an uint16_t over uint8_t uart is possible because 'Speed' is never greater than 255 when i'm testing it
}
/*Send data over UART when buffer is empty*/
void UART_Transmit(uint8_t data)
{
/* Wait for empty transmit buffer */
while ( !( UCSR0A & (1<<UDRE0)) )
;
/* Put data into buffer, sends the data */
UDR0 = data;
}
//============================================================================================//
找到了！
Distance = (8 << distanceH); //Combine the data low and data high
Distance |= distanceL;
应该是：
Distance = (distanceH << 8); //Combine the data low and data high
Distance |= distanceL;
为了一个学校项目，我一直在研究一个系统，该系统会在出现危险时自动刹车。只是要求比仅使用 ping 传感器并检测固定距离更严格（安全）。我也将实施传入 object 的速度。
在互联网上我发现了这个很棒的 100Hz 样品激光雷达模块，它非常便宜。它使用带有 2 header 帧 (0x59) 的 UART，然后是一个 dataLowbyte 和一个 dataHighbyte。这里是 datasheet for the module.
所以我开始在 atmega328P (Arduino) 上输入咖啡并输出代码（C 语言）。
如您所见，我已经设置了代码来读取传感器并处理数据。问题是它会返回奇怪的读数。就像我远离时的距离差和靠近时的，即使我说不是。有没有人有使用这个模块的经验？或者我可能误读了数据表的某些内容。
//Includes
//============================================================================================//
#include <avr/io.h>
#include <avr/interrupt.h>
#include <math.h>
//============================================================================================//
//Defines
//============================================================================================//
#define F_CPU 16000000 //CPU frequency
#define Baudrate 115200 //UART Baudrate
#define UBRR_VALUE (((F_CPU / (Baudrate * 8))) - 1) //UART Baudrate set in Arduino
// #define SafetyRating 8
// #define sampleTime 0x65 //Time between 2 samples in 10^-4. This value comes from testing (its 1,01ms)
//============================================================================================//
//Enums
//============================================================================================//
enum {Safe, Warning, Danger}State; //Possible States
//============================================================================================//
//Global variables
//============================================================================================//
uint8_t distanceL; //Distance (low value) data taken from the UART buffer coming from Lidar
uint8_t distanceH; //Distance (high value) data taken from the UART buffer coming from Lidar
uint16_t Distance; //Distance combined
uint8_t frameByteCount = 0; //To count which byte in the frame is current
uint16_t Speed; //Speed variable
//volatile uint8_t DangerLevel; //Danger indicator
//Function prototypes
//============================================================================================//
void calcSpeed(uint16_t Measurement); //Function to calculate the speed out of distance readings
void UART_Transmit(uint8_t data); //Funtion to send data via uart
//============================================================================================//
//Interrupts
//============================================================================================//
/*UART receive complete interrupt*/
ISR(USART_RX_vect) //Data taken from the UART buffer
{
uint8_t sample;
sample = UDR0; //Read a sample from the receiver buffer
if(frameByteCount == 3) //If its the 4th byte of the frame
{
frameByteCount = 0; //Reset the counter
distanceH = sample; //Read the distance data high byte
Distance = (8 << distanceH); //Combine the data low and data high
Distance |= distanceL;
calcSpeed(Distance); //Send the data to laptop for debugging purposes
}
if(frameByteCount == 2) //If its the 3rd byte in the frame read the distance data low byte
{
distanceL = sample;
frameByteCount++; //Increment the counter
}
if (sample == 0x59) //If a sample is a header increment a counter
{
frameByteCount++;
}
else if (frameByteCount != 2 && frameByteCount != 3) //If its not a header or distance data byte reset counter
{
frameByteCount = 0;
}
}
//============================================================================================//
//Timers and Counters
//============================================================================================//
/*Timer0 Counter, this decreases the danger level periodically so a really slow approach will not keep incrementing the danger level*/
ISR(TIMER0_OVF_vect)
{
// if (DangerLevel > 0)
// {
// DangerLevel--;
// }
}
//============================================================================================//
//Main
//============================================================================================//
int main(void)
{
DDRB |= 0x02; //For debugging purposes LED pin PB1 (pin 9) as output
UBRR0H = (UBRR_VALUE >> 8); //Setting the Baudrate register high value
UBRR0L = UBRR_VALUE; //Setting the Baudrate register low value
UCSR0A |= (1<<U2X0); //Setting the data sample to double speed to make Baudrate error less
UCSR0C |= (1<<UCSZ01) | (1<<UCSZ00); //Initializing UART, setting frame to 8 data bits
UCSR0B |= (1<<RXCIE0) | (1<<TXEN0) | (1<<RXEN0); //Setting receiver interrupt enable, transmitter enable and receiver enable... Transmitter for debugging purposes
TCCR0A = 0x00; //Timer0 Setup
TCCR0B |= 0x05; //Timer0 clock prescaler to 1024
TIMSK0 |= 0x01; //Timer0 overflow interrupt enable
sei(); //Set interrupts
State = Safe; //Set state to safe
while (1)
{
//checkState(DangerLevel);
}
}
//============================================================================================//
//Functions
//============================================================================================//
/*Calculate the danger level out of distance readings*/
void calcSpeed(uint16_t Measurement)
{
static uint8_t samplenumber = 0; //Sample tracker
static uint16_t value0 = 0; //First sample
static uint16_t value1 = 0; //Second sample
switch(samplenumber) //To store the measurements alternately
{
case 0:
value0 = Measurement; //Store first measurement
samplenumber = 1; //So next measurement goes to a different variable
break;
case 1:
value1 = Measurement; //Store 2nd measurement
samplenumber = 0; //So next measurement goes to a different variable
break;
default:
break;
}
if (samplenumber == 0 && value1 < value0) //When value0 is first and value1 is second and the object is closing in
{
Speed = value0 - value1;
}
else if (samplenumber == 1 && value0 < value1) //When value1 is first and value0 is second and the object is closing in
{
Speed = value1 - value0;
}
else
{
Speed = 0;
}
UART_Transmit(Speed); //I think sending an uint16_t over uint8_t uart is possible because 'Speed' is never greater than 255 when i'm testing it
}
/*Send data over UART when buffer is empty*/
void UART_Transmit(uint8_t data)
{
/* Wait for empty transmit buffer */
while ( !( UCSR0A & (1<<UDRE0)) )
;
/* Put data into buffer, sends the data */
UDR0 = data;
}
//============================================================================================//
找到了！
Distance = (8 << distanceH); //Combine the data low and data high
Distance |= distanceL;
应该是：
Distance = (distanceH << 8); //Combine the data low and data high
Distance |= distanceL;