Speed measurement from lidar sensor for automatic braking system - c

Question

For a school project I have been working on a system that will automatically brake when there is danger. Only the requirements are more strict (safety) than just using a ping sensor and detecting a fixed distance. I am going to implement the speed of an incoming object as well.
On the internet I found this awesome 100Hz sample Lidar module that's very cheap. It uses UART with 2 header frames (0x59), after that a dataLowbyte and a dataHighbyte. Here is the datasheet for the module.
So I've started inputting coffee and outputting code (in C) on the atmega328P (Arduino).
As you can see here I've set up the code to read the sensor and process the data. The problem is that it gives back weird readings. Like a distance difference when I'm moving further away and when moving closer even though I stated it not to. Does anyone have experience using this module? Or maybe I misread something of the datasheet.
//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;
}
//============================================================================================//

Answer 1

I found it!
Distance = (8 << distanceH); //Combine the data low and data high
Distance |= distanceL;
Should be:
Distance = (distanceH << 8); //Combine the data low and data high
Distance |= distanceL;