<commit_before>/******************************************************************************
 * Fake Frog                                                                  *
 * An Arduino-based project to build a frog-shaped temperature logger.        *
 * Author: David Lougheed. Copyright 2017.                                    *
 ******************************************************************************/

#define VERSION "0.1.0"

// Includes

#include <Arduino.h>
#include <Wire.h>
#include <LiquidCrystal.h>

#include <SD.h>
#include <RTClib.h>

// Compile-Time Settings

#define SERIAL_LOGGING          true    // Log to the serial display for debug.
#define FILE_LOGGING            true    // Log to file on SD card. (recommended)
#define DISPLAY_ENABLED         true    // Show menus and information on an LCD.
#define NUM_SAMPLES             10      // Samples get averaged to reduce noise.
#define SAMPLE_DELAY            10      // Milliseconds between samples.
#define READING_INTERVAL        60      // Seconds between readings.

// Hardware Settings
// - The data logging shield uses A4, A5, and digital pins 10, 11, 12, and 13.

#define NUM_THERMISTORS         4

#define THERMISTOR_1_PIN        0       // Analog pin
#define THERMISTOR_2_PIN        1       // Analog pin
#define THERMISTOR_3_PIN        2       // Analog pin
#define THERMISTOR_4_PIN        3       // Analog pin

const uint8_t thermistor_pins[NUM_THERMISTORS] = {
    THERMISTOR_1_PIN,
    THERMISTOR_2_PIN,
    THERMISTOR_3_PIN,
    THERMISTOR_4_PIN
};

#define THERMISTOR_SERIES_RES   10000
#define THERMISTOR_RES_NOM      10000   // Nominal resistance, R0.
#define THERMISTOR_B_COEFF      3950    // Beta coefficient of the thermistor.
#define THERMISTOR_TEMP_NOM     25      // Nominal temperature of R0.

#define BUTTON_1_PIN            2
#define BUTTON_2_PIN            3
#define SD_CARD_PIN             10
#define RTC_PIN_1               A4      // Analog pin
#define RTC_PIN_2               A5      // Analog pin
#define LCD_PIN_RS              4
#define LCD_PIN_EN              5
#define LCD_PIN_DB4             6
#define LCD_PIN_DB5             7
#define LCD_PIN_DB6             8
#define LCD_PIN_DB7             9

#define LCD_ROWS                2
#define LCD_COLUMNS             16

#define RTC_TYPE                RTC_PCF8523

// Other Compile-Time Constants

#define MAX_LOG_FILES           1000
#define MAX_DATA_FILES          1000

// Globals

bool serial_logging_started = false;

// - Files
File log_file;
File data_file;

// - Hardware Objects
RTC_TYPE rtc;
LiquidCrystal* lcd;

// - Data Point Variables
//   (to save memory, use global data point variables)
DateTime now;
char formatted_timestamp[] = "0000-00-00T00:00:00";
char* data_file_entry_buffer = (char*) malloc(sizeof(char) * 50);
char temperature_string[4][8];
double latest_resistance[4];
double latest_temperature[4];

/*
    DISPLAY MODES (ALL WITH LOGGING)
    0: Idle
    1: Information (RAM free)
    2: RTC Editor
*/
uint8_t display_mode = 0;

uint16_t i, z; // 16-bit iterator
uint8_t timer = 0; // Counts seconds
uint32_t milli_timer = 0; // Counts time taken to do a loop
uint32_t uptime = 0;
uint8_t cursor = 0; // Maximum: 31 (second row, last column)

bool button_1 = false;
bool button_2 = false;

// Utility Methods

// Determine amount of free RAM.
// - Retrieved 2017-05-19 (https://playground.arduino.cc/Code/AvailableMemory)
int freeRAM() {
    extern int __heap_start, *__brkval;
    int v;
    return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}

// Log a generic message.
void log(const char* msg, bool with_newline = true) {
    if (SERIAL_LOGGING) {
        if(!serial_logging_started) {
            Serial.begin(9600);
            Serial.println();
            serial_logging_started = true;
        }

        if (with_newline) {
            Serial.println(msg);
        } else {
            Serial.print(msg);
        }
    }

    if (FILE_LOGGING) {
        if (log_file) {
            if (with_newline) {
                log_file.println(msg);
            } else {
                log_file.print(msg);
            }
        }
    }
}

// Flush various logging buffers.
void log_flush() {
    if (SERIAL_LOGGING) {
        Serial.flush();
    }
    if (FILE_LOGGING) {
        log_file.flush();
    }
}

// Log an error message. Uses standard log method, then hangs forever.
void log_error(const char* msg, bool with_newline = true) {
    log(msg, with_newline);
    log_flush();
    while (true); // Loop forever
}

// Update the LCD to display latest values for the set display mode.
void update_display() {
    if (DISPLAY_ENABLED && lcd) {
        lcd->clear();
        cursor = 0;

        switch (display_mode) {
            case 1:     // Information
                lcd->print("Free RAM: ");
                lcd->print(freeRAM(), 10);
                lcd->noBlink();
                break;
            case 2:     // RTC Editor
                lcd->print("TBD");
                lcd->setCursor(0, 0);
                lcd->blink();
                break;
            case 0:     // Idle
            default:
                lcd->noBlink();
                break;
        }
    }
}

// Switch the display mode, triggering a display update.
void switch_display_mode(uint8_t m) {
    display_mode = m % 3;
    update_display();
}

// Data Methods

// Update the global formatted timestamp string with the contents of 'now'.
void update_formatted_timestamp() {
    sprintf(formatted_timestamp, "%04u-%02u-%02uT%02u:%02u:%02u", now.year(),
        now.month(), now.day(), now.hour(), now.minute(), now.second());
}

double resistance_to_temperature(double resistance) {
    // Formula: T = 1/(1/B * ln(R/R_0) + (1/T0)) - 273.15 (celcius)
    return 1 / ((log(resistance / THERMISTOR_RES_NOM) / THERMISTOR_B_COEFF) + 1
        / (THERMISTOR_TEMP_NOM + 273.15)) - 273.15;
}

void take_reading(uint8_t t) {
    now = rtc.now();

    latest_resistance[t] = 0;

    for (i = 0; i < NUM_SAMPLES; i++) {
        latest_resistance[t] += (double) analogRead(thermistor_pins[t]);
        delay(SAMPLE_DELAY);
    }

    // Formulas: R = sr / (1023 / mean_of_samples - 1)
    //           sr = thermistor series resistance

    latest_resistance[t] = THERMISTOR_SERIES_RES
        / (1023 / (latest_resistance[t] / NUM_SAMPLES) - 1); // Resistance
    latest_temperature[t] = resistance_to_temperature(latest_resistance[t]);

    // TODO: Error calculations
}

void save_reading_to_card() {
    if (data_file) {
        update_formatted_timestamp();
        for (i = 0; i < NUM_THERMISTORS; i++) {
            dtostrf(latest_temperature[i], 5, 2, temperature_string[i]);
        }

        log("Took reading: ", false);
        log(formatted_timestamp, false); log(",", false);
        log(temperature_string[0], false); log(",", false);
        log(temperature_string[1], false); log(",", false);
        log(temperature_string[2], false); log(",", false);
        log(temperature_string[3]);
        log_flush();

        data_file.print(formatted_timestamp); data_file.print(",");
        data_file.print(temperature_string[0]); data_file.print(",");
        data_file.print(temperature_string[1]); data_file.print(",");
        data_file.print(temperature_string[2]); data_file.print(",");
        data_file.println(temperature_string[3]);
        // data_file.println(data_file_entry_buffer);
        data_file.flush();
    }
}

// Main Methods

void setup() {
    // SET UP EXTERNAL ANALOG VOLTAGE REFERENCE
    // Typically from 3.3V Arduino supply. This reduces the voltage noise seen
    // from reading analog values.
    analogReference(EXTERNAL);

    // INITIALIZE SD CARD
    log("Initializing SD card... ", false);
    pinMode(SD_CARD_PIN, OUTPUT);
    if (!SD.begin()) {
        log_error("Failed.");
    }
    log("Done.");

    // SET UP LOG FILE
    if (FILE_LOGGING) {
        log("Creating log file... ", false);
        char log_file_name[] = "log_000.txt";
        for (i = 0; i < MAX_LOG_FILES; i++) {
            // Increment until we can find a log file slot.

            // Need to add 48 to get ASCII number characters.
            log_file_name[4] = i / 100 + 48;
            log_file_name[5] = i / 10 % 10 + 48;
            log_file_name[6] = i % 10 + 48;

            if (!SD.exists(log_file_name)) {
                log_file = SD.open(log_file_name, FILE_WRITE);
                break;
            }
        }
        if (log_file) {
            log("Done.");
        } else {
            log_error("Failed.");
        }
    }

    // SET UP RTC
    log("Initializing RTC...", false);
    Wire.begin();
    if (!rtc.begin()) {
        log_error("Failed.");
    }
    log("Done.");

    // INPUT RTC TIME
    if (SERIAL_LOGGING) {
        uint16_t year;
        uint8_t month, day, hour, minute, second;

        Serial.print("Change clock? (y/n) ");
        while (Serial.available() < 1);
        if (Serial.read() == 'y') {
            Serial.println();
            Serial.print("Enter Year: ");
            while (Serial.available() < 4);
            year += (Serial.read() - 48) * 1000;
            year += (Serial.read() - 48) * 100;
            year += (Serial.read() - 48) * 10;
            year += (Serial.read() - 48);
            Serial.println(year);

            Serial.print("Enter Month: ");
            while (Serial.available() < 2);
            month += (Serial.read() - 48) * 10;
            month += (Serial.read() - 48);
            Serial.println(month);

            Serial.print("Enter Day: ");
            while (Serial.available() < 2);
            day += (Serial.read() - 48) * 10;
            day += (Serial.read() - 48);
            Serial.println(day);

            Serial.print("Enter Hour: ");
            while (Serial.available() < 2);
            hour += (Serial.read() - 48) * 10;
            hour += (Serial.read() - 48);
            Serial.println(hour);

            Serial.print("Enter Minute: ");
            while (Serial.available() < 2);
            minute += (Serial.read() - 48) * 10;
            minute += (Serial.read() - 48);
            Serial.println(minute);

            Serial.print("Enter Second: ");
            while (Serial.available() < 2);
            second += (Serial.read() - 48) * 10;
            second += (Serial.read() - 48);
            Serial.println(second);

            rtc.adjust(DateTime(year, month, day, hour, minute, second));
        }
    }

    // SET UP DATA FILE
    log("Creating data file...", false);
    char data_file_name[] = "dat_000.csv";
    for (i = 0; i < MAX_DATA_FILES; i++) {
        // Increment until we can find a data file slot.

        // Need to add 48 to get ASCII digit characters.
        data_file_name[4] = i / 100 + 48;
        data_file_name[5] = i / 10 % 10 + 48;
        data_file_name[6] = i % 10 + 48;

        if (!SD.exists(data_file_name)) {
            data_file = SD.open(data_file_name, FILE_WRITE);
            break;
        }
    }
    if (data_file) {
        log("Done.");
    } else {
        log_error("Failed.");
    }

    // PRINT DATA FILE CSV HEADERS
    data_file.println("Timestamp,Temp1,Temp2,Temp3,Temp4");
    data_file.flush();

    // SET UP LCD
    if (DISPLAY_ENABLED) {
        lcd = new LiquidCrystal(LCD_PIN_RS, LCD_PIN_EN, LCD_PIN_DB4,
            LCD_PIN_DB5, LCD_PIN_DB6, LCD_PIN_DB7);
        lcd->begin(LCD_COLUMNS, LCD_ROWS);

        update_display();
    }

    // SET UP BUTTONS
    pinMode(BUTTON_1_PIN, INPUT);
    pinMode(BUTTON_2_PIN, INPUT);

    // Finished everything!
    now = rtc.now();
    update_formatted_timestamp();
    log("Data logger started at ", false);
    log(formatted_timestamp, false);
    log(". Software version: ", false);
    log(VERSION);
    log_flush();
}

void loop() {
    // Time the loop to make sure it runs rounded to the nearest second.
    milli_timer = millis();
    if (timer >= READING_INTERVAL) {
        timer = 0;
        for (z = 0; z < NUM_THERMISTORS; z++) { // Loop through all thermistors
            take_reading(z);
        }
        save_reading_to_card();
    }

    button_1 = digitalRead(BUTTON_1_PIN);
    button_2 = digitalRead(BUTTON_2_PIN);

    if (button_1 && button_2) {
        switch_display_mode(++display_mode);
    } else if (button_1) {

    } else if (button_2) {
        cursor = (cursor + 1) % 32;
        lcd->setCursor(cursor % 16, cursor > 15 ? 1 : 0);
    }

    milli_timer = millis() - milli_timer;
    while (milli_timer >= 1000) {
        // Prevent an integer overflow error by making sure milli_timer < 1000
        timer++; // An extra second has occurred - don't let it slip away!
        milli_timer -= 1000;
    }
    timer++;
    uptime++;
    delay(1000 - milli_timer); // (Ideally) 1 second between loops
}
<commit_msg>Fix small text formatting issues<commit_after>/******************************************************************************
 * Fake Frog                                                                  *
 * An Arduino-based project to build a frog-shaped temperature logger.        *
 * Author: David Lougheed. Copyright 2017.                                    *
 ******************************************************************************/

#define VERSION "0.1.0"

// Includes

#include <Arduino.h>
#include <Wire.h>
#include <LiquidCrystal.h>

#include <SD.h>
#include <RTClib.h>

// Compile-Time Settings

#define SERIAL_LOGGING          true    // Log to the serial display for debug.
#define FILE_LOGGING            true    // Log to file on SD card. (recommended)
#define DISPLAY_ENABLED         true    // Show menus and information on an LCD.
#define NUM_SAMPLES             10      // Samples get averaged to reduce noise.
#define SAMPLE_DELAY            10      // Milliseconds between samples.
#define READING_INTERVAL        60      // Seconds between readings.

// Hardware Settings
// - The data logging shield uses A4, A5, and digital pins 10, 11, 12, and 13.

#define NUM_THERMISTORS         4

#define THERMISTOR_1_PIN        0       // Analog pin
#define THERMISTOR_2_PIN        1       // Analog pin
#define THERMISTOR_3_PIN        2       // Analog pin
#define THERMISTOR_4_PIN        3       // Analog pin

const uint8_t thermistor_pins[NUM_THERMISTORS] = {
    THERMISTOR_1_PIN,
    THERMISTOR_2_PIN,
    THERMISTOR_3_PIN,
    THERMISTOR_4_PIN
};

#define THERMISTOR_SERIES_RES   10000
#define THERMISTOR_RES_NOM      10000   // Nominal resistance, R0.
#define THERMISTOR_B_COEFF      3950    // Beta coefficient of the thermistor.
#define THERMISTOR_TEMP_NOM     25      // Nominal temperature of R0.

#define BUTTON_1_PIN            2
#define BUTTON_2_PIN            3
#define SD_CARD_PIN             10
#define RTC_PIN_1               A4      // Analog pin
#define RTC_PIN_2               A5      // Analog pin
#define LCD_PIN_RS              4
#define LCD_PIN_EN              5
#define LCD_PIN_DB4             6
#define LCD_PIN_DB5             7
#define LCD_PIN_DB6             8
#define LCD_PIN_DB7             9

#define LCD_ROWS                2
#define LCD_COLUMNS             16

#define RTC_TYPE                RTC_PCF8523

// Other Compile-Time Constants

#define MAX_LOG_FILES           1000
#define MAX_DATA_FILES          1000

// Globals

bool serial_logging_started = false;

// - Files
File log_file;
File data_file;

// - Hardware Objects
RTC_TYPE rtc;
LiquidCrystal* lcd;

// - Data Point Variables
//   (to save memory, use global data point variables)
DateTime now;
char formatted_timestamp[] = "0000-00-00T00:00:00";
char* data_file_entry_buffer = (char*) malloc(sizeof(char) * 50);
char temperature_string[4][8];
double latest_resistance[4];
double latest_temperature[4];

/*
    DISPLAY MODES (ALL WITH LOGGING)
    0: Idle
    1: Information (RAM free)
    2: RTC Editor
*/
uint8_t display_mode = 0;

uint16_t i, z; // 16-bit iterator
uint8_t timer = 0; // Counts seconds
uint32_t milli_timer = 0; // Counts time taken to do a loop
uint32_t uptime = 0;
uint8_t cursor = 0; // Maximum: 31 (second row, last column)

bool button_1 = false;
bool button_2 = false;

// Utility Methods

// Determine amount of free RAM.
// - Retrieved 2017-05-19 (https://playground.arduino.cc/Code/AvailableMemory)
int freeRAM() {
    extern int __heap_start, *__brkval;
    int v;
    return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}

// Log a generic message.
void log(const char* msg, bool with_newline = true) {
    if (SERIAL_LOGGING) {
        if(!serial_logging_started) {
            Serial.begin(9600);
            Serial.println();
            serial_logging_started = true;
        }

        if (with_newline) {
            Serial.println(msg);
        } else {
            Serial.print(msg);
        }
    }

    if (FILE_LOGGING) {
        if (log_file) {
            if (with_newline) {
                log_file.println(msg);
            } else {
                log_file.print(msg);
            }
        }
    }
}

// Flush various logging buffers.
void log_flush() {
    if (SERIAL_LOGGING) {
        Serial.flush();
    }
    if (FILE_LOGGING) {
        log_file.flush();
    }
}

// Log an error message. Uses standard log method, then hangs forever.
void log_error(const char* msg, bool with_newline = true) {
    log(msg, with_newline);
    log_flush();
    while (true); // Loop forever
}

// Update the LCD to display latest values for the set display mode.
void update_display() {
    if (DISPLAY_ENABLED && lcd) {
        lcd->clear();
        cursor = 0;

        switch (display_mode) {
            case 1:     // Information
                lcd->print("Free RAM: ");
                lcd->print(freeRAM(), 10);
                lcd->noBlink();
                break;
            case 2:     // RTC Editor
                lcd->print("TBD");
                lcd->setCursor(0, 0);
                lcd->blink();
                break;
            case 0:     // Idle
            default:
                lcd->noBlink();
                break;
        }
    }
}

// Switch the display mode, triggering a display update.
void switch_display_mode(uint8_t m) {
    display_mode = m % 3;
    update_display();
}

// Data Methods

// Update the global formatted timestamp string with the contents of 'now'.
void update_formatted_timestamp() {
    sprintf(formatted_timestamp, "%04u-%02u-%02uT%02u:%02u:%02u", now.year(),
        now.month(), now.day(), now.hour(), now.minute(), now.second());
}

double resistance_to_temperature(double resistance) {
    // Formula: T = 1/(1/B * ln(R/R_0) + (1/T0)) - 273.15 (celcius)
    return 1 / ((log(resistance / THERMISTOR_RES_NOM) / THERMISTOR_B_COEFF) + 1
        / (THERMISTOR_TEMP_NOM + 273.15)) - 273.15;
}

void take_reading(uint8_t t) {
    now = rtc.now();

    latest_resistance[t] = 0;

    for (i = 0; i < NUM_SAMPLES; i++) {
        latest_resistance[t] += (double) analogRead(thermistor_pins[t]);
        delay(SAMPLE_DELAY);
    }

    // Formulas: R = sr / (1023 / mean_of_samples - 1)
    //           sr = thermistor series resistance

    latest_resistance[t] = THERMISTOR_SERIES_RES
        / (1023 / (latest_resistance[t] / NUM_SAMPLES) - 1); // Resistance
    latest_temperature[t] = resistance_to_temperature(latest_resistance[t]);

    // TODO: Error calculations
}

void save_reading_to_card() {
    if (data_file) {
        update_formatted_timestamp();
        for (i = 0; i < NUM_THERMISTORS; i++) {
            dtostrf(latest_temperature[i], 5, 2, temperature_string[i]);
        }

        log("Took reading: ", false);
        log(formatted_timestamp, false); log(",", false);
        log(temperature_string[0], false); log(",", false);
        log(temperature_string[1], false); log(",", false);
        log(temperature_string[2], false); log(",", false);
        log(temperature_string[3]);
        log_flush();

        data_file.print(formatted_timestamp); data_file.print(",");
        data_file.print(temperature_string[0]); data_file.print(",");
        data_file.print(temperature_string[1]); data_file.print(",");
        data_file.print(temperature_string[2]); data_file.print(",");
        data_file.println(temperature_string[3]);
        // data_file.println(data_file_entry_buffer);
        data_file.flush();
    }
}

// Main Methods

void setup() {
    // SET UP EXTERNAL ANALOG VOLTAGE REFERENCE
    // Typically from 3.3V Arduino supply. This reduces the voltage noise seen
    // from reading analog values.
    analogReference(EXTERNAL);

    // INITIALIZE SD CARD
    log("Initializing SD card... ", false);
    pinMode(SD_CARD_PIN, OUTPUT);
    if (!SD.begin()) {
        log_error("Failed.");
    }
    log("Done.");

    // SET UP LOG FILE
    if (FILE_LOGGING) {
        log("Creating log file... ", false);
        char log_file_name[] = "log_000.txt";
        for (i = 0; i < MAX_LOG_FILES; i++) {
            // Increment until we can find a log file slot.

            // Need to add 48 to get ASCII number characters.
            log_file_name[4] = i / 100 + 48;
            log_file_name[5] = i / 10 % 10 + 48;
            log_file_name[6] = i % 10 + 48;

            if (!SD.exists(log_file_name)) {
                log_file = SD.open(log_file_name, FILE_WRITE);
                break;
            }
        }
        if (log_file) {
            log("Done.");
        } else {
            log_error("Failed.");
        }
    }

    // SET UP RTC
    log("Initializing RTC... ", false);
    Wire.begin();
    if (!rtc.begin()) {
        log_error("Failed.");
    }
    log("Done.");

    // INPUT RTC TIME
    if (SERIAL_LOGGING) {
        uint16_t year;
        uint8_t month, day, hour, minute, second;

        Serial.print("Change clock? (y/n) ");
        while (Serial.available() < 1);
        Serial.println();
        if (Serial.read() == 'y') {
            Serial.println();
            Serial.print("Enter Year: ");
            while (Serial.available() < 4);
            year += (Serial.read() - 48) * 1000;
            year += (Serial.read() - 48) * 100;
            year += (Serial.read() - 48) * 10;
            year += (Serial.read() - 48);
            Serial.println(year);

            Serial.print("Enter Month: ");
            while (Serial.available() < 2);
            month += (Serial.read() - 48) * 10;
            month += (Serial.read() - 48);
            Serial.println(month);

            Serial.print("Enter Day: ");
            while (Serial.available() < 2);
            day += (Serial.read() - 48) * 10;
            day += (Serial.read() - 48);
            Serial.println(day);

            Serial.print("Enter Hour: ");
            while (Serial.available() < 2);
            hour += (Serial.read() - 48) * 10;
            hour += (Serial.read() - 48);
            Serial.println(hour);

            Serial.print("Enter Minute: ");
            while (Serial.available() < 2);
            minute += (Serial.read() - 48) * 10;
            minute += (Serial.read() - 48);
            Serial.println(minute);

            Serial.print("Enter Second: ");
            while (Serial.available() < 2);
            second += (Serial.read() - 48) * 10;
            second += (Serial.read() - 48);
            Serial.println(second);

            rtc.adjust(DateTime(year, month, day, hour, minute, second));
        }
    }

    // SET UP DATA FILE
    log("Creating data file... ", false);
    char data_file_name[] = "dat_000.csv";
    for (i = 0; i < MAX_DATA_FILES; i++) {
        // Increment until we can find a data file slot.

        // Need to add 48 to get ASCII digit characters.
        data_file_name[4] = i / 100 + 48;
        data_file_name[5] = i / 10 % 10 + 48;
        data_file_name[6] = i % 10 + 48;

        if (!SD.exists(data_file_name)) {
            data_file = SD.open(data_file_name, FILE_WRITE);
            break;
        }
    }
    if (data_file) {
        log("Done.");
    } else {
        log_error("Failed.");
    }

    // PRINT DATA FILE CSV HEADERS
    data_file.println("Timestamp,Temp1,Temp2,Temp3,Temp4");
    data_file.flush();

    // SET UP LCD
    if (DISPLAY_ENABLED) {
        lcd = new LiquidCrystal(LCD_PIN_RS, LCD_PIN_EN, LCD_PIN_DB4,
            LCD_PIN_DB5, LCD_PIN_DB6, LCD_PIN_DB7);
        lcd->begin(LCD_COLUMNS, LCD_ROWS);

        update_display();
    }

    // SET UP BUTTONS
    pinMode(BUTTON_1_PIN, INPUT);
    pinMode(BUTTON_2_PIN, INPUT);

    // Finished everything!
    now = rtc.now();
    update_formatted_timestamp();
    log("Data logger started at ", false);
    log(formatted_timestamp, false);
    log(". Software version: ", false);
    log(VERSION);
    log_flush();
}

void loop() {
    // Time the loop to make sure it runs rounded to the nearest second.
    milli_timer = millis();
    if (timer >= READING_INTERVAL) {
        timer = 0;
        for (z = 0; z < NUM_THERMISTORS; z++) { // Loop through all thermistors
            take_reading(z);
        }
        save_reading_to_card();
    }

    button_1 = digitalRead(BUTTON_1_PIN);
    button_2 = digitalRead(BUTTON_2_PIN);

    if (button_1 && button_2) {
        switch_display_mode(++display_mode);
    } else if (button_1) {

    } else if (button_2) {
        cursor = (cursor + 1) % 32;
        lcd->setCursor(cursor % 16, cursor > 15 ? 1 : 0);
    }

    milli_timer = millis() - milli_timer;
    while (milli_timer >= 1000) {
        // Prevent an integer overflow error by making sure milli_timer < 1000
        timer++; // An extra second has occurred - don't let it slip away!
        milli_timer -= 1000;
    }
    timer++;
    uptime++;
    delay(1000 - milli_timer); // (Ideally) 1 second between loops
}
<|endoftext|>


<commit_before><commit_msg>edited main.cpp<commit_after>#include "../headerse/parser.h"
#include <stdio.h>
#include <unistd.h>
#include <iostream>

using namespace std;

int main()
{
	//get host name and user id
	string cmd;
	string user_id;
	bool prev_cmd = true;
	char hostname[100];

	//FIXME:instantiate object of type parse

	if (getlogin() == NULL)
	{
		user_id = "";
		perror("ERROR: could not log in");
	}

	else
	{
		user_id = getlogin();
	}

	if (gethostname(hostname, 100) == -1)
	{
		perror("ERROR: could not get host name");
	}

	while (1)
	{
		//print out hostname and user id
		if (getlogin() != NULL)
		{
			cout << "[" <<  user_id << "@" << hostname << "]";
		}

		cout << "$ ";
		//get user input
		getline(cin,cmd);
		//FIXME: need a line of code for parsing here
	}

	return 0;
}
<|endoftext|>


<commit_before>/*
  Original code from tekkies/CVdrone (get actual address from github)

  Modified by Elliot Greenlee, Caleb Mennen, Jacob Pollack
  COSC 402 Senior Design

  Min Kao Drone Tour
  See readme at (get actual address from github)
*/

#include "ardrone/ardrone.h"
#include "structures.h"
#include "manual/manual.h"
#include "objectFollowing/objectFollowing.h"
#include "lineFollowing/lineFollowing.h"
#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

using namespace std;

class Control {
  public:
    const string flightLog = "flight_log.txt";

    //AR.Drone class
    ARDrone ardrone;

    cv::Mat image;

    int key;
    FlyingMode flyingMode = Manual;
    double speed = 0.0;
    int batteryPercentage;
    bool flying;

    ControlMovements velocities;

    FILE *flight_log;

    void initializeDroneControl(ObjectFollowing *objectFollowing);
    void detectFlyingMode();
    bool detectEscape();
    void changeSpeed();
    void getImage();

    void overlayControl();
};

/*
*/
void Control::initializeDroneControl(ObjectFollowing *objectFollowing) {
  //Initializing Message
  printf("Connecting to the drone\n");
  printf("If there is no version number response in the next 10 seconds, please restart the drone and code.\n");
  fflush(stdout);

  // Initialize
  if (!ardrone.open()) {
    printf("Failed to initialize.\n");
    //TODO: fix this return -1;
  }

  //Set drone on flat surface and initialize
  ardrone.setFlatTrim();

  //initialize object following code
  objectFollowing->initializeObjectFollowing();
  flight_log = fopen(flightLog.c_str(), "w");

  //Print default command information
  printf("To disconnect, press the ESC key\n\n");
  printf("Currently the drone is in manual mode.\n");
  printf("Use the b key for manual mode, the n key for object following, and the m key for line following. The number keys can be used to set a speed. Use spacebar to take off and land, which is required before any control can be executed.\n\n");
}

/*
  Detect ESC key press and
*/
bool Control::detectEscape() {
  //Escape key
  if (key == 0x1b) { return true; }
  return false;
}

/*
*/
void Control::changeSpeed() {
  if ((key >= '0') && (key <= '9')) //number keys
  {
    speed = (key-'0')*0.1;
  }
}

/*
*/
void Control::getImage() {
//Get an image
  image = ardrone.getImage();
}

/*
  Switch between flying modes
*/
void Control::detectFlyingMode() {

  if (key == 'b') {
    flyingMode = Manual;
    ardrone.setCamera(0);
    printf("Manual flying mode is enabled\n");
    printf("Press n for object following and m for line following\n");
    printf("While in manual mode, use q and a for up and down, t and g for forward and backward, and f and h for left and right. Press space to take off.\n\n");
  }
  else if (key == 'n') {
    flyingMode = ObjectFollow;
    ardrone.setCamera(0);
    printf("Object Following flying mode is enabled\n");
    printf("Press b for manual and m for line following\n");
    printf("While in object following mode, use l to toggle learning a specific color. Press space to take off after learning a color.\n\n");
  }
  else if (key == 'm') {
    flyingMode = LineFollow;
    ardrone.setCamera(1);
    printf("Line Following flying mode is enabled\n");
    printf("Press b for manual and n for object following\n");
    printf("No control for line following yet exists\n\n");
  }
}

void Control::overlayControl() {
  //TODO: move this so it isnt called every time
  cv::Scalar green = CV_RGB(0,255,0); //putText color value

  char modeDisplay[80]; //print buffer for flying mode
  char speedDisplay[80]; //print buffer for speed

  if (flyingMode == Manual) {
    sprintf(modeDisplay, "Manual Mode");
  }
  else if (flyingMode == ObjectFollow) {
    sprintf(modeDisplay, "Object Following Mode");
  }
  else if (flyingMode == LineFollow) {
    sprintf(modeDisplay, "Line Following Mode");
  }

  sprintf(speedDisplay, "Speed = %3.2f", speed);

  //add speed to overlay
  putText(image, speedDisplay, cvPoint(30,40), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

  //add flying mode to overlay
  putText(image, modeDisplay, cvPoint(30,20), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

  cv::imshow("camera", image); //Display the camera feed
}

int main(int argc, char *argv[])
{
  Control control;

  //Controlling classes
  ObjectFollowing objectFollowing;
  //TODO: ManualDriving manualDriving;
  //TODO: LineFollowing lineFollwing;

  //Display variables
  char flyingDisplay[80]; //print buffer for if flying

  cv::Scalar green = CV_RGB(0,255,0); //putText color value

  line_main();
  control.initializeDroneControl(&objectFollowing);

  // Main loop
  while (1) {
    control.key = cv::waitKey(33); // Key input

    if (control.detectEscape()) { break; } //Press ESC to close program

    control.detectFlyingMode(); //Press b, n, m to change mode

    control.changeSpeed(); //Press 0-9 to change speed

    control.getImage(); //get the image from the camera

    //Take off / Landing
    if (control.key == ' ') { //spacebar
      if (control.ardrone.onGround()) {
        control.ardrone.takeoff();
	fprintf(control.flight_log, "TAKEOFF\n");
        std::this_thread::sleep_for(std::chrono::milliseconds(5000));
      }
      else {
        control.ardrone.landing();
	fprintf(control.flight_log, "LAND\n");
      }
    }

    //TODO:Write battery percentage to screen
    printf("%d\n", control.ardrone.getBatteryPercentage());

    //Write if grounded or flying to image
    if (control.ardrone.onGround()) {
      sprintf(flyingDisplay, "Landed");
    }
    else {
      sprintf(flyingDisplay, "Flying");
    }
    putText(control.image, flyingDisplay, cvPoint(200,40), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

    switch (control.flyingMode) {
      case Manual:
        //TODO: Allow user to set camera mode in Manual
        //TODO: Change 0/1 to CONSTANTS of 0 = front, 1 = bottom

        //TODO: Scale these values for normal human control when in manual mode
        control.velocities = manualMovement(control.key);

        //TODO: Move this into manualMovement(control.key) function
        displayManualInfo(&(control.image), control.velocities);

        break;

      case ObjectFollow:
        control.velocities = objectFollowing.detectObject(control.image, control.key);

        break;

      case LineFollow:
          //TODO implement for real
        control.velocities = lineFollowingControl();
        break;
    }

    control.overlayControl();

    control.ardrone.move3D(control.velocities.vx * control.speed, control.velocities.vy * control.speed, control.velocities.vz * control.speed, control.velocities.vr);

  }

  //Write hsv values to a file
  objectFollowing.closeObjectFollowing();
  //TODO: closeManual();
  //TODO: closeLineFollowing();

  //Close connection to drone
  control.ardrone.close();

  return 0;
}
<commit_msg>testing<commit_after>/*
  Original code from tekkies/CVdrone (get actual address from github)

  Modified by Elliot Greenlee, Caleb Mennen, Jacob Pollack
  COSC 402 Senior Design

  Min Kao Drone Tour
  See readme at (get actual address from github)
*/

#include "ardrone/ardrone.h"
#include "structures.h"
#include "manual/manual.h"
#include "objectFollowing/objectFollowing.h"
#include "lineFollowing/lineFollowing.h"
#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

using namespace std;

class Control {
  public:
    const string flightLog = "flight_log.txt";

    //AR.Drone class
    ARDrone ardrone;

    cv::Mat image;

    int key;
    FlyingMode flyingMode = Manual;
    double speed = 0.0;
    int batteryPercentage;
    bool flying;

    ControlMovements velocities;

    FILE *flight_log;

    void initializeDroneControl(ObjectFollowing *objectFollowing);
    void detectFlyingMode();
    bool detectEscape();
    void changeSpeed();
    void getImage();

    void overlayControl();
};

/*
*/
void Control::initializeDroneControl(ObjectFollowing *objectFollowing) {
  //Initializing Message
  printf("Connecting to the drone\n");
  printf("If there is no version number response in the next 10 seconds, please restart the drone and code.\n");
  fflush(stdout);

  // Initialize
  if (!ardrone.open()) {
    printf("Failed to initialize.\n");
    //TODO: fix this return -1;
  }

  //Set drone on flat surface and initialize
  ardrone.setFlatTrim();

  //initialize object following code
  objectFollowing->initializeObjectFollowing();
  flight_log = fopen(flightLog.c_str(), "w");

  //Print default command information
  printf("To disconnect, press the ESC key\n\n");
  printf("Currently the drone is in manual mode.\n");
  printf("Use the b key for manual mode, the n key for object following, and the m key for line following. The number keys can be used to set a speed. Use spacebar to take off and land, which is required before any control can be executed.\n\n");
}

/*
  Detect ESC key press and
*/
bool Control::detectEscape() {
  //Escape key
  if (key == 0x1b) { return true; }
  return false;
}

/*
*/
void Control::changeSpeed() {
  if ((key >= '0') && (key <= '9')) //number keys
  {
    speed = (key-'0')*0.1;
  }
}

/*
*/
void Control::getImage() {
//Get an image
  image = ardrone.getImage();
}

/*
  Switch between flying modes
*/
void Control::detectFlyingMode() {

  if (key == 'b') {
    flyingMode = Manual;
    ardrone.setCamera(0);
    printf("Manual flying mode is enabled\n");
    printf("Press n for object following and m for line following\n");
    printf("While in manual mode, use q and a for up and down, t and g for forward and backward, and f and h for left and right. Press space to take off.\n\n");
  }
  else if (key == 'n') {
    flyingMode = ObjectFollow;
    ardrone.setCamera(0);
    printf("Object Following flying mode is enabled\n");
    printf("Press b for manual and m for line following\n");
    printf("While in object following mode, use l to toggle learning a specific color. Press space to take off after learning a color.\n\n");
  }
  else if (key == 'm') {
    flyingMode = LineFollow;
    ardrone.setCamera(1);
    printf("Line Following flying mode is enabled\n");
    printf("Press b for manual and n for object following\n");
    printf("No control for line following yet exists\n\n");
  }
}

void Control::overlayControl() {
  //TODO: move this so it isnt called every time
  cv::Scalar green = CV_RGB(0,255,0); //putText color value

  char modeDisplay[80]; //print buffer for flying mode
  char speedDisplay[80]; //print buffer for speed

  if (flyingMode == Manual) {
    sprintf(modeDisplay, "Manual Mode");
  }
  else if (flyingMode == ObjectFollow) {
    sprintf(modeDisplay, "Object Following Mode");
  }
  else if (flyingMode == LineFollow) {
    sprintf(modeDisplay, "Line Following Mode");
  }

  sprintf(speedDisplay, "Speed = %3.2f", speed);

  //add speed to overlay
  putText(image, speedDisplay, cvPoint(30,40), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

  //add flying mode to overlay
  putText(image, modeDisplay, cvPoint(30,20), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

  cv::imshow("camera", image); //Display the camera feed
}

int main(int argc, char *argv[])
{
  Control control;

  //Controlling classes
  ObjectFollowing objectFollowing;
  //TODO: ManualDriving manualDriving;
  //TODO: LineFollowing lineFollwing;

  //Display variables
  char flyingDisplay[80]; //print buffer for if flying

  cv::Scalar green = CV_RGB(0,255,0); //putText color value

  line_main();
  return 1;
  control.initializeDroneControl(&objectFollowing);

  // Main loop
  while (1) {
    control.key = cv::waitKey(33); // Key input

    if (control.detectEscape()) { break; } //Press ESC to close program

    control.detectFlyingMode(); //Press b, n, m to change mode

    control.changeSpeed(); //Press 0-9 to change speed

    control.getImage(); //get the image from the camera

    //Take off / Landing
    if (control.key == ' ') { //spacebar
      if (control.ardrone.onGround()) {
        control.ardrone.takeoff();
	fprintf(control.flight_log, "TAKEOFF\n");
        std::this_thread::sleep_for(std::chrono::milliseconds(5000));
      }
      else {
        control.ardrone.landing();
	fprintf(control.flight_log, "LAND\n");
      }
    }

    //TODO:Write battery percentage to screen
    printf("%d\n", control.ardrone.getBatteryPercentage());

    //Write if grounded or flying to image
    if (control.ardrone.onGround()) {
      sprintf(flyingDisplay, "Landed");
    }
    else {
      sprintf(flyingDisplay, "Flying");
    }
    putText(control.image, flyingDisplay, cvPoint(200,40), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

    switch (control.flyingMode) {
      case Manual:
        //TODO: Allow user to set camera mode in Manual
        //TODO: Change 0/1 to CONSTANTS of 0 = front, 1 = bottom

        //TODO: Scale these values for normal human control when in manual mode
        control.velocities = manualMovement(control.key);

        //TODO: Move this into manualMovement(control.key) function
        displayManualInfo(&(control.image), control.velocities);

        break;

      case ObjectFollow:
        control.velocities = objectFollowing.detectObject(control.image, control.key);

        break;

      case LineFollow:
          //TODO implement for real
        control.velocities = lineFollowingControl();
        break;
    }

    control.overlayControl();

    control.ardrone.move3D(control.velocities.vx * control.speed, control.velocities.vy * control.speed, control.velocities.vz * control.speed, control.velocities.vr);

  }

  //Write hsv values to a file
  objectFollowing.closeObjectFollowing();
  //TODO: closeManual();
  //TODO: closeLineFollowing();

  //Close connection to drone
  control.ardrone.close();

  return 0;
}
<|endoftext|>


<commit_before>/*
 * This program is free software; you can redistribute it and/or modify it under
 * the terms of the GNU General Public License as published by the Free Software
 * Foundation; version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
 * details.
 */

#include <QtGui/QApplication>
#include "config.h"
#include "dbmanager.h"
#include "iconmanager.h"
#include "mainwindow.h"
#include "plugins/pluginmanager.h"
#include "sqlhighlighter.h"
#include "tabwidget/abstracttabwidget.h"
#include "widgets/querytextedit.h"

int main(int argc, char *argv[])
{
  QApplication::setApplicationName("dbmaster");
  QApplication::setApplicationVersion("0.8");
  QApplication::setOrganizationDomain("dbmaster.org");
  QApplication a(argc, argv);

  QSplashScreen splash(QPixmap(":/img/splash.png"));
  splash.show();

  // Loading translations
  splash.showMessage(QObject::tr("Loading translations..."), Qt::AlignBottom);

  QTranslator translator;
  // getting the current locale
  QString lang = QLocale::system().name();
  QString transdir;
#if defined(Q_WS_X11)
  // for *nix
  transdir = QString(PREFIX).append("/share/dbmaster/tr");
#endif

#if defined(Q_WS_WIN)
  transdir = "share/tr";
#endif

  QString path;
  /* on teste d'abord si le .qm est dans le dossier courant -> alors on est en
   * dev, on le charge, sinon il est dans le répertoire d'installation. */
  path = QDir::currentPath() + QString("/../tr/%1.qm").arg(lang);
  if (!QFile::exists(path))
    path = transdir.append("/%1.qm").arg(lang);
  translator.load(path);
  a.installTranslator(&translator);

  QTranslator qtTranslator;
  qtTranslator.load("qt_" + lang,
#if defined (Q_WS_X11)
                    QLibraryInfo::location(QLibraryInfo::TranslationsPath));
#endif
#if defined (Q_WS_WIN)
                    QDir::currentPath());
#endif
  a.installTranslator(&qtTranslator);

  // Ajout des plugins
  splash.showMessage(QObject::tr("Loading plugins..."), Qt::AlignBottom);
  PluginManager::init();

  splash.showMessage(QObject::tr("Initialization..."), Qt::AlignBottom);

  IconManager::init();
  DbManager::init();
  Config::init();
  QueryTextEdit::reloadCompleter();

  MainWindow *w = new MainWindow();
  w->show();
  splash.finish(w);

  if (a.arguments().size() >= 2) {
    for (int i=1; i<a.arguments().size(); i++) {
      w->openQuery(a.arguments()[i]);
    }
  }

  return a.exec();
}
<commit_msg>Num version<commit_after>/*
 * This program is free software; you can redistribute it and/or modify it under
 * the terms of the GNU General Public License as published by the Free Software
 * Foundation; version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
 * details.
 */

#include <QtGui/QApplication>
#include "config.h"
#include "dbmanager.h"
#include "iconmanager.h"
#include "mainwindow.h"
#include "plugins/pluginmanager.h"
#include "sqlhighlighter.h"
#include "tabwidget/abstracttabwidget.h"
#include "widgets/querytextedit.h"

int main(int argc, char *argv[]) {
  QApplication::setApplicationName("dbmaster");
  QApplication::setApplicationVersion("0.8.1");
  QApplication::setOrganizationDomain("dbmaster.org");
  QApplication a(argc, argv);

  QSplashScreen splash(QPixmap(":/img/splash.png"));
  splash.show();

  // Loading translations
  splash.showMessage(QObject::tr("Loading translations..."), Qt::AlignBottom);

  QTranslator translator;
  // getting the current locale
  QString lang = QLocale::system().name();
  QString transdir;
#if defined(Q_WS_X11)
  // for *nix
  transdir = QString(PREFIX).append("/share/dbmaster/tr");
#endif

#if defined(Q_WS_WIN)
  transdir = "share/tr";
#endif

  QString path;
  /* on teste d'abord si le .qm est dans le dossier courant -> alors on est en
   * dev, on le charge, sinon il est dans le répertoire d'installation. */
  path = QDir::currentPath() + QString("/../tr/%1.qm").arg(lang);
  if (!QFile::exists(path))
    path = transdir.append("/%1.qm").arg(lang);
  translator.load(path);
  a.installTranslator(&translator);

  QTranslator qtTranslator;
  qtTranslator.load("qt_" + lang,
#if defined (Q_WS_X11)
                    QLibraryInfo::location(QLibraryInfo::TranslationsPath));
#endif
#if defined (Q_WS_WIN)
                    QDir::currentPath());
#endif
  a.installTranslator(&qtTranslator);

  // Ajout des plugins
  splash.showMessage(QObject::tr("Loading plugins..."), Qt::AlignBottom);
  PluginManager::init();

  splash.showMessage(QObject::tr("Initialization..."), Qt::AlignBottom);

  IconManager::init();
  DbManager::init();
  Config::init();
  QueryTextEdit::reloadCompleter();

  MainWindow *w = new MainWindow();
  w->show();
  splash.finish(w);

  if (a.arguments().size() >= 2) {
    for (int i=1; i<a.arguments().size(); i++) {
      w->openQuery(a.arguments()[i]);
    }
  }

  return a.exec();
}
<|endoftext|>


<commit_before>#include <Arduino.h>
#include <avr/wdt.h>
#include <TinyGPS++.h>
#include <idDHT11.h>
#include <SDCard.h>
#include <Utils.h>
#include <PinMap.h>

#ifdef DEBUG
#include <MemoryFree.h>
#include <SoftwareSerial.h>
SoftwareSerial gpsSerial(8, 9);
#endif

SDCard sd(SD_PIN);
TinyGPSPlus gps;
Utils utils;

bool firstEntry = true;
char fileName [11];
int ledState = LOW;

/**
 * initialize dht as on library documentation
 * @see https://github.com/niesteszeck/idDHT11/blob/master/examples/idDHT11_Lib_example/idDHT11_Lib_example.ino
 * @see https://www.arduino.cc/en/Reference/AttachInterrupt
 */
void dht11_wrapper();
idDHT11 dht(DHT_PIN, 1, dht11_wrapper);
void dht11_wrapper() {
    dht.isrCallback();
}

/**
 * wrap all data together to be appended to csv log file
 */
void serialize(char* entry) {
    while(dht.acquiring());
    int result = dht.getStatus();
    if (result != IDDHTLIB_OK) {
        #ifdef DEBUG
        dht.printError(result);
        #endif
    } else {
        // save new values
        dht.saveLastValidData();
    }
    sprintf(
        entry,
        "%i,%i,%i,%i,%0.2f,%0.2f,%0.2f",
        utils.readMQ(MQ2_PIN), utils.readMQ(MQ135_PIN),
        (int)dht.getLastValidCelsius(), (int)dht.getLastValidHumidity(),
        gps.location.lat(), gps.location.lng(), gps.altitude.meters()
    );
}

void createFileName(char *str) {
    // limited to 8.3 file format https://en.wikipedia.org/wiki/8.3_filename
    // so .csv was removed
    sprintf(str, "%d-%d-%d", gps.date.day(), gps.time.hour(), gps.time.minute());
}

/**
 * delay while keep reading gps data
 */
void smartDelay(const unsigned long &delay) {
    unsigned long initialTime = millis();
    unsigned long currentTime;
    do {
        #ifdef DEBUG
        if (gpsSerial.available() > 0) {
            gps.encode(gpsSerial.read());
        #else
        if (Serial.available() > 0) {
            gps.encode(Serial.read());
        #endif
        }
        currentTime = millis();
    } while (currentTime - initialTime < delay);
}

void setup() {

    Serial.begin(9600);
    #ifdef DEBUG
    Serial.println(F("setup: initializing sensor platform..."));
    gpsSerial.begin(9600);
    #endif

    // setup control led
    pinMode(LED_PIN, OUTPUT);
    digitalWrite(LED_PIN, LOW);

    // enable watchdog with 2 seconds timer
    wdt_enable(WDTO_2S);

    if(!sd.begin()) {
        utils.error("setup: sd could not begin!");
    }

    // start acquiring first value, result is interrupt driven
    // the dht sensor is having issues since the soldering on the board,
    // this is a tmeporary work around to get valid values
    if (dht.acquireAndWait() != IDDHTLIB_OK) {
        utils.error("setup: dht could not acquire proper data!");
    } else {
        dht.saveLastValidData();
    }

    #ifdef DEBUG
    Serial.println(F("setup: initialization went okay!"));
    #endif

}

void loop() {
    smartDelay(255);
    if (firstEntry && gps.date.isValid() && gps.location.isValid()) {
        // if valid state is entered, led is on
        digitalWrite(LED_PIN, HIGH);
        createFileName(fileName);
        if (sd.exists(fileName)) {
            #ifdef DEBUG
            Serial.print(F("appending to file "));
            Serial.println(fileName);
            #endif
            firstEntry = false;
        } else {
            #ifdef DEBUG
            Serial.print(F("new log file "));
            Serial.println(fileName);
            #endif
            firstEntry = false;
        }
    }
    if (!firstEntry && gps.location.isValid()) {
        // if valid state is entered, led is on
        digitalWrite(LED_PIN, HIGH);
        if (gps.location.isUpdated()) {
            // get new dht value
            dht.acquire();
            char entry [40];
            serialize(entry);
            #ifdef DEBUG
            Serial.println(entry);
            #endif
            if (!sd.writeToFile(fileName, entry)) {
                utils.error("could not write data to file");
            }
        }
    } else {
        // blink for invalid gps data
        if (ledState == LOW) {
            ledState = HIGH;
        } else {
            ledState = LOW;
        }
        digitalWrite(LED_PIN, ledState);
    }
    // reset watchdog timer
    wdt_reset();
}
<commit_msg>add more precision to lat/lon<commit_after>#include <Arduino.h>
#include <avr/wdt.h>
#include <TinyGPS++.h>
#include <idDHT11.h>
#include <SDCard.h>
#include <Utils.h>
#include <PinMap.h>

#ifdef DEBUG
#include <MemoryFree.h>
#include <SoftwareSerial.h>
SoftwareSerial gpsSerial(8, 9);
#endif

SDCard sd(SD_PIN);
TinyGPSPlus gps;
Utils utils;

bool firstEntry = true;
char fileName [11];
int ledState = LOW;

/**
 * initialize dht as on library documentation
 * @see https://github.com/niesteszeck/idDHT11/blob/master/examples/idDHT11_Lib_example/idDHT11_Lib_example.ino
 * @see https://www.arduino.cc/en/Reference/AttachInterrupt
 */
void dht11_wrapper();
idDHT11 dht(DHT_PIN, 1, dht11_wrapper);
void dht11_wrapper() {
    dht.isrCallback();
}

/**
 * wrap all data together to be appended to csv log file
 */
void serialize(char* entry) {
    while(dht.acquiring());
    int result = dht.getStatus();
    if (result != IDDHTLIB_OK) {
        #ifdef DEBUG
        dht.printError(result);
        #endif
    } else {
        // save new values
        dht.saveLastValidData();
    }
    sprintf(
        entry,
        "%i,%i,%i,%i,%0.6f,%0.6f,%0.2f",
        utils.readMQ(MQ2_PIN), utils.readMQ(MQ135_PIN),
        (int)dht.getLastValidCelsius(), (int)dht.getLastValidHumidity(),
        gps.location.lat(), gps.location.lng(), gps.altitude.meters()
    );
}

void createFileName(char *str) {
    // limited to 8.3 file format https://en.wikipedia.org/wiki/8.3_filename
    // so .csv was removed
    sprintf(str, "%d-%d-%d", gps.date.day(), gps.time.hour(), gps.time.minute());
}

/**
 * delay while keep reading gps data
 */
void smartDelay(const unsigned long &delay) {
    unsigned long initialTime = millis();
    unsigned long currentTime;
    do {
        #ifdef DEBUG
        if (gpsSerial.available() > 0) {
            gps.encode(gpsSerial.read());
        #else
        if (Serial.available() > 0) {
            gps.encode(Serial.read());
        #endif
        }
        currentTime = millis();
    } while (currentTime - initialTime < delay);
}

void setup() {

    Serial.begin(9600);
    #ifdef DEBUG
    Serial.println(F("setup: initializing sensor platform..."));
    gpsSerial.begin(9600);
    #endif

    // setup control led
    pinMode(LED_PIN, OUTPUT);
    digitalWrite(LED_PIN, LOW);

    // enable watchdog with 2 seconds timer
    wdt_enable(WDTO_2S);

    if(!sd.begin()) {
        utils.error("setup: sd could not begin!");
    }

    // start acquiring first value, result is interrupt driven
    // the dht sensor is having issues since the soldering on the board,
    // this is a tmeporary work around to get valid values
    if (dht.acquireAndWait() != IDDHTLIB_OK) {
        utils.error("setup: dht could not acquire proper data!");
    } else {
        dht.saveLastValidData();
    }

    #ifdef DEBUG
    Serial.println(F("setup: initialization went okay!"));
    #endif

}

void loop() {
    smartDelay(255);
    if (firstEntry && gps.date.isValid() && gps.location.isValid()) {
        // if valid state is entered, led is on
        digitalWrite(LED_PIN, HIGH);
        createFileName(fileName);
        if (sd.exists(fileName)) {
            #ifdef DEBUG
            Serial.print(F("appending to file "));
            Serial.println(fileName);
            #endif
            firstEntry = false;
        } else {
            #ifdef DEBUG
            Serial.print(F("new log file "));
            Serial.println(fileName);
            #endif
            firstEntry = false;
        }
    }
    if (!firstEntry && gps.location.isValid()) {
        // if valid state is entered, led is on
        digitalWrite(LED_PIN, HIGH);
        if (gps.location.isUpdated()) {
            // get new dht value
            dht.acquire();
            char entry [40];
            serialize(entry);
            #ifdef DEBUG
            Serial.println(entry);
            #endif
            if (!sd.writeToFile(fileName, entry)) {
                utils.error("could not write data to file");
            }
        }
    } else {
        // blink for invalid gps data
        if (ledState == LOW) {
            ledState = HIGH;
        } else {
            ledState = LOW;
        }
        digitalWrite(LED_PIN, ledState);
    }
    // reset watchdog timer
    wdt_reset();
}
<|endoftext|>


<commit_before>#include "libhaloc/lc.h"
#include "libhaloc/utils.h"
#include <iostream>
#include <numeric>
#include <stdlib.h>
#include <fstream>
#include <sstream>
#include <string>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

namespace fs=boost::filesystem;

/** \brief Parameter constructor. Sets the parameter struct to default values.
  */
haloc::LoopClosure::Params::Params() :
  work_dir(""),
  desc_type("SIFT"),
  num_proj(DEFAULT_NUM_PROJ),
  desc_thresh(DEFAULT_DESC_THRESH),
  epipolar_thresh(DEFAULT_EPIPOLAR_THRESH),
  min_neighbour(DEFAULT_MIN_NEIGHBOUR),
  n_candidates(DEFAULT_N_CANDIDATES),
  min_matches(DEFAULT_MIN_MATCHES),
  min_inliers(DEFAULT_MIN_INLIERS),
  max_reproj_err(DEFAULT_MAX_REPROJ_ERR),
  validate(DEFAULT_VALIDATE)
{}

/** \brief LoopClosure class constructor.
  */
haloc::LoopClosure::LoopClosure(){}

/** \brief Sets the class parameters.
  * \param stuct of parameters.
  */
void haloc::LoopClosure::setParams(const Params& params)
{
  params_ = params;
}

/** \brief Sets the camera model.
  * \param Stereo camera model.
  */
void haloc::LoopClosure::setCameraModel(image_geometry::StereoCameraModel stereo_camera_model, Mat camera_matrix)
{
  img_.setCameraModel(stereo_camera_model);
  camera_matrix_ = camera_matrix;
}

/** \brief Initializes the loop closure class.
  */
void haloc::LoopClosure::init()
{
  // Working directory sanity check
  if (params_.work_dir[params_.work_dir.length()-1] != '/')
    params_.work_dir += "/ex_" + boost::lexical_cast<string>(time(0));
  else
    params_.work_dir += "ex_" + boost::lexical_cast<string>(time(0));

  // Create the directory to store the keypoints and descriptors
  if (fs::is_directory(params_.work_dir))
    fs::remove_all(params_.work_dir);
  fs::path dir(params_.work_dir);
  if (!fs::create_directory(dir))
    ROS_ERROR("[Haloc:] ERROR -> Impossible to create the execution directory.");

  // Initialize image properties
  haloc::Image::Params img_params;
  img_params.desc_type = params_.desc_type;
  img_params.desc_thresh = params_.desc_thresh;
  img_params.epipolar_thresh = params_.epipolar_thresh;
  img_.setParams(img_params);

  // Initialize hash
  haloc::Hash::Params hash_params;
  hash_params.num_proj = params_.num_proj;
  hash_.setParams(hash_params);

  // Init main variables
  hash_table_.clear();
  img_idx_ = 0;
}

/** \brief Finalizes the loop closure class.
  */
void haloc::LoopClosure::finalize()
{
  // Remove the temporal directory
  if (fs::is_directory(params_.work_dir))
    fs::remove_all(params_.work_dir);
}

/** \brief Compute kp, desc and hash for one image (mono verion).
  * \param cvMat containing the image.
  * \param human readable name for this image
  */
void haloc::LoopClosure::setNode(Mat img, string name)
{
  // Set the image
  img_.setMono(img);

  // Save kp and descriptors
  vector<Point3f> empty;
  FileStorage fs(params_.work_dir+"/"+boost::lexical_cast<string>(img_idx_)+".yml", FileStorage::WRITE);
  write(fs, "name", name);
  write(fs, "kp", img_.getKp());
  write(fs, "desc", img_.getDesc());
  write(fs, "threed", empty);
  fs.release();
  img_idx_++;
}

/** \brief Compute kp, desc and hash for two images (stereo verion).
  * \param cvMat containing the left image.
  * \param cvMat containing the right image.
  * \param human readable name for this image
  */
void haloc::LoopClosure::setNode(Mat img_l, Mat img_r, string name)
{
  // Set the image
  img_.setStereo(img_l, img_r);

  // Save kp and descriptors
  FileStorage fs(params_.work_dir+"/"+boost::lexical_cast<string>(img_idx_)+".yml", FileStorage::WRITE);
  write(fs, "name", name);
  write(fs, "kp", img_.getKp());
  write(fs, "desc", img_.getDesc());
  write(fs, "threed", img_.get3D());
  fs.release();
  img_idx_++;
}

/** \brief Try to find a loop closure between last node and all other nodes.
  * @return true if valid loop closure, false otherwise.
  * \param Return the index of the image that closes loop (-1 if no loop).
  */
bool haloc::LoopClosure::getLoopClosure(int& lc_img_idx, string& lc_name)
{
  tf::Transform trans;
  return getLoopClosure(lc_img_idx, lc_name, trans);
}
/** \brief Try to find a loop closure between last node and all other nodes.
  * @return true if valid loop closure, false otherwise.
  * \param Return the index of the image that closes loop (-1 if no loop).
  * \param Return the name of the image that closes loop (empty if no loop).
  * \param Return the transform between nodes if loop closure is valid.
  */
bool haloc::LoopClosure::getLoopClosure(int& lc_img_idx, string& lc_name, tf::Transform& trans)
{
  // Initialize hash
  if (!hash_.isInitialized())
  {
    hash_.init(img_.getDesc(), true);
    return false;
  }

  // Compute the hash for this image
  vector<float> hash_val = hash_.getHash(img_.getDesc());
  hash_table_.push_back(make_pair(img_idx_-1, hash_val));

  // Check if enough neighours
  if (hash_table_.size() <= params_.min_neighbour)
    return false;

  // Compute the hash matchings for this image with all other sequence
  vector< pair<int,float> > matchings;
  for (uint i=0; i<hash_table_.size()-params_.min_neighbour; i++)
  {
    // Hash matching
    vector<float> cur_hash = hash_table_[i].second;
    float m = hash_.match(hash_val, cur_hash);
    matchings.push_back(make_pair(hash_table_[i].first, m));
  }

  // Sort the hash matchings
  sort(matchings.begin(), matchings.end(), haloc::Utils::sortByMatching);

  // Check for loop closure
  trans.setIdentity();
  lc_img_idx = -1;
  lc_name = "";
  int best_m = 0;
  int matches = 0;
  int inliers = 0;
  bool valid = false;
  while (best_m<params_.n_candidates)
  {
    // Sanity check
    if(best_m >= matchings.size())
    {
      best_m = 0;
      break;
    }

    // Loop-closure?
    valid = compute(img_,
                    params_.work_dir+"/"+boost::lexical_cast<string>(matchings[best_m].first)+".yml",
                    lc_name,
                    matches,
                    inliers,
                    trans);

    // If the loop closure is valid and the seconds step validation is disabled, that's all.
    if (valid && !params_.validate) break;

    // Validate the loop closure?
    if (valid && params_.validate)
    {
      // Initialize validation
      bool validate_valid = false;
      int matches_val, inliers_val;
      tf::Transform trans_val;
      string tmp_name;

      // Loop closure for the previous image?
      validate_valid = compute(img_,
                               params_.work_dir+"/"+boost::lexical_cast<string>(matchings[best_m].first - 1)+".yml",
                               tmp_name,
                               matches_val,
                               inliers_val,
                               trans_val);

      if (!validate_valid)
      {
        // Previous validation does not works, try to validate with the next image
        validate_valid = compute(img_,
                                 params_.work_dir+"/"+boost::lexical_cast<string>(matchings[best_m].first + 1)+".yml",
                                 tmp_name,
                                 matches_val,
                                 inliers_val,
                                 trans_val);
      }

      // If validation, exit. If not, mark as non-valid
      if (validate_valid)
        break;
      else
        valid = false;
    }

    best_m++;
  }

  // Get the image of the loop closure
  if (valid && best_m < matchings.size())
    lc_img_idx = matchings[best_m].first;
  else
    lc_name = "";

  // Return true if any valid loop closure has been found.
  return valid;
}

/** \brief Compute the loop closure (if any).
  * @return true if valid loop closure, false otherwise.
  * \param Reference image object.
  * \param Current image filename with all the properties.
  * \param Return the number of matches found.
  * \param Return the number of inliers found.
  * \param Return the transform between nodes if loop closure is valid.
  */
bool haloc::LoopClosure::compute(Image ref_image,
                                 string cur_filename,
                                 string &lc_name,
                                 int &matches,
                                 int &inliers,
                                 tf::Transform& trans)
{
  // Initialize outputs
  matches = 0;
  inliers = 0;

  // Sanity check
  if ( !fs::exists(cur_filename) ) return false;

  // Get the image keypoints and descriptors
  FileStorage fs;
  fs.open(cur_filename, FileStorage::READ);
  if (!fs.isOpened())
    ROS_ERROR("[Haloc:] ERROR -> Failed to open the image keypoints and descriptors.");
  vector<Point2f> cur_kp;
  Mat cur_desc;
  vector<Point3f> points_3d;
  fs["name"] >> lc_name;
  fs["kp"] >> cur_kp;
  fs["desc"] >> cur_desc;
  fs["threed"] >> points_3d;
  fs.release();

  // Descriptors crosscheck matching
  vector<DMatch> desc_matches;
  Mat match_mask;
  haloc::Utils::crossCheckThresholdMatching(ref_image.getDesc(),
                                            cur_desc,
                                            params_.desc_thresh,
                                            match_mask, desc_matches);
  matches = (int)desc_matches.size();

  // Check matches size
  if (matches < params_.min_matches)
    return false;

  // Get the matched keypoints
  vector<Point2f> ref_kp = ref_image.getKp();
  vector<Point2f> ref_points;
  vector<Point2f> cur_points;
  for(int i=0; i<matches; i++)
  {
    ref_points.push_back(ref_kp[desc_matches[i].queryIdx]);
    cur_points.push_back(cur_kp[desc_matches[i].trainIdx]);
  }

  // Proceed depending on mono or stereo
  if (points_3d.size() == 0) // Mono
  {
    // Check the epipolar geometry
    Mat status;
    Mat F = findFundamentalMat(ref_points, cur_points, FM_RANSAC, params_.epipolar_thresh, 0.999, status);

    // Is the fundamental matrix valid?
    Scalar f_sum_parts = cv::sum(F);
    float f_sum = (float)f_sum_parts[0] + (float)f_sum_parts[1] + (float)f_sum_parts[2];
    if (f_sum < 1e-3)
      return false;

    // Check inliers size
    inliers = (int)cv::sum(status)[0];
    if (inliers < params_.min_inliers)
      return false;
  }
  else // Stereo
  {
    Mat rvec, tvec;
    vector<int> solvepnp_inliers;
    solvePnPRansac(points_3d, cur_points, camera_matrix_,
                   cv::Mat(), rvec, tvec, false,
                   100, params_.max_reproj_err,
                   40, solvepnp_inliers);

    inliers = (int)solvepnp_inliers.size();
    if (inliers < params_.min_inliers)
      return false;

    trans = haloc::Utils::buildTransformation(rvec, tvec);
  }

  // If we arrive here, there is a loop closure.
  return true;
}<commit_msg>Fix incorrect indices for stereo loop closure<commit_after>#include "libhaloc/lc.h"
#include "libhaloc/utils.h"
#include <iostream>
#include <numeric>
#include <stdlib.h>
#include <fstream>
#include <sstream>
#include <string>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

namespace fs=boost::filesystem;

/** \brief Parameter constructor. Sets the parameter struct to default values.
  */
haloc::LoopClosure::Params::Params() :
  work_dir(""),
  desc_type("SIFT"),
  num_proj(DEFAULT_NUM_PROJ),
  desc_thresh(DEFAULT_DESC_THRESH),
  epipolar_thresh(DEFAULT_EPIPOLAR_THRESH),
  min_neighbour(DEFAULT_MIN_NEIGHBOUR),
  n_candidates(DEFAULT_N_CANDIDATES),
  min_matches(DEFAULT_MIN_MATCHES),
  min_inliers(DEFAULT_MIN_INLIERS),
  max_reproj_err(DEFAULT_MAX_REPROJ_ERR),
  validate(DEFAULT_VALIDATE)
{}

/** \brief LoopClosure class constructor.
  */
haloc::LoopClosure::LoopClosure(){}

/** \brief Sets the class parameters.
  * \param stuct of parameters.
  */
void haloc::LoopClosure::setParams(const Params& params)
{
  params_ = params;
}

/** \brief Sets the camera model.
  * \param Stereo camera model.
  */
void haloc::LoopClosure::setCameraModel(image_geometry::StereoCameraModel stereo_camera_model, Mat camera_matrix)
{
  img_.setCameraModel(stereo_camera_model);
  camera_matrix_ = camera_matrix;
}

/** \brief Initializes the loop closure class.
  */
void haloc::LoopClosure::init()
{
  // Working directory sanity check
  if (params_.work_dir[params_.work_dir.length()-1] != '/')
    params_.work_dir += "/ex_" + boost::lexical_cast<string>(time(0));
  else
    params_.work_dir += "ex_" + boost::lexical_cast<string>(time(0));

  // Create the directory to store the keypoints and descriptors
  if (fs::is_directory(params_.work_dir))
    fs::remove_all(params_.work_dir);
  fs::path dir(params_.work_dir);
  if (!fs::create_directory(dir))
    ROS_ERROR("[Haloc:] ERROR -> Impossible to create the execution directory.");

  // Initialize image properties
  haloc::Image::Params img_params;
  img_params.desc_type = params_.desc_type;
  img_params.desc_thresh = params_.desc_thresh;
  img_params.epipolar_thresh = params_.epipolar_thresh;
  img_.setParams(img_params);

  // Initialize hash
  haloc::Hash::Params hash_params;
  hash_params.num_proj = params_.num_proj;
  hash_.setParams(hash_params);

  // Init main variables
  hash_table_.clear();
  img_idx_ = 0;
}

/** \brief Finalizes the loop closure class.
  */
void haloc::LoopClosure::finalize()
{
  // Remove the temporal directory
  if (fs::is_directory(params_.work_dir))
    fs::remove_all(params_.work_dir);
}

/** \brief Compute kp, desc and hash for one image (mono verion).
  * \param cvMat containing the image.
  * \param human readable name for this image
  */
void haloc::LoopClosure::setNode(Mat img, string name)
{
  // Set the image
  img_.setMono(img);

  // Save kp and descriptors
  vector<Point3f> empty;
  FileStorage fs(params_.work_dir+"/"+boost::lexical_cast<string>(img_idx_)+".yml", FileStorage::WRITE);
  write(fs, "name", name);
  write(fs, "kp", img_.getKp());
  write(fs, "desc", img_.getDesc());
  write(fs, "threed", empty);
  fs.release();
  img_idx_++;
}

/** \brief Compute kp, desc and hash for two images (stereo verion).
  * \param cvMat containing the left image.
  * \param cvMat containing the right image.
  * \param human readable name for this image
  */
void haloc::LoopClosure::setNode(Mat img_l, Mat img_r, string name)
{
  // Set the image
  img_.setStereo(img_l, img_r);

  // Save kp and descriptors
  FileStorage fs(params_.work_dir+"/"+boost::lexical_cast<string>(img_idx_)+".yml", FileStorage::WRITE);
  write(fs, "name", name);
  write(fs, "kp", img_.getKp());
  write(fs, "desc", img_.getDesc());
  write(fs, "threed", img_.get3D());
  fs.release();
  img_idx_++;
}

/** \brief Try to find a loop closure between last node and all other nodes.
  * @return true if valid loop closure, false otherwise.
  * \param Return the index of the image that closes loop (-1 if no loop).
  */
bool haloc::LoopClosure::getLoopClosure(int& lc_img_idx, string& lc_name)
{
  tf::Transform trans;
  return getLoopClosure(lc_img_idx, lc_name, trans);
}

/** \brief Try to find a loop closure between last node and all other nodes.
  * @return true if valid loop closure, false otherwise.
  * \param Return the index of the image that closes loop (-1 if no loop).
  * \param Return the name of the image that closes loop (empty if no loop).
  * \param Return the transform between nodes if loop closure is valid.
  */
bool haloc::LoopClosure::getLoopClosure(int& lc_img_idx, string& lc_name, tf::Transform& trans)
{
  // Initialize hash
  if (!hash_.isInitialized())
  {
    hash_.init(img_.getDesc(), true);
    return false;
  }

  // Compute the hash for this image
  vector<float> hash_val = hash_.getHash(img_.getDesc());
  hash_table_.push_back(make_pair(img_idx_-1, hash_val));

  // Check if enough neighours
  if (hash_table_.size() <= params_.min_neighbour)
    return false;

  // Compute the hash matchings for this image with all other sequence
  vector< pair<int,float> > matchings;
  for (uint i=0; i<hash_table_.size()-params_.min_neighbour; i++)
  {
    // Hash matching
    vector<float> cur_hash = hash_table_[i].second;
    float m = hash_.match(hash_val, cur_hash);
    matchings.push_back(make_pair(hash_table_[i].first, m));
  }

  // Sort the hash matchings
  sort(matchings.begin(), matchings.end(), haloc::Utils::sortByMatching);

  // Check for loop closure
  trans.setIdentity();
  lc_img_idx = -1;
  lc_name = "";
  int best_m = 0;
  int matches = 0;
  int inliers = 0;
  bool valid = false;
  while (best_m<params_.n_candidates)
  {
    // Sanity check
    if(best_m >= matchings.size())
    {
      best_m = 0;
      break;
    }

    // Loop-closure?
    valid = compute(img_,
                    params_.work_dir+"/"+boost::lexical_cast<string>(matchings[best_m].first)+".yml",
                    lc_name,
                    matches,
                    inliers,
                    trans);

    // If the loop closure is valid and the seconds step validation is disabled, that's all.
    if (valid && !params_.validate) break;

    // Validate the loop closure?
    if (valid && params_.validate)
    {
      // Initialize validation
      bool validate_valid = false;
      int matches_val, inliers_val;
      tf::Transform trans_val;
      string tmp_name;

      // Loop closure for the previous image?
      validate_valid = compute(img_,
                               params_.work_dir+"/"+boost::lexical_cast<string>(matchings[best_m].first - 1)+".yml",
                               tmp_name,
                               matches_val,
                               inliers_val,
                               trans_val);

      if (!validate_valid)
      {
        // Previous validation does not works, try to validate with the next image
        validate_valid = compute(img_,
                                 params_.work_dir+"/"+boost::lexical_cast<string>(matchings[best_m].first + 1)+".yml",
                                 tmp_name,
                                 matches_val,
                                 inliers_val,
                                 trans_val);
      }

      // If validation, exit. If not, mark as non-valid
      if (validate_valid)
        break;
      else
        valid = false;
    }

    best_m++;
  }

  // Get the image of the loop closure
  if (valid && best_m < matchings.size())
    lc_img_idx = matchings[best_m].first;
  else
    lc_name = "";

  // Return true if any valid loop closure has been found.
  return valid;
}

/** \brief Compute the loop closure (if any).
  * @return true if valid loop closure, false otherwise.
  * \param Reference image object.
  * \param Current image filename with all the properties.
  * \param Return the number of matches found.
  * \param Return the number of inliers found.
  * \param Return the transform between nodes if loop closure is valid.
  */
bool haloc::LoopClosure::compute(Image ref_image,
                                 string cur_filename,
                                 string &lc_name,
                                 int &matches,
                                 int &inliers,
                                 tf::Transform& trans)
{
  // Initialize outputs
  matches = 0;
  inliers = 0;

  // Sanity check
  if ( !fs::exists(cur_filename) ) return false;

  // Get the image keypoints and descriptors
  FileStorage fs;
  fs.open(cur_filename, FileStorage::READ);
  if (!fs.isOpened())
    ROS_ERROR("[Haloc:] ERROR -> Failed to open the image keypoints and descriptors.");
  vector<Point2f> cur_kp;
  Mat cur_desc;
  vector<Point3f> cur_3d;
  fs["name"] >> lc_name;
  fs["kp"] >> cur_kp;
  fs["desc"] >> cur_desc;
  fs["threed"] >> cur_3d;
  fs.release();

  // Descriptors crosscheck matching
  vector<DMatch> desc_matches;
  Mat match_mask;
  haloc::Utils::crossCheckThresholdMatching(cur_desc,
                                            ref_image.getDesc(),
                                            params_.desc_thresh,
                                            match_mask, desc_matches);
  matches = (int)desc_matches.size();

  // Check matches size
  if (matches < params_.min_matches)
    return false;

  // Get the matched keypoints
  vector<Point2f> ref_kp = ref_image.getKp();
  vector<Point2f> ref_matched_kp;
  vector<Point2f> cur_matched_kp;
  vector<Point3f> cur_matched_3d_points;
  for(int i=0; i<matches; i++)
  {
    ref_matched_kp.push_back(ref_kp[desc_matches[i].trainIdx]);
    cur_matched_kp.push_back(cur_kp[desc_matches[i].queryIdx]);

    // Only stereo
    if (cur_3d.size() == 0)
    {
      cur_matched_3d_points.push_back(cur_3d[desc_matches[i].queryIdx]);
    }
  }

  // Proceed depending on mono or stereo
  if (cur_3d.size() == 0) // Mono
  {
    // Check the epipolar geometry
    Mat status;
    Mat F = findFundamentalMat(cur_matched_kp, ref_matched_kp, FM_RANSAC, params_.epipolar_thresh, 0.999, status);

    // Is the fundamental matrix valid?
    Scalar f_sum_parts = cv::sum(F);
    float f_sum = (float)f_sum_parts[0] + (float)f_sum_parts[1] + (float)f_sum_parts[2];
    if (f_sum < 1e-3)
      return false;

    // Check inliers size
    inliers = (int)cv::sum(status)[0];
    if (inliers < params_.min_inliers)
      return false;
  }
  else // Stereo
  {
    Mat rvec, tvec;
    vector<int> solvepnp_inliers;
    solvePnPRansac(cur_matched_3d_points, ref_matched_kp, camera_matrix_,
                   cv::Mat(), rvec, tvec, false,
                   100, params_.max_reproj_err,
                   40, solvepnp_inliers);

    inliers = (int)solvepnp_inliers.size();
    if (inliers < params_.min_inliers)
      return false;

    trans = haloc::Utils::buildTransformation(rvec, tvec);
  }

  // If we arrive here, there is a loop closure.
  return true;
}<|endoftext|>


<commit_before>#include <iostream>
#include <cstdlib>
#include <list>
#include <iomanip>
#include <string>

#if WIN32
	#include <windows.h>
#else
	#include <sys/ioctl.h>
	#include <stdio.h>
	#include <unistd.h>
#endif

int main(int argc, char** argv)
{
	std::list<std::wstring> consoleLines;
	int columns, rows;
	bool isRunning = true;

	consoleLines.push_front(std::wstring(L"[00:00] System: dc-console v1.0"));

	while (isRunning)
	{
#if WIN32
		std::system("cls");

		CONSOLE_SCREEN_BUFFER_INFO csbi;
		GetConsoleScreenBufferInfo(GetStdHandle(STD_OUTPUT_HANDLE), &csbi);
		columns = csbi.srWindow.Right - csbi.srWindow.Left + 1;
		rows = csbi.srWindow.Bottom - csbi.srWindow.Top + 1;
#else
		std::system("clear");

		struct winsize w;
		ioctl(STDOUT_FILENO, TIOCGWINSZ, &w);
		columns = w.ws_col;
		rows = w.ws_row;
#endif

		for (int i = 0; i < rows - 1; ++i)
		{
			if (consoleLines.size() > rows - i - 2)
			{
				std::list<std::wstring>::iterator it = std::next(consoleLines.begin(), rows - i - 2);
				std::cout << std::setiosflags(std::ios::left) << std::setw(columns) << std::string(it->begin(), it->end());
			}
			else
				std::cout << std::setiosflags(std::ios::left) << std::setw(columns) << " ";
		}
		std::cout << "> ";

		std::string line;
		std::getline(std::cin, line);
		if (line == "exit")
			isRunning = false;
		else
			consoleLines.push_front(std::wstring(L"[00:00] User: ").append(std::wstring(line.begin(), line.end())));

	}

	return 0;
}<commit_msg>Fixed build error on linux.<commit_after>#include <iostream>
#include <cstdlib>
#include <list>
#include <iomanip>
#include <string>

#if WIN32
	#include <windows.h>
#else
	#include <sys/ioctl.h>
	#include <stdio.h>
	#include <unistd.h>
#endif

int main(int argc, char** argv)
{
	std::list<std::wstring> consoleLines;
	int columns, rows;
	bool isRunning = true;

	consoleLines.push_front(std::wstring(L"[00:00] System: dc-console v1.0"));

	while (isRunning)
	{
#if WIN32
		std::system("cls");

		CONSOLE_SCREEN_BUFFER_INFO csbi;
		GetConsoleScreenBufferInfo(GetStdHandle(STD_OUTPUT_HANDLE), &csbi);
		columns = csbi.srWindow.Right - csbi.srWindow.Left + 1;
		rows = csbi.srWindow.Bottom - csbi.srWindow.Top + 1;
#else
		std::system("clear");

		struct winsize w;
		ioctl(STDOUT_FILENO, TIOCGWINSZ, &w);
		columns = w.ws_col;
		rows = w.ws_row;
#endif

		for (int i = 0; i < rows - 1; ++i)
		{
			if (consoleLines.size() > rows - i - 2)
			{
				std::list<std::wstring>::iterator it = consoleLines.begin();
				std::advance(it, rows - i - 2);

				std::cout << std::setiosflags(std::ios::left) << std::setw(columns) << std::string(it->begin(), it->end());
			}
			else
				std::cout << std::setiosflags(std::ios::left) << std::setw(columns) << " ";
		}
		std::cout << "> ";

		std::string line;
		std::getline(std::cin, line);
		if (line == "exit")
			isRunning = false;
		else
			consoleLines.push_front(std::wstring(L"[00:00] User: ").append(std::wstring(line.begin(), line.end())));

	}

	return 0;
}<|endoftext|>