Commit 233a7ed7 authored by ARnaud Blanchard's avatar ARnaud Blanchard
Browse files

Improve time management

parent 505e6d55
#include "bl_raspi.h" #include "bl_raspi.h"
#include "blc_channel.h"
#include "bl_raspi_core.h" #include "bl_raspi_core.h"
#include "blc_core.h" #include "blc_core.h"
//#include "mmapGpio.h"
#include <sys/ioctl.h> //ioctl #include <sys/ioctl.h> //ioctl
#include <sys/mman.h> #include <sys/mman.h>
#include <fcntl.h> //O_RDWR, ... #include <fcntl.h> //O_RDWR, ...
...@@ -16,15 +16,20 @@ ...@@ -16,15 +16,20 @@
#include <vector> #include <vector>
static const int DEBUG_BUFFER_SIZE=256;
#define DEBUG_VALUE(value) do{debug_channel.floats[debug_index++]=value; if (debug_index==DEBUG_BUFFER_SIZE) debug_index=0;}while(0)
#define FLAG(pos) (1<<pos) #define FLAG(pos) (1<<pos)
#define DELTA_NS(current_ns, previous_ns) ((previous_ns > current_ns) ? 1000000000-previous_ns+current_ns : current_ns-previous_ns) #define DELTA_NS(current_ns, previous_ns) ((previous_ns > current_ns) ? 1000000000-previous_ns+current_ns : current_ns-previous_ns)
using namespace std; using namespace std;
uint32_t it_since_clock=0; uint32_t it_since_clock=0;
uint32_t current_ns, previous_ns; uint32_t current_ns, previous_ns;
float ns_per_it=1; float ns_per_it=1, previous_ns_per_it, var_ns_per_it;
uint32_t min_ns_per_it=1000; uint32_t min_ns_per_it=1000;
bl_raspi raspi; bl_raspi raspi;
vector <Ohm_input> ohm_inputs; vector <Ohm_input> ohm_inputs;
...@@ -33,40 +38,62 @@ int run_loop=1; ...@@ -33,40 +38,62 @@ int run_loop=1;
volatile int bl_raspi_iterations_nb; volatile int bl_raspi_iterations_nb;
int buffer_size; int buffer_size;
blc_channel debug_channel;
int debug_index=0;
inline void update_time(){ inline void update_time(){
struct timespec timestamp; struct timespec timestamp;
// current_ns_per_it;
float current_ns_per_it, error;
int32_t delta_ns; int32_t delta_ns;
clock_gettime(CLOCK_MONOTONIC, &timestamp); clock_gettime(CLOCK_MONOTONIC, &timestamp);
current_ns=timestamp.tv_nsec; current_ns=timestamp.tv_nsec;
delta_ns=DELTA_NS(current_ns, previous_ns); delta_ns=DELTA_NS(current_ns, previous_ns);
if (it_since_clock){ if (it_since_clock){
ns_per_it=(float)delta_ns/(float)it_since_clock; current_ns_per_it=(delta_ns+1)/it_since_clock;
// ns_per_it+=(((float)delta_ns/(float)it_since_clock)-ns_per_it)/100.f; // if (current_ns_per_it/ns_per_it)
error=ns_per_it-current_ns_per_it;
if (error*error < var_ns_per_it) ns_per_it+=(current_ns_per_it-ns_per_it+1)/2;
var_ns_per_it+=(error*error - var_ns_per_it+1)/2;
// ns_per_it+=(((float)delta_ns/(float)it_since_clock)-ns_per_it)/100.f;
it_since_clock=0; it_since_clock=0;
} }
previous_ns=current_ns; previous_ns=current_ns;
DEBUG_VALUE(ns_per_it);
// DEBUG_VALUE(error*error);
// DEBUG_VALUE(var_ns_per_it);
for( auto &input : ohm_inputs){ for( auto &input : ohm_inputs){
// DEBUG_VALUE(input.activated);
if (input.activated){ if (input.activated){
delta_ns=DELTA_NS(current_ns, input.spike_ns); delta_ns=DELTA_NS(current_ns, input.spike_ns);
if (delta_ns > input.tau) input.activated=1;
else input.activated=(input.tau-delta_ns)/ns_per_it; // DEBUG_VALUE(delta_ns);
// DEBUG_VALUE(input.tau);
if (delta_ns >= input.tau) input.activated=1;
else input.activated=(input.tau-delta_ns)/ns_per_it+1;
} }
} }
// fprintf(stderr, "%d %f\n", delta_ns, ns_per_it);
} }
Ohm_input::Ohm_input(int pin, int us_tau, uint32_t *buffer):index(0), flag(FLAG(pin)), pin(pin), buffer(buffer), activated(us_tau*1000){ Ohm_input::Ohm_input(int pin, int us_tau, uint32_t *buffer):index(0), flag(FLAG(pin)), pin(pin), buffer(buffer), activated(us_tau*1000){
tau=300000;
} }
inline void Ohm_input::rest(){ inline void Ohm_input::rest(){
raspi.set_pin_dir(pin, raspi.INPUT); raspi.set_pin_dir(pin, raspi.INPUT);
update_time(); update_time();
buffer[index]=DELTA_NS(current_ns, spike_ns); buffer[index]=current_ns;
index++; index++;
if (index==buffer_size) index=0; if (index==buffer_size) index=0;
rest_ns=current_ns; rest_ns=current_ns;
...@@ -74,7 +101,6 @@ inline void Ohm_input::rest(){ ...@@ -74,7 +101,6 @@ inline void Ohm_input::rest(){
} }
inline void Ohm_input::spike(){ inline void Ohm_input::spike(){
raspi.set_pin_dir(pin, raspi.OUTPUT); raspi.set_pin_dir(pin, raspi.OUTPUT);
tau=50000;
update_time(); update_time();
spike_ns=current_ns; spike_ns=current_ns;
buffer[index]=DELTA_NS(current_ns, rest_ns); buffer[index]=DELTA_NS(current_ns, rest_ns);
...@@ -116,7 +142,6 @@ void *thread_loop(void*){ ...@@ -116,7 +142,6 @@ void *thread_loop(void*){
*raspi.gpfset0|=input.flag; //Set pin high *raspi.gpfset0|=input.flag; //Set pin high
raspi.set_pin_mode(input.pin, raspi.PULL_OFF); raspi.set_pin_mode(input.pin, raspi.PULL_OFF);
raspi.set_pin_dir(input.pin, raspi.OUTPUT); raspi.set_pin_dir(input.pin, raspi.OUTPUT);
input.stop_ns=current_ns+input.tau*1000;
} }
for(auto &output : volt_outputs){ for(auto &output : volt_outputs){
*raspi.gpfset0|=output.flag; //Set pin high *raspi.gpfset0|=output.flag; //Set pin high
...@@ -126,7 +151,7 @@ void *thread_loop(void*){ ...@@ -126,7 +151,7 @@ void *thread_loop(void*){
uint32_t results; uint32_t results;
while(run_loop){ while(run_loop){
// results =*(raspi.gplev0); // results =*(raspi.gplev0);
for(auto &input : ohm_inputs){ for(auto &input : ohm_inputs){
if (input.activated) { if (input.activated) {
input.activated--; input.activated--;
...@@ -142,37 +167,37 @@ void *thread_loop(void*){ ...@@ -142,37 +167,37 @@ void *thread_loop(void*){
} }
//Output volts //Output volts
// FOR (i, bl_raspi_volt_outputs_nb){ // FOR (i, bl_raspi_volt_outputs_nb){
/*COmputing current ( not yet working) /*COmputing current ( not yet working)
if (volt_outputs[0].activated_input==0 && ((result & volt_outputs[0].flag)==0)){ if (volt_outputs[0].activated_input==0 && ((result & volt_outputs[0].flag)==0)){
gpio_channel.floats[6]=current_ns-volt_outputs[0].ns_timestamps_input; gpio_channel.floats[6]=current_ns-volt_outputs[0].ns_timestamps_input;
if (iterations_nb%100==0){ if (iterations_nb%100==0){
fwrite(&current_ns, sizeof(float), 1, volt_outputs[0].file_input); fwrite(&current_ns, sizeof(float), 1, volt_outputs[0].file_input);
fwrite(&gpio_channel.floats[6], sizeof(float), 1, volt_outputs[0].file_input); fwrite(&gpio_channel.floats[6], sizeof(float), 1, volt_outputs[0].file_input);
} }
volt_outputs[0].activated_input=1; volt_outputs[0].activated_input=1;
}*/ }*/
/* delta_ns=current_ns-volt_outputs[i].us_timestamps; /* delta_ns=current_ns-volt_outputs[i].us_timestamps;
if (delta_ns > 10){ if (delta_ns > 10){
volt_outputs[i].average+=(volt_outputs[i].activated-volt_outputs[i].average)*(delta_ns/100); volt_outputs[i].average+=(volt_outputs[i].activated-volt_outputs[i].average)*(delta_ns/100);
if (bl_raspi_volt_outputs[i] > rand()/(RAND_MAX/255)){// volt_outputs[i].average*100){ //Comparing average time up with the requested one//){ if (bl_raspi_volt_outputs[i] > rand()/(RAND_MAX/255)){// volt_outputs[i].average*100){ //Comparing average time up with the requested one//){
raspi.set_pin_dir(volt_outputs[i].pin, raspi.OUTPUT); raspi.set_pin_dir(volt_outputs[i].pin, raspi.OUTPUT);
volt_outputs[i].activated=1; volt_outputs[i].activated=1;
} }
else{ else{
if (volt_outputs[i].activated==1){ if (volt_outputs[i].activated==1){
volt_outputs[i].activated_input=0; volt_outputs[i].activated_input=0;
volt_outputs[i].us_timestamps_input=current_ns; volt_outputs[i].us_timestamps_input=current_ns;
raspi.set_pin_dir(volt_outputs[i].pin, raspi.INPUT); raspi.set_pin_dir(volt_outputs[i].pin, raspi.INPUT);
volt_outputs[i].activated=0; volt_outputs[i].activated=0;
} }
} }
volt_outputs[i].us_timestamps=current_ns; volt_outputs[i].us_timestamps=current_ns;
} }
}*/ }*/
bl_raspi_iterations_nb++; bl_raspi_iterations_nb++;
it_since_clock++; it_since_clock++;
if (it_since_clock == 1024) update_time(); if (it_since_clock == 32) update_time();
} }
//Stop voltages before living for security //Stop voltages before living for security
for(auto &output : volt_outputs) raspi.set_pin_dir(output.pin, raspi.INPUT); for(auto &output : volt_outputs) raspi.set_pin_dir(output.pin, raspi.INPUT);
...@@ -186,10 +211,13 @@ void bl_raspi_start_loop(uint32_t *RC_results, uchar const *voltage_commands){ ...@@ -186,10 +211,13 @@ void bl_raspi_start_loop(uint32_t *RC_results, uchar const *voltage_commands){
struct sched_param sched_param; struct sched_param sched_param;
run_loop=1; run_loop=1;
/* if (RC_results==NULL) bl_raspi_RC_inputs=MANY_ALLOCATIONS(ohm_inputs.size(), uint32_t); debug_channel.create_or_open("/raspinobo.debug", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, DEBUG_BUFFER_SIZE);
else bl_raspi_RC_inputs=RC_results;
if (voltage_commands==NULL) bl_raspi_volt_outputs=MANY_ALLOCATIONS(bl_raspi_volt_outputs_nb, uchar);
else bl_raspi_volt_outputs=voltage_commands;*/ /* if (RC_results==NULL) bl_raspi_RC_inputs=MANY_ALLOCATIONS(ohm_inputs.size(), uint32_t);
else bl_raspi_RC_inputs=RC_results;
if (voltage_commands==NULL) bl_raspi_volt_outputs=MANY_ALLOCATIONS(bl_raspi_volt_outputs_nb, uchar);
else bl_raspi_volt_outputs=voltage_commands;*/
/* BLC_PTHREAD_CHECK(pthread_attr_init(&attr), "Init pthread attribute"); /* BLC_PTHREAD_CHECK(pthread_attr_init(&attr), "Init pthread attribute");
BLC_PTHREAD_CHECK(pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED), NULL); BLC_PTHREAD_CHECK(pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED), NULL);
...@@ -202,3 +230,4 @@ void bl_raspi_start_loop(uint32_t *RC_results, uchar const *voltage_commands){ ...@@ -202,3 +230,4 @@ void bl_raspi_start_loop(uint32_t *RC_results, uchar const *voltage_commands){
void bl_raspi_stop_loop(){ void bl_raspi_stop_loop(){
run_loop=0; run_loop=0;
} }
...@@ -65,7 +65,7 @@ int main(int argc, char **argv){ ...@@ -65,7 +65,7 @@ int main(int argc, char **argv){
// target_channel.create_or_open(target_channel_name, BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 1); // target_channel.create_or_open(target_channel_name, BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 1);
buffer_size=1024; buffer_size=1024;
buffer_channel.create_or_open("/buffer", BLC_CHANNEL_WRITE, 'UI32', 'NDEF', 1, buffer_size); buffer_channel.create_or_open("/buffer", BLC_CHANNEL_WRITE, 'UI32', 'NDEF', 2, 2, buffer_size/2);
bl_raspi_add_RC_input(SOUND_PIN, 1, buffer_channel.uints32);//"left_pot_file.bin") bl_raspi_add_RC_input(SOUND_PIN, 1, buffer_channel.uints32);//"left_pot_file.bin")
/* /*
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment