So I am working on a cheap lidar system that I can mount on my phantom and fly it around and hopefully get some data back. Its for a school project and I have managed to get the code to give me text file with lat, lon, altitude, and distance. great I thought, except once I have it operating on its own nothing happens, only when I start the serial monitor does it create a log. I have just about given up and decided to see if anyone can help... help.
Here is the code...
#include <SD.h>
#include <SPI.h>
#include <SoftwareSerial.h>
#include "TinyGPS++.h"
#include <I2C.h>
#define LIDARLite_ADDRESS 0x62 // Default I2C Address of LIDAR-Lite.
#define RegisterMeasure 0x00 // Register to write to initiate ranging.
#define MeasureValue 0x04 // Value to initiate ranging.
#define RegisterHighLowB 0x8f // Register to get both High and Low bytes in 1 call.
int ledPin = 9;
int programStatus = 0;
File file;
String longtitude;
String latitude;
String dayString;
String monthString;
String altString;
int id = 0;
TinyGPSPlus gps;
SoftwareSerial ss(8, 7);
void setup()
{
pinMode(ledPin, OUTPUT);
digitalWrite(ledPin, HIGH);
ss.begin(9600);
Serial.begin(115200);
programStatus = initializeSD();
delay(3000);
openFileToWrite("log.csv");
writeHeaderToFile();
I2c.begin(); // Opens & joins the irc bus as master
delay(100); // Waits to make sure everything is powered up before sending or receiving data
I2c.timeOut(50); // Sets a timeout to ensure no locking up of sketch if I2C communication fails
}
int lidargetDistance(){
// Write 0x04 to register 0x00
uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses
while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
nackack = I2c.write(LIDARLite_ADDRESS,RegisterMeasure, MeasureValue); // Write 0x04 to 0x00
delay(1); // Wait 1 ms to prevent overpolling
}
byte distanceArray[2]; // array to store distance bytes from read function
// Read 2byte distance from register 0x8f
nackack = 100; // Setup variable to hold ACK/NACK resopnses
while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
nackack = I2c.read(LIDARLite_ADDRESS,RegisterHighLowB, 2, distanceArray); // Read 2 Bytes from LIDAR-Lite Address and store in array
delay(1); // Wait 1 ms to prevent overpolling
}
return (distanceArray[0] << 8) + distanceArray[1]; // Shift high byte [0] 8 to the left and add l
}
void loop()
{
String entry;
String hourString;
String minuteString;
String secondsString;
while (ss.available() > 0)
gps.encode(ss.read());
if (gps.location.isUpdated())
{
programStatus = 2;
blinkQuickly();
latitude = String(gps.location.lat(),5);
longtitude = String(gps.location.lng(),5);
dayString = String(gps.date.day());
monthString = String(gps.date.month());
hourString = String(gps.time.hour());
minuteString = String(gps.time.minute());
altString = String(gps.altitude.meters());
if(gps.time.hour()<10)
{
hourString = "0"+String(gps.time.hour());
}
else
{
hourString = String(gps.time.hour());
}
if(gps.time.second()<10)
{
secondsString = "0"+String(gps.time.second());
}
else
{
secondsString = String(gps.time.second());
}
id++;
int distance = lidargetDistance();
entry = String(id)+","+dayString+"/"+monthString+","+hourString+":"+minuteString+":"+secondsString+","+latitude+","+longtitude+","+altString+","+distance;
writeEntryToFile(entry);
delay(1000);
}
else if(programStatus == 1)
{
digitalWrite(ledPin, HIGH);
delay(300);
digitalWrite(ledPin,LOW);
delay(300);
}
}
int initializeSD()
{
Serial.println("Initializing SD card...");
pinMode(10, OUTPUT);
if (SD.begin(4))
{
Serial.println("SD card is ready to use.");
return 1;
} else
{
Serial.println("SD card initialization failed");
return 0;
}
}
int openFileToWrite(char filename[])
{
file = SD.open(filename, FILE_WRITE);
if (file)
{
return 1;
} else
{
return 0;
}
}
int writeToFile(String text)
{
if (file)
{
file.println(text);
return 1;
} else
{
return 0;
}
}
void closeFile()
{
if (file)
{
file.close();
}
}
void writeEntryToFile(String entry)
{
openFileToWrite("log.txt");
Serial.println(entry);
writeToFile(entry);
closeFile();
}
void blinkQuickly()
{
digitalWrite(ledPin, HIGH);
delay(100);
digitalWrite(ledPin,LOW);
delay(100);
}
void writeHeaderToFile()
{
writeToFile("Id,Date,Time,Latitude,Longitude,Altitude,Distance");
closeFile();
}