#include "MPU9250BF.h" //Bolder Flight 9250 Library
#include <TimeLib.h>
#include <SD.h>
#include <SPI.h>
#include <Snooze.h>
// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250BF IMUW(Wire,0x68);
MPU9250BF IMU(Wire1,0x68);
int status;
unsigned int StartTime = 0;
//Configure Snooze class
SnoozeDigital digital;
SnoozeBlock config_teensy36(digital);
// Setup SD Logging parameters
// Chip Select Pin on Teensy 3.6 is Built-in
const int chipSelect = BUILTIN_SDCARD;
char logFileName[11]; //Active logging file
String logFileBuffer; //Buffer to store data before writing to SD card
int SD_MAX_FILE_SIZE = 10000000; // 5MB max file size, increment to next file before surpassing
unsigned int SD_LOG_WRITE_BUFFER_SIZE = 4096; // buffer size to store data before writing to SD card
int LOG_FILE_INDEX_MAX = 9999; //maximum number of log files
void setup() {
Serial.begin(115200);
// enabling wake on motion low power mode with a threshold of 400 mg and
// an accelerometer data rate of 15.63 Hz.
//IMUW.enableWakeOnMotion(400,MPU9250BF::LP_ACCEL_ODR_15_63HZ);
/********************************************************
Set RTC alarm wake up in (hours, minutes, seconds).
********************************************************/
//alarm.setRtcTimer(8, 0, 0);// hour, min, sec
//timer.setTimer(10000);
digital.pinMode(10, INPUT, CHANGE);
// start communication with IMU
status = IMU.begin();
if (status < 0) {
Serial.println("IMU initialization unsuccessful");
Serial.println("Check IMU wiring or try cycling power");
Serial.print("Status: ");
Serial.println(status);
while(1) {}
}
// setting the accelerometer full scale range to +/-8G
IMU.setAccelRange(MPU9250BF::ACCEL_RANGE_16G);
// setting the gyroscope full scale range to +/-500 deg/s
IMU.setGyroRange(MPU9250BF::GYRO_RANGE_2000DPS);
// setting DLPF bandwidth to 20 Hz
IMU.setDlpfBandwidth(MPU9250BF::DLPF_BANDWIDTH_20HZ);
// setting SRD to 19 for a 50 Hz update rate
IMU.setSrd(17);
// set the Time library to use Teensy 3.0's RTC to keep time
setSyncProvider(getTeensy3Time);
if (Serial.available()) {
time_t t = processSyncMessage();
if (t != 0) {
Teensy3Clock.set(t); // set the RTC
setTime(t);
}
}
//Initialize SDCard
//Serial.print("Initializing SD card...");
// see if the card is present and can be initialized:
if (!SD.begin(chipSelect)) {
//Serial.println("Card failed, or not present");
// don't do anything more:
return;
}
else{
//Serial.println("card initialized.");
nextLogFile(logFileName); //initialize new log file after reboot or power cycle
}
}
void loop() {
SensorData(); //check IMU and log data
//delay(5);
//if (hour() >= 22){
//Snooze.hibernate(config_teensy36);//Go to Sleep
//setSyncProvider(getTeensy3Time);
//}
}
void wakeUp(void){
Serial.println("Awake!");
//setSyncProvider(getTeensy3Time);
}
void SensorData(void){
String Data = "";
Data += printDigits(day());
Data += String("-");
Data += printDigits(month());
Data += String("-");
Data += String(year());
Data += String(",");
Data += printDigits(hour());
Data += String(":");
Data += printDigits(minute());
Data += String(":");
Data += printDigits(second());
Data += String(" ");
IMU.readSensor();
if (abs(IMU.getGyroX_rads()) >= 0.5){
StartTime = millis();
//Serial.println(StartTime);
}
else if (millis() > StartTime+10000){
Serial.println("Sleep");
Snooze.hibernate(config_teensy36);//Go to Sleep
setSyncProvider(getTeensy3Time);
Serial.println("Wake!");
}
else{
//Serial.println(StartTime);
}
Data += String(IMU.getAccelX_mss(),6) + String(" ");
Data += String(IMU.getAccelY_mss(),6) + String(" ");
Data += String(IMU.getAccelZ_mss(),6) + String(" ");
Data += String(IMU.getGyroX_rads(),6) + String(" ");
Data += String(IMU.getGyroY_rads(),6) + String(" ");
Data += String(IMU.getGyroZ_rads(),6);
Data += "\r\n"; // Add a new line
// If adding this log line will put us over the buffer length:
if (Data.length() + logFileBuffer.length() >= SD_LOG_WRITE_BUFFER_SIZE){
sdLogString(logFileBuffer); // Log SD buffer
logFileBuffer = ""; // Clear SD log buffer
//blinkLED(); // Blink LED every time a new buffer is logged to SD
}
// Add new line to SD log buffer
logFileBuffer += Data;
Serial.println(Data);
// Serial.println(logFileBuffer.size());
}
time_t getTeensy3Time()
{
return Teensy3Clock.get();
}
/* code to process time sync messages from the serial port for RTC */
#define TIME_HEADER "T" // Header tag for serial time sync message
unsigned long processSyncMessage() {
unsigned long pctime = 0L;
const unsigned long DEFAULT_TIME = 1357041600; // Jan 1 2013
if(Serial.find(TIME_HEADER)) {
pctime = Serial.parseInt();
return pctime;
if( pctime < DEFAULT_TIME) { // check the value is a valid time (greater than Jan 1 2013)
pctime = 0L; // return 0 to indicate that the time is not valid
}
}
return pctime;
}
// Zero Pad the time for minutes and hours < 10
String printDigits(int digits){
String DigitsOut = "";
// utility function for digital clock display: prints preceding colon and leading 0
//DigitsOut += ":";
if(digits<10)
DigitsOut += "0";
DigitsOut += String(digits);
return DigitsOut;
}
// Log a string to the SD card
void sdLogString(String toLog)
{
// Open the current file name:
File logFile = SD.open(logFileName, FILE_WRITE);
// If the file will get too big with this new string, create
// a new one, and open it.
if (logFile.size() > (SD_MAX_FILE_SIZE - toLog.length()))
{
nextLogFile(logFileName);
logFile = SD.open(logFileName, FILE_WRITE);
}
// If the log file opened properly, add the string to it.
if (logFile)
{
logFile.print(toLog);
logFile.close();
}
}
// Find the next available log file. Or return a null string
// if we've reached the maximum file limit.
void nextLogFile(char logFileName[11])
{
//char filename[];
//int logIndex = 0;
for (int i = 0; i < LOG_FILE_INDEX_MAX; i++){
sprintf(logFileName, "log%03d.txt", i);
//Serial.println(logFileName);
// If the file name doesn't exist, return it
if (!SD.exists(logFileName))
{
break;
//return filename;
}
// Otherwise increment the index, and try again
}
}