Mapping Drone

A project I have been wanting to work on for a while is building a mapping drone, It’s time to get started on it. I bought the lidar for the drone 6 months ago in anticipation of building it. I will be working on that project on and off.

This is a program that I wrote in C++ it initializes the laser and outputs that data to a csv file. The libraries are in ydlidar SDK and had to be built with cmake before it could be used.

#include "main.h"
#include <inttypes.h>
#include <string>
#include "CYdLidar.h"
#include "iostream"
#include <fstream>
#include <vector>     
#include <utility>       
#include <cstdlib>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>


using namespace ydlidar;
using namespace std;

#if defined(_MSC_VER)
#pragma comment(lib, "ydlidar_sdk.lib")
#endif


void print(vector <int> const &a) {
   cout << "The vector elements are : ";

   for(int i=0; i < a.size(); i++)
   cout << a.at(i) << ' ';
}

//This Function Appends The next Set of Coordinates to a csv file.
void write_csv(string filename, float angle, float range, float intensity){    
    ofstream myFile(filename, ios::app);
    
	myFile << angle << "," << range << "," << intensity  << "\n";
    
    myFile.close();
}


void read_csv(string filename, float angle, float range, float intensity) {
	
}

int main(int argc, char *argv[]) {
	// init system signal
	ydlidar::os_init();

	CYdLidar laser;

	map < string, string > ports = ydlidar::lidarPortList();


	string port = "/dev/ydlidar";
	if (!ports.empty()) {
		port = ports.begin()->second;
	}

	/// lidar port
	laser.setlidaropt(LidarPropSerialPort, port.c_str(), port.size());
	/// ignore array
	string ignore_array;
	ignore_array.clear();
	laser.setlidaropt(LidarPropIgnoreArray, ignore_array.c_str(),
		ignore_array.size());

//int propertys
//lidar baudrate
	int optval = 115200;
	laser.setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int));
	/// tof lidar
	optval = TYPE_TRIANGLE;
	laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int));
	/// device type
	optval = YDLIDAR_TYPE_SERIAL;
	laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int));
	/// sample rate
	optval = 9;
	laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int));
	/// abnormal count
	optval = 4;
	laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int));

	//////////////////////bool property/////////////////
  /// fixed angle resolution
	bool b_optvalue = false;
	laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool));
	/// rotate 180
	laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool));
	/// Counterclockwise
	b_optvalue = true;
	laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool));
	b_optvalue = true;
	laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool));
	/// one-way communication
	b_optvalue = true;
	laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool));
	/// intensity
	b_optvalue = false;
	laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
	/// Motor DTR
	b_optvalue = true;
	laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
	/// HeartBeat
	b_optvalue = true;
	laser.setlidaropt(LidarPropSupportHeartBeat, &b_optvalue, sizeof(bool));


	//////////////////////float property/////////////////
	/// unit: °
	float f_optvalue = 180.0f;
	laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float));
	f_optvalue = -180.0f;
	laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float));
	/// unit: m
	f_optvalue = 16.0f;
	laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float));
	f_optvalue = 0.1f;
	laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float));
	/// unit: Hz
	f_optvalue = 10.f;
	laser.setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float));

	// initialize SDK and LiDAR
	bool ret = laser.initialize();
	if (ret) {//success
	  //Start the device scanning routine which runs on a separate thread and enable motor.
		ret = laser.turnOn();
	}
	else {
		fprintf(stderr, "%s\n", laser.DescribeError());
		fflush(stderr);
	}

	// Turn On success and loop  
	while (ret && ydlidar::os_isOk()) {
		LaserScan scan;
		LaserPoint data;

		if (laser.doProcessSimple(scan)) {
			
			if(GetKeyState(VK_SPACE) & 0x8000)
			{    
				cout << fixed << "\n" << "Next Array Of Coordinates!!" "\n";
				unsigned int ranges = scan.points.size();

				// Get necessary scan Data from the Lidar Using YDlidar SDK
				// For every data point for 1 full rotation.
				for(int i = 0; i < scan.points.size(); i++) {
					//The degree of rotation of laser when current range was recieved. 
					//The output is in radians so I convert it to degrees.
					float angleDeg = scan.points[i].angle*(180/(22/7));
					//The distance until that laser hit an object.
					float rangeFeet = scan.points[i].range*3.28084;
					//Return strength of that laser beam. Varies with the destination object composition.
					float intensity = scan.points[i].intensity;
					//current LiDAR point stamp
					uint64_t timestamp = scan.stamp + i * scan.config.time_increment * 1e9;


						

                                                                                                                                
					
// Write the vector to CSV
					write_csv("I:\\Lidar_Projects\\LidarX2_Project\\LidarX2\\data\\coords.csv", angleDeg, rangeFeet, intensity);
					
				} 
			
				
			}	
				
				
		}
		else {
			fprintf(stderr, "Failed to get Lidar Data\n");
			fflush(stderr);
		}
	}
	// Stop the device scanning thread and disable  motor.

	laser.turnOff();
	// Uninitialize the SDK and Disconnect the LiDAR.
	laser.disconnecting();
	return 0;
}

Just a quick 30 second video of the lidar output using ROS.