Newer
Older
XV Lidar Controller v1.4.0
Copyright 2014-2016 James LeRoy getSurreal
https://github.com/getSurreal/XV_Lidar_Controller
http://www.getsurreal.com/products/xv-lidar-controller
Contributions by:
Doug Hilton mailto: six.speed (at) yahoo (dot) com
See README for additional information
The F() macro in the Serial statements tells the compiler to keep your strings in PROGMEM
*/
#include "TimerThree.h" // used for ultrasonic PWM motor control
#include "PID.h"
#include "EEPROM.h"
#include "EEPROMAnything.h"
#include "SerialCommand.h"
const int N_ANGLES = 360; // # of angles (0..359)
const int SHOW_ALL_ANGLES = N_ANGLES; // value means 'display all angle data, 0..359'
int motor_pwm_pin; // pin connected to mosfet for motor speed control
double rpm_setpoint; // desired RPM (uses double to be compatible with PID library)
double pwm_max; // max analog value. probably never needs to change from 1023
double pwm_min; // min analog pulse value to spin the motor
int sample_time; // how often to calculate the PID values
boolean motor_enable; // to spin the laser or not. No data when not spinning
boolean raw_data; // to retransmit the seiral data to the USB port
boolean show_dist; // controlled by ShowDist and HideDist commands
boolean show_rpm; // controlled by ShowRPM and HideRPM commands
boolean show_interval; // true = show time interval, once per revolution, at angle=0
boolean show_errors; // Show CRC, signal strength and invalid data errors
boolean aryAngles[N_ANGLES]; // array of angles to display
}
const byte EEPROM_ID = 0x07; // used to validate EEPROM initialized
unsigned long now;
unsigned long motor_check_timer = millis();
unsigned long motor_check_interval = 200;
unsigned int rpm_err_thresh = 10; // 2 seconds (10 * 200ms) to shutdown motor with improper RPM and high voltage
James LeRoy
committed
unsigned long curMillis;
unsigned long lastMillis = millis();
const unsigned char COMMAND = 0xFA; // Start of new packet
const int INDEX_LO = 0xA0; // lowest index value
const int INDEX_HI = 0xF9; // highest index value
const int N_DATA_QUADS = 4; // there are 4 groups of data elements
const int N_ELEMENTS_PER_QUAD = 4; // viz., 0=distance LSB; 1=distance MSB; 2=sig LSB; 3=sig MSB
getSurreal
committed
// Offsets to bytes within 'Packet'
const int OFFSET_TO_START = 0;
const int OFFSET_TO_INDEX = OFFSET_TO_START + 1;
const int OFFSET_TO_SPEED_LSB = OFFSET_TO_INDEX + 1;
const int OFFSET_TO_SPEED_MSB = OFFSET_TO_SPEED_LSB + 1;
const int OFFSET_TO_4_DATA_READINGS = OFFSET_TO_SPEED_MSB + 1;
const int OFFSET_TO_CRC_L = OFFSET_TO_4_DATA_READINGS + (N_DATA_QUADS * N_ELEMENTS_PER_QUAD);
const int OFFSET_TO_CRC_M = OFFSET_TO_CRC_L + 1;
const int PACKET_LENGTH = OFFSET_TO_CRC_M + 1; // length of a complete packet
getSurreal
committed
// Offsets to the (4) elements of each of the (4) data quads
const int OFFSET_DATA_DISTANCE_LSB = 0;
const int OFFSET_DATA_DISTANCE_MSB = OFFSET_DATA_DISTANCE_LSB + 1;
const int OFFSET_DATA_SIGNAL_LSB = OFFSET_DATA_DISTANCE_MSB + 1;
const int OFFSET_DATA_SIGNAL_MSB = OFFSET_DATA_SIGNAL_LSB + 1;
int Packet[PACKET_LENGTH]; // an input packet
int ixPacket = 0; // index into 'Packet' array
const int VALID_PACKET = 0;
const int INVALID_PACKET = VALID_PACKET + 1;
const byte INVALID_DATA_FLAG = (1 << 7); // Mask for byte 1 of each data quad "Invalid data"
getSurreal
committed
getSurreal
committed
/* REF: https://github.com/Xevel/NXV11/wiki
The bit 7 of byte 1 seems to indicate that the distance could not be calculated.
It's interesting to see that when this bit is set, the second byte is always 80, and the values of the first byte seem to be
only 02, 03, 21, 25, 35 or 50... When it's 21, then the whole block is 21 80 XX XX, but for all the other values it's the
data block is YY 80 00 00 maybe it's a code to say what type of error ? (35 is preponderant, 21 seems to be when the beam is
interrupted by the supports of the cover) .
const byte STRENGTH_WARNING_FLAG = (1 << 6); // Mask for byte 1 of each data quat "Strength Warning"
The bit 6 of byte 1 is a warning when the reported strength is greatly inferior to what is expected at this distance.
This may happen when the material has a low reflectance (black material...), or when the dot does not have the expected
size or shape (porous material, transparent fabric, grid, edge of an object...), or maybe when there are parasitic
reflections (glass... ).
const byte BAD_DATA_MASK = (INVALID_DATA_FLAG | STRENGTH_WARNING_FLAG);
const byte eState_Find_COMMAND = 0; // 1st state: find 0xFA (COMMAND) in input stream
const byte eState_Build_Packet = eState_Find_COMMAND + 1; // 2nd state: build the packet
int eState = eState_Find_COMMAND;
PID rpmPID(&motor_rpm, &pwm_val, &xv_config.rpm_setpoint, xv_config.Kp, xv_config.Ki, xv_config.Kd, DIRECT);
uint8_t inByte = 0; // incoming serial byte
uint8_t motor_rph_low_byte = 0;
uint16_t aryDist[N_DATA_QUADS] = {0, 0, 0, 0}; // thre are (4) distances, one for each data quad
// so the maximum distance is 16383 mm (0x3FFF)
uint16_t aryQuality[N_DATA_QUADS] = {0, 0, 0, 0}; // same with 'quality'
uint16_t motor_rph = 0;
getSurreal
committed
uint16_t startingAngle = 0; // the first scan angle (of group of 4, based on 'index'), in degrees (0..359)
SerialCommand sCmd;
boolean ledState = LOW;
#if defined(__AVR_ATmega2560__) // if arduino mega 2560
const int ledPin = LED_BUILTIN;
#elif defined(__AVR_ATmega32U4__) && defined(CORE_TEENSY) // if Teensy 2.0
const int ledPin = 11;
#elif defined(__AVR_ATmega32U4__) // if Leonardo (no LED for Pro Micro)
const int ledPin = 13;
#elif defined(__MK20DX256__) // if Teensy 3.1
const int ledPin = 13;
#elif defined(__MKL26Z64__) // if Teensy LC
const int ledPin = 13;
#endif
James LeRoy
committed
getSurreal
committed
// initialization (before 'loop')
EEPROM_readAnything(0, xv_config);
if ( xv_config.id != EEPROM_ID) { // verify EEPROM values have been initialized
initEEPROM();
pinMode(xv_config.motor_pwm_pin, OUTPUT);
Serial.begin(115200); // USB serial
#if defined(__AVR_ATmega2560__) // if arduino mega 2560
Serial1.begin(115200); // XV LDS data
#elif defined(__AVR_ATmega32U4__)
Serial1.begin(115200); // XV LDS data
#elif defined(__MK20DX256__) // if Teensy 3.1
Serial1.begin(115200); // XV LDS data
#elif defined(__MKL26Z64__) // if Teensy LC
Serial1.begin(115200); // XV LDS data
#endif
Timer3.initialize(30); // set PWM frequency to 32.768kHz
rpmPID.SetOutputLimits(xv_config.pwm_min, xv_config.pwm_max);
rpmPID.SetSampleTime(xv_config.sample_time);
rpmPID.SetTunings(xv_config.Kp, xv_config.Ki, xv_config.Kd);
rpmPID.SetMode(AUTOMATIC);
James LeRoy
committed
pinMode(ledPin, OUTPUT);
eState = eState_Find_COMMAND;
for (ixPacket = 0; ixPacket < PACKET_LENGTH; ixPacket++) // Initialize
getSurreal
committed
Packet[ixPacket] = 0;
void loop() {
byte aryInvalidDataFlag[N_DATA_QUADS] = {0, 0, 0, 0}; // non-zero = INVALID_DATA_FLAG or STRENGTH_WARNING_FLAG is set
sCmd.readSerial(); // check for incoming serial commands
if (Serial1.available() > 0) { // read byte from LIDAR and relay to USB
inByte = Serial1.read(); // get incoming byte:
if (xv_config.raw_data)
Serial.write(inByte); // relay
getSurreal
committed
// Switch, based on 'eState':
// State 1: We're scanning for 0xFA (COMMAND) in the input stream
// State 2: Build a complete data packet
if (eState == eState_Find_COMMAND) { // flush input until we get COMMAND byte
if (inByte == COMMAND) {
eState++; // switch to 'build a packet' state
Packet[ixPacket++] = inByte; // store 1st byte of data into 'Packet'
getSurreal
committed
}
else { // eState == eState_Build_Packet
Packet[ixPacket++] = inByte; // keep storing input into 'Packet'
if (ixPacket == PACKET_LENGTH) { // we've got all the input bytes, so we're done building this packet
if (eValidatePacket() == VALID_PACKET) { // Check packet CRC
startingAngle = processIndex(); // get the starting angle of this group (of 4), e.g., 0, 4, 8, 12, ...
processSpeed(); // process the speed
Loading full blame...