/home/jgoppert/Projects/uvsim/uvsim/src/uvsim/sensing/Imu6DofSensor.cc

Go to the documentation of this file.

/*
 * Imu6DofSensor.cc
 * Copyright (C) Travis Ramp 2009 <tramp@purdue.edu>
 *
 * Imu6DofSensor.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.
 *
 * Imu6DofSensor.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 "Imu6DofSensor.h"
#include <iostream>
#include <iomanip>
#include "uvsim/utilities/utilities.h"
#include "uvsim/utilities/constants.h"
#include <boost/numeric/ublas/io.hpp>

using namespace boost::numeric::ublas;

namespace uvsim
{

// Communication with Port
Imu6DofSensor::Imu6DofSensor(std::string port, vector<double> freqCutMb,
                             vector<double> freqCutFb, vector<double> freqCutWb):
        fb(3), wb(3), mb(3), fbRaw(3), wbRaw(3), mbRaw(3), freqCutMb(freqCutMb),
        freqCutFb(freqCutFb), freqCutWb(freqCutWb)
{
    isStarted = false;

    // setup serial communication
    serialPort.Open(port);
    serialPort.SetBaudRate(SerialStreamBuf::BAUD_115200);
    serialPort.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
    serialPort.SetParity(SerialStreamBuf::PARITY_NONE);
    serialPort.SetNumOfStopBits(1);
    serialPort.SetFlowControl(SerialStreamBuf::FLOW_CONTROL_NONE);
    serialPort.unsetf(std::ios_base::skipws);
    if (!serialPort.good())
    {
        std::cerr << "Error: IMU Serial port not setup correctly." << std::endl;
        exit(1);
    }
    else
    {
        std::clog << "IMU Serial port setup complete." << std::endl;
    }
}

Imu6DofSensor::~Imu6DofSensor()
{
    if (isStarted) stop();
    serialPort.Close();
}

// Read File
bool Imu6DofSensor::read()
{
    char c, c_prev = 0;
    int A, B;
    unsigned short array[10];
    bool dataOk;

    while (1)
    {
        // Assume data is ok initially
        dataOk = true;

        // Get Data
        c = serialPort.get();

        // Debug: show raw data
        //cout << c << endl;

        // Byte Calculation and Manipulation
        // Byte Range
        if (c_prev == 'Z' && c == 'A')
        {
            for (int i = 0; i < 10; i++)
            {
                // Byte Manipulation
                A = serialPort.get();
                B = serialPort.get();
                array[i] = (A << 8) | B;

                // Test if data in correct range
                if ((i != 0) && array[i] > 1024)
                {
                    std::cout << "rejected: " << i << " : " << array[i] << std::endl;
                    dataOk = false;
                    break;
                }
            }
            if (dataOk)
            {
                // get data
                for (int i=0; i<3; i++)
                {
                    mbRaw(i) = array[1+i];
                    fbRaw(i) = array[4+i];
                    wbRaw(i) = array[7+i];
                }
                return true;
            }
        }
        c_prev = c;
    }
    return false;
}

void Imu6DofSensor::update()
{
    if (!isStarted) std::cerr << "Error: you must start the imu before calling update" << std::endl;
    read();
    lowPass(freq,freqCutMb,mbRaw,mb);
    lowPass(freq,freqCutFb,fbRaw,fb);
    lowPass(freq,freqCutWb,wbRaw,wb);
}

void Imu6DofSensor::stop()
{
    isStarted = false;
    serialPort.write(&stopCmd, 1);
}

void Imu6DofSensor::start(double frequency)
{
    freq = frequency;
    serialPort.write(&exitMenuCmd, 1);
    serialPort.write(&startCmd, 1);
    read();
    mb = mbRaw;
    fb = fbRaw;
    wb = wbRaw;
    isStarted = true;
}

void Imu6DofSensor::print()
{
    std::cout << std::setprecision(3) << "mb: " << mb << " fb: " << fb << " wb: " << wb << std::endl;
}

} // namespace uvsim

// vim:ts=4:sw=4

---