Chordata_timer.cpp 5.03 KB
Newer Older
Bruno Laurencich's avatar
Bruno Laurencich committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46
/**
 * @file Chordata_timer.cpp
 * Time-handlind classes: Scheduler to organize the reading of the nodes, and Timer to launch each read. 
 *
 * @author Bruno Laurencich
 * @version 0.1.0 
 * @date 2018/08/20
 *
 * Notochord  
 * -- Hub program for the Chordata Open Source motion capture system
 *
 * http://chordata.cc
 * contact@chordata.cc
 *
 *
 * Copyright 2018 Bruno Laurencich
 *
 * This file is part of Notochord.
 *
 * Notochord 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.
 *
 * Notochord 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 Notochord. If not, see <https://www.gnu.org/licenses/>.
 *
 * This program uses code from various sources, the default license is GNU GPLv3
 * for all code, the dependencies where originally distributed as follows:
 * -LSM9DS# libraries: Copyright 2014,2015 Jim Lindblom @ SparkFun Electronics, as beerware.
 * -fmt library: Copyright (c) 2012 - present, Victor Zverovich, Under BSD 2-clause License.
 * -spdlog library: Copyright(c) 2015 Gabi Melman, under MIT license.
 * -args library: Copyright (c) 2016 Taylor C. Richberger, under MIT license.
 * -bcm2835 library: Copyright 2011-2013 Mike McCaule, Under GNU-GPLv2 License.
 * -oscpack library: Copyright (c) 2004-2013 Ross Bencina, under MIT license.
 * -Sensor fusion algorithms: Copyright 2011 SOH Madgwick, Under GNU-GPLv3 License.
 * -tinyxml2 library: Copyright <unknowk year> Lee Thomason, under zlib License.
 * -pstreams library: Copyright (C) 2001 - 2017 Jonathan Wakely, under Boost Software License
 *
 */

Bruno Laurencich's avatar
Bruno Laurencich committed
47
#include "Chordata_communicator.h"
Bruno Laurencich's avatar
Bruno Laurencich committed
48 49 50 51 52 53
#include "Chordata_timer.h"
#include "Chordata_def.h"
#include "Chordata_utils.h"
#include "Chordata_node.h"
#include <iostream>

Bruno Laurencich's avatar
Bruno Laurencich committed
54
namespace comm = Chordata::Communicator;
Bruno Laurencich's avatar
Bruno Laurencich committed
55 56 57

using namespace std;
using namespace Chordata;
58
using scheduler_citer = std::vector<I2C_Node*>::const_iterator;
Bruno Laurencich's avatar
Bruno Laurencich committed
59

Bruno Laurencich's avatar
Bruno Laurencich committed
60 61
#define LAP_MODIF_RATE 10
#define LAP_TOLERANCE 100
62
#define LAP_CORRECT_TIME_MARGIN 10*1000
Bruno Laurencich's avatar
Bruno Laurencich committed
63

Bruno Laurencich's avatar
Bruno Laurencich committed
64 65 66 67 68 69 70 71 72
void Node_Scheluder::addNode(I2C_Node *n){
		nodelist.push_back(n);
		current = nodelist.end()-1;

		if (n->isSensor())
			ODRModifiers++;
		
	};

73 74 75 76 77 78
const scheduler_citer seek_first_sensor(const std::vector<I2C_Node*>& list){
	for (scheduler_citer i = list.begin(); i != list.end(); ++i){
		if ((*i)->isSensor()) return i;
	}
}

Bruno Laurencich's avatar
Bruno Laurencich committed
79

Bruno Laurencich's avatar
Bruno Laurencich committed
80 81
long int Node_Scheluder::bang(long int wait){
	//not threaded at the moment
Bruno Laurencich's avatar
Bruno Laurencich committed
82 83 84
	// thread nodeSearch(&Node_Scheluder::doSearch, this);
	// nodeSearch.detach();
	
85
	static long int lap_teoric_duration = 1000000 / Chordata::getConf().odr;
Bruno Laurencich's avatar
Bruno Laurencich committed
86
	static long int teoric_wait = lap_teoric_duration / getODRModifiers();
87 88 89
	static auto first_sensor = seek_first_sensor(nodelist);
	
	if (current == first_sensor){
Bruno Laurencich's avatar
Bruno Laurencich committed
90 91 92
		//Check if lap duration is correct and adjust;
		
		auto lap  = comm::timekeeper.ellapsedLapMicros();
93
		comm::trace(" ===== Ellapsed Lap Micros: {} =====", lap);
Bruno Laurencich's avatar
Bruno Laurencich committed
94
		comm::timekeeper.resetLap();
95
		
Bruno Laurencich's avatar
Bruno Laurencich committed
96
		if (comm::timekeeper.ellapsedMillis() > LAP_CORRECT_TIME_MARGIN ){
97 98 99
			//Leave the first second of execution untouched
			//after that start cheking if the lap has to be increased or decreased
			
Bruno Laurencich's avatar
Bruno Laurencich committed
100 101 102 103 104 105 106 107 108 109
			if (lap > lap_teoric_duration + LAP_TOLERANCE){
				wait -= LAP_MODIF_RATE;
				comm::debug(" (- ) Read wait reduced, now is {} (teoric = {}, [Lap measured: {}, Lap teoric: {}])", 
					wait, teoric_wait,
					lap, lap_teoric_duration);
			} else if (lap < lap_teoric_duration - LAP_TOLERANCE){
				wait += LAP_MODIF_RATE;
				comm::debug(" ( +) Read wait increased, now is {} (teoric = {}, [Lap measured: {}, Lap teoric: {}])", 
					wait, teoric_wait,
					lap, lap_teoric_duration);
110
					
Bruno Laurencich's avatar
Bruno Laurencich committed
111
			}
Bruno Laurencich's avatar
Bruno Laurencich committed
112
		}
113
	} // End lap duration cheking
114
	
Bruno Laurencich's avatar
Bruno Laurencich committed
115
	doSearch();
Bruno Laurencich's avatar
Bruno Laurencich committed
116
	return wait;
Bruno Laurencich's avatar
Bruno Laurencich committed
117 118 119 120 121 122 123 124 125 126
}			

void Node_Scheluder::doSearch(){
	//not threaded at the moment
	// mp->lock();
		// cout << "LABEL: " << (*current)->getLabel() << endl;
		main_buffer.put(*current);
	// mp->unlock();
	step();

127
	while (!(*current)->isSensor()){
Bruno Laurencich's avatar
Bruno Laurencich committed
128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169
		main_buffer.put(*current);
		step();
	}

	//TODO: at this point inform another thread, and it will "eat" the nodes in the buffer.
	//This other thread will be responsable for checking eventual throws of the banged node.
	main_buffer.eat();
	
}


////////////////////////////////// 
/// for testing individual utils
//////////////////////////////////

#ifdef __TEST_TIMER__
int main(int argc, char const *argv[])
{	
	typedef Timer<Node_Scheluder> Timer_Sch;
	
	Node_Scheluder n;
	mutex *m = n.getMutex();
	Timer_Sch t(&n, 50000, m);

	// thread contador(&Timer_Sch::startTimer, t );
	thread contador([&]{t.startTimer();} );
	

	// while(1){
	// 		m->lock();
	// 			cout<< "MAIN THREAD MUTEX: " << m << endl;
	// 		m->unlock();
	// 		thread_sleep(micros(1000000));
	// }

	if(contador.joinable())
		contador.join();

	return 0;
}

#endif