ftp.hitl.washington.edu/ftp.hitl.washington.edu.tar/ftp.hitl.washington.edu/pub/people/peter/ER/BP_driver.cxx

C/C++ Source or Header | 1998-07-07 | 5KB | 210 lines

/*
 BP_driver.cc
*/

#include "BP_driver.h"

#include <iostream.h>
#include <vector.h>
#include <math.h>

#include "v3dSystem.h"

///////////////////////////
//    BPDriver    //
/////////////////////////

//const double BPDriver::ms_Interval = 25.0;    // 25 ms per interval
const double BPDriver::ms_Interval = 40.0;    // 40 ms per interval

vector <vector <BP_DATA_TYPE> >    test_BP_values;

template<class T>
vector<T> to_vec (int num_values, T *data_ary)
{
        vector<T> new_vec;
        for (int i = 0; i < num_values; ++i)
                new_vec.push_back (data_ary[i]);
        return new_vec;
}

#include <stdio.h>

bool
BPDriver::read_data (char *filename)
{
    FILE *fp = fopen(filename, "r");
    if (fp == NULL) {
        cerr << "BPDriver::read_data: Error opening file: " <<
            filename << endl;
        return false;
    }

    BP_DATA_TYPE    alt;

    vector<BP_DATA_TYPE> alt_vec;

    while (!feof(fp)) {
        fscanf (fp, "%f", &alt);
        if (feof(fp))
            break;
        alt_vec.push_back(alt);
    }

    test_BP_values.push_back(alt_vec);

    fclose (fp);

    return true;
}

BPDriver::BPDriver(char* data_filename)
{
    read_data (data_filename);

    data_list = 0;

    switch_data (0);

    // Register this object's event handler with the system loop.
    // This is for demo purposes only.
    RegisterEventHandler(v3dSystem::GetInstance(),
        v3dSystem::SYSTEM_SIMULATION_LOOP, &BP_update_callback, 0);

    paused_ = false;
}

BPDriver::~BPDriver()
{
    delete data_list;
}

void
BPDriver::Pause()
{
    if (!paused_) {
        // Unregister this object's event handler with the system loop.
        // This is for demo purposes only.
//        UnregisterEventHandler(v3dSystem::GetInstance(),
//            v3dSystem::SYSTEM_SIMULATION_LOOP, &BP_update_callback);
        paused_ = true;
    }
}

void
BPDriver::Unpause()
{
    if (paused_) {
        // Re-register this object's event handler with the system loop.
        // This is for demo purposes only.
//        RegisterEventHandler(v3dSystem::GetInstance(),
//            v3dSystem::SYSTEM_SIMULATION_LOOP, &BP_update_callback, 0);
        paused_ = false;
    }
}

BP_DATA_RANGE
BPDriver::switch_data (int set_no)
{
    // A negative set number parameter increments the current set number.
    if (set_no < 0)
        ++current_set_;
    else
        current_set_ = set_no;

    // Choose set 0 if the specified set is out of range.
    if (current_set_ >= test_BP_values.size())
        current_set_ = 0;

    int    set_size = test_BP_values[current_set_].size(),
        num_cycles = 100 / set_size + 1;
    max_index_ = set_size * num_cycles;
cerr << "Set size: " << set_size << " Number of cycles: " << num_cycles <<
    " Max index: " << max_index_ << endl;

    // Allocate a new array to hold the data values, and copy them from
    // the vector of values.
    delete [] data_list;

    data_list = new BP_DATA_TYPE [max_index_];

    for (int j = 0; j < max_index_; ++j)
        data_list[j] = test_BP_values[current_set_][j % set_size];

    // Set the index to the start of the data.
    reset_current_index();

    // Get the ranges of the data.
    data_range_.maximum_value = data_list[0];
    data_range_.minimum_value = data_list[0];
    for (int i = 1; i < set_size; ++i) {
        if (data_list[i] > data_range_.maximum_value)
            data_range_.maximum_value = data_list[i];
        if (data_list[i] < data_range_.minimum_value)
            data_range_.minimum_value = data_list[i];
    }

    return data_range_;
}

void
BPDriver::BP_update_callback(const v3dEventMessage *message)
{
    ((BPDriver *)message->GetEventReceiver())->update_current_index();
}

// usec_diff
//    Calculate the difference in microseconds between the two times
//    passed in.
inline float
usec_diff (const timeval& current_time, const timeval& past_time)
{
    return (current_time.tv_sec - past_time.tv_sec) * 1000000 +
        current_time.tv_usec - past_time.tv_usec;
}

// BPDriver::update_current_index
//    Changes the current index to the BP data depending on how much
//    time has elapsed since the last update.
//    Each value in the array represents a piece of BP data read at
//    40ms intervals.
int
BPDriver::update_current_index ()
{
    if (IsPaused()) return current_data_;

    timeval current_time;

//cerr << "clock / sec: " << CLOCKS_PER_SEC;
    gettimeofday (¤t_time);
//cerr << " elapsed clock: " << current_time.tv_usec - last_time_.tv_usec;
//cerr << " elapsed int: " << ((current_time.tv_sec - last_time_.tv_sec) *
//        1000000 + current_time.tv_usec - last_time_.tv_usec)
//         / (1000 * ms_Interval);
//cerr << endl;

//    true_index_ += ((current_time.tv_sec - last_time_.tv_sec) * 1000000 +
//        current_time.tv_usec - last_time_.tv_usec) /
//        (1000 * ms_Interval);
    true_index_ += usec_diff (current_time, last_time_) /
        (1000 * ms_Interval);
    while (true_index_ >= max_index_)
        true_index_ -= max_index_;
    current_data_ = (int) ffloor (true_index_);
    last_time_ = current_time;

//cerr << " true index: " << true_index_;
//cerr << " current index: " << current_data_ << endl;
    return current_data_;
}

int
BPDriver::reset_current_index ()
{
//    last_time_ = clock();
    gettimeofday(&last_time_);
    true_index_ = 0;
    return current_data_ = 0;
}