自动制动系统激光雷达传感器的速度测量

在一个学校项目中,我一直在研究一个系统,该系统在有危险时会自动刹车。只有要求比使用ping传感器和检测固定距离更严格(安全)。我还将实现传入对象的速度。

我在网上找到了这个很棒的100Hz样品激光雷达模块,非常便宜。它使用具有2个头帧(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;
}
//============================================================================================//