/home/jgoppert/Projects/uvsim/uvsim/src/apps/calibration.cc

Go to the documentation of this file.

/*
 * calibration.cc
 * Copyright (C) Travis Ramp 2009 <rampt@users.sourceforge.net>
 *
 * calibration.cc 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, either version 3 of the License, or
 * (at your option) any later version.
 *
 * calibration.cc 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.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "uvsim/sensing/Imu6DofSensor.h"
#include <iostream>
#include <fstream>
#include <cmath>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include "uvsim/utilities/utilities.h"

using namespace boost::numeric::ublas;

// Class And Variable Declarations
class Calibration
{
private:
    std::string device;                                 // Calibration Device
    int choice;                                                 // Menu Input
    double arm_length;                              // Arm Length
    std::string orientation;                             // Orientation std::string
    int cycle;                                          // Initial Cycle Count (0)
    int numCycles;                                      // End Number Of Cycles
    vector<double>
    sum_mb, sum_fb, sum_wb,                                     // Summers
    sumSq_mb, sumSq_fb, sumSq_wb,                       // Square Summers
    avg_mb, avg_fb, avg_wb,                                     // Averages
    max_mb, min_mb, bias_mb, scale_mb,          // Magnetometer Specific
    std_mb, std_fb, std_wb,                             // Standard Deviations
    freqCutMb, freqCutFb, freqCutWb;                    // Cut off frequencies
    uvsim::Imu6DofSensor * imu;                         // Calibration imu
    std::string notes, file, noteFileName, dataFileName; // Note & File std::strings
        std::ofstream noteFile, dataFile;                       // Output Files
    double freq;                                                                        // Frequency of output
public:
    Calibration();                                      // Constructer
    ~Calibration();                                     // Deconstructer
    void setup();                                       // Setup Program
    void centrifugeCalibration();                   // Run Centrifuge Calibration
    void staticMagnetometerCalibration();           // Run Static Magnetometer Calibration
    void menu();                                        // Menu Program
};

// Constructor
Calibration::Calibration() : sum_mb(3), sum_fb(3), sum_wb(3), sumSq_mb(3),
        sumSq_fb(3), sumSq_wb(3), avg_mb(3), avg_fb(3), avg_wb(3), max_mb(3),
        min_mb(3), bias_mb(3), scale_mb(3), std_mb(3), std_fb(3), std_wb(3),
        freqCutMb(3), freqCutFb(3), freqCutWb(3), imu()
{
    // set default
    numCycles = 10000;
    device = "/dev/rfcomm0";
    file = "test";
    freq = 500;
    freqCutMb(0) = freqCutMb(1) = freqCutMb(2) = 1;
    freqCutFb(0) = freqCutFb(1) = 350;
    freqCutFb(2) = 150;
    freqCutWb(0) = freqCutWb(1) = freqCutWb(2) = 120;
    arm_length = 0.7;
    orientation = "+X";
}

// Deconstructor
Calibration::~Calibration()
{
    // Delete imu Connection
    if (imu) delete imu;

    // Close All Files
    if (noteFile.is_open()) noteFile.close();
    if (dataFile.is_open()) dataFile.close();
}

// User Menu
void Calibration::menu()
{
    while (1)
    {
        uvsim::clear();
        std::cout << "UVSIM IMU CALIBRATION APPLICATION" << std::endl;
        std::cout << "\nCurrent Settings " << std::endl;
        std::cout << "------------------------------------------------------------------" << std::endl;
        std::cout << "Number of Cycles: "<< numCycles << std::endl;
        std::cout << "Device: " << device << std::endl;
        std::cout << "Output File: " << file << std::endl;
        std::cout << "Output Frequency: " <<  freq << std::endl;
        std::cout << "Arm Length: " << arm_length << std::endl;
        std::cout << "Orientation: " << orientation << std::endl;
        std::cout << "Magnetometer Cutoff Frequencies: " << freqCutMb << std::endl;
        std::cout << "Accelerometer Cutoff Frequencies: " << freqCutFb << std::endl;
        std::cout << "Gyro Cutoff Frequencies: " << freqCutWb << std::endl;
        std::cout << "\nMenu" << std::endl;
        std::cout << "------------------------------------------------------------------" << std::endl;
        std::cout << "1) Change Settings" << std::endl;
        std::cout << "2) Run Centrifuge Calibration" << std::endl;
        std::cout << "3) Run Magnetometer Static Calibration" << std::endl;
        std::cout << "4) Quit" << std::endl;
        std::cout << "\nSelection: ";
        std::cin >> choice;
        std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
        switch (choice)
        {
        case 1:
            setup();
            break;
        case 2:
            centrifugeCalibration();
            break;
        case 3:
            staticMagnetometerCalibration();
            break;
        case 4:
            std::cout << "Exiting application." << std::endl;
            return;
        default:
            std::cout << "Unknown selection." << std::endl;
            break;
        }
        std::cout << "\nPress <enter> to continue ..." << std::endl;
        std::cin.get();
    }
}

// Setup Program
void Calibration::setup ()
{
    // User Inputs
    std::cout << "Input Directions" << std::endl;
    std::cout << "------------------------------------------------------------------" << std::endl;
    std::cout << "For all vectors the following format is required: [3](x, y, z)" << std::endl;
    std::cout << "Press enter to accept default shown in {}" << std::endl;

    std::cout << "\nSettings" << std::endl;
    std::cout << "------------------------------------------------------------------" << std::endl;
    std::cout << "How many readings to be averaged {" << numCycles << "} : ";
    if (std::cin.peek() != '\n') std::cin >>  numCycles;
    else std::cin.get();

    std::cout << "Device {" << device << "} : ";
    if (std::cin.peek() != '\n') std::cin >> device;
    else std::cin.get();

    std::cout << "Output file {" << file << "} : ";
    if (std::cin.peek() != '\n') std::cin >> file;
    else std::cin.get();

    std::cout << "Output Frequency Hz {" << freq << "} : ";
    if (std::cin.peek() != '\n') std::cin >> freq;
    else std::cin.get();

    std::cout << "Arm Length {" << arm_length << "} : ";
    if (std::cin.peek() != '\n') std::cin >> arm_length;
    else std::cin.get();

    std::cout << "Orientation {" << orientation << "} : ";
    if (std::cin.peek() != '\n') std::cin >> orientation;
    else std::cin.get();

    std::cout << "Magnetometer Cutoff Frequency {" << freqCutMb << "} : ";
    if (std::cin.peek() != '\n') std::cin >> freqCutMb;
    else std::cin.get();

    std::cout << "Accelerometer Cutoff Frequency {" << freqCutFb << "} : ";
    if (std::cin.peek() != '\n') std::cin >> freqCutFb;
    else std::cin.get();

    std::cout << "Gyro Cutoff Frequency {" << freqCutWb << "} : ";
    if (std::cin.peek() != '\n') std::cin >> freqCutWb;
    else std::cin.get();


}

// Run Centrifuge Calibration Cycle
void Calibration::centrifugeCalibration()
{
    // Open Notes File
    if (noteFile.is_open()) noteFile.close();
    noteFileName = file + ".notes";
    noteFile.open(noteFileName.c_str());

    // Open Data File
    if (dataFile.is_open()) dataFile.close();
    dataFileName = file + ".data";
    dataFile.open(dataFileName.c_str());

    // User Inputs
    std::cout << "\nCentrifuge Calibration Started" << std::endl;
    std::cout << "------------------------------------------------------------------" << std::endl;
    std::cout << "Please input frequency and voltage seperated with a space: ";
    getline(std::cin,notes);

    // imu Connectioon
    std::cout << "Waiting for imu to connect via device: " << device << std::endl;
    imu = new uvsim::Imu6DofSensor(device, freqCutMb, freqCutFb, freqCutWb);

    // imu Initialization
    imu->start(freq);

    // Initialize Cycle
    imu->update();
    max_mb = imu->mb;
    min_mb = imu->mb;
    sum_mb = sum_fb = sum_wb = sumSq_mb = sumSq_fb = sumSq_wb = zero_vector<double>(3);

    // Cycle Loop
    for (int cycle = 0; cycle<numCycles; cycle++)
    {
        // Status
        uvsim::clear();
        std::cout << 100.*(cycle+1)/numCycles << "% Complete" << std::endl;

        // Imu Update
        imu->update();

        // Max/ Min
        for (int i=0;i<3;i++)
        {
            // Max/ Min for Magenetometer
            if (imu->mb(i) > max_mb(i)) max_mb(i) = imu->mb(i);
            if (imu->mb(i) < min_mb(i)) min_mb(i) = imu->mb(i);
        }

        // Sums
        sum_mb=sum_mb+imu->mb;
        sum_fb=sum_fb+imu->fb;
        sum_wb=sum_wb+imu->wb;

        // Square Sums
        sumSq_mb=element_prod(imu->mb,imu->mb)+sumSq_mb;
        sumSq_fb=element_prod(imu->fb,imu->fb)+sumSq_fb;
        sumSq_wb=element_prod(imu->wb,imu->wb)+sumSq_wb;
    }

    // Stop imu Data Flow
    imu->stop();

    // Averages
    avg_mb = sum_mb/numCycles;
    avg_fb = sum_fb/numCycles;
    avg_wb = sum_wb/numCycles;

    // Bias
    bias_mb = (max_mb + min_mb)/2;

    // Standard Deviations
    for (int i=0;i<3;i++)
    {
        // Standard Deviation
        std_mb[i]=sqrt((sumSq_mb[i]/numCycles)-(avg_mb[i]*avg_mb[i]));
        std_fb[i]=sqrt((sumSq_fb[i]/numCycles)-(avg_fb[i]*avg_fb[i]));
        std_wb[i]=sqrt((sumSq_wb[i]/numCycles)-(avg_wb[i]*avg_wb[i]));
    }

    // Terminal Output
    std::cout << "\nCentrifuge Calibration Results" << std::endl;
    std::cout << "------------------------------------------------------------------\n" << std::endl;
    std::cout << "Frequency & Voltage: " << notes << std::endl;
    std::cout << std::endl;
    std::cout << "Magnetometer" << std::endl;
    std::cout << "\tAverages: " << avg_mb << std::endl;
    std::cout << "\tStandard Deviations: " << std_mb << std::endl;
    std::cout << "\tMax: " << max_mb << std::endl;
    std::cout << "\tMin: " << min_mb << std::endl;
    std::cout << "\tBias: " << bias_mb << std::endl;
    std::cout << std::endl;
    std::cout << "Accelerometer" << std::endl;
    std::cout << "\tAverages: " << avg_fb << std::endl;
    std::cout << "\tStandard Deviations: " << std_fb << std::endl;
    std::cout << std::endl;
    std::cout << "Gyros" << std::endl;
    std::cout << "\tAverages: " << avg_wb << std::endl;
    std::cout << "\tStandard Deviations: " << std_wb << std::endl;

    // Text Output
    noteFile << orientation << " " << arm_length << " " << numCycles << " " << notes << std::endl;
    dataFile <<  avg_mb << " " << avg_fb << " " << avg_wb << std::endl;
}

// Run Static Magnetometer Calibration Cycle
void Calibration::staticMagnetometerCalibration()
{
    // Open Data File
    if (dataFile.is_open()) dataFile.close();
    dataFileName = file + ".data";
    dataFile.open(dataFileName.c_str());

    // User Inputs
    std::cout << "\nStatic Magnetometer Calibration Started" << std::endl;
    std::cout << "------------------------------------------------------------------\n" << std::endl;

    // imu Connection
    std::cout << "Waiting for imu to connect via device: " << device << std::endl;
    imu = new uvsim::Imu6DofSensor(device, freqCutMb, freqCutFb, freqCutWb);

    // imu Initialization
    imu->start(freq);

    // Initialize Cycle
    imu->update();
    max_mb = imu->mb;
    min_mb = imu->mb;
    sum_mb = sum_fb = sum_wb = sumSq_mb = sumSq_fb = sumSq_wb = zero_vector<double>(3);

    // Cycle Loop
    for (int cycle = 0; cycle<numCycles; cycle++)
    {
        // Status
        uvsim::clear();
        std::cout << 100.*(cycle+1)/numCycles << "% Complete" << std::endl;

        // Imu Update
        imu->update();

        // Max/ Min
        for (int i=0;i<3;i++)
        {
            // Max/ Min for Magenetometer
            if (imu->mb(i) > max_mb(i)) max_mb(i) = imu->mb(i);
            if (imu->mb(i) < min_mb(i)) min_mb(i) = imu->mb(i);
        }

        // Sums
        sum_mb=sum_mb+imu->mb;
        sum_fb=sum_fb+imu->fb;
        sum_wb=sum_wb+imu->wb;

        // Square Sums
        sumSq_mb=element_prod(imu->mb,imu->mb)+sumSq_mb;
        sumSq_fb=element_prod(imu->fb,imu->fb)+sumSq_fb;
        sumSq_wb=element_prod(imu->wb,imu->wb)+sumSq_wb;
    }

    // Stop imu Data Flow
    imu->stop();

    // Averages
    avg_mb = sum_mb/numCycles;
    avg_fb = sum_fb/numCycles;
    avg_wb = sum_wb/numCycles;

    // Bias and Scale
    for (int i=0; i< max_mb.size(); i++)
    {
        scale_mb(i) = 2/(max_mb(i)-min_mb(i));
        bias_mb(i) = 1-scale_mb(i)*max_mb(i);
    }

    // Standard Deviations
    for (int i=0;i<3;i++)
    {
        // Standard Deviation
        std_mb[i]=sqrt((sumSq_mb[i]/numCycles)-(avg_mb[i]*avg_mb[i]));
        std_fb[i]=sqrt((sumSq_fb[i]/numCycles)-(avg_fb[i]*avg_fb[i]));
        std_wb[i]=sqrt((sumSq_wb[i]/numCycles)-(avg_wb[i]*avg_wb[i]));
    }

    // Terminal Output
    std::cout << "\nStatic Magnetometer Results" << std::endl;
    std::cout << "------------------------------------------------------------------\n" << std::endl;
    std::cout << "Magnetometer" << std::endl;
    std::cout << "\tAverages: " << avg_mb << std::endl;
    std::cout << "\tStandard Deviations: " << std_mb << std::endl;
    std::cout << "\tMax: " << max_mb << std::endl;
    std::cout << "\tMin: " << min_mb << std::endl;
    std::cout << "\tBias: " << bias_mb << std::endl;
    std::cout << "\tScale: " << scale_mb << std::endl;

    // Text Output
    for (int i=0; i<scale_mb.size(); i++)
        dataFile << scale_mb(i) << " " << -bias_mb(i) << std::endl;
}

// Main Program
int main()
{
    Calibration calibration;
    calibration.menu();
    return 0;
}

// vim:ts=4:sw=4

---