Skip to content

GitLab

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

Improve time management

parent 505e6d55
Hide whitespace changes
Inline Side-by-side
Showing with 76 additions and 47 deletions
bl_raspi.cpp
View file @ 233a7ed7
#include "bl_raspi.h"
#include "blc_channel.h"
#include "bl_raspi_core.h"
#include "blc_core.h"
//#include "mmapGpio.h"
#include <sys/ioctl.h> //ioctl
#include <sys/mman.h>
#include <fcntl.h> //O_RDWR, ...
......@@ -16,15 +16,20 @@
#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 DELTA_NS(current_ns, previous_ns) ((previous_ns > current_ns) ? 1000000000-previous_ns+current_ns : current_ns-previous_ns)
using namespace std;
uint32_t it_since_clock=0;
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;
bl_raspi raspi;
vector <Ohm_input> ohm_inputs;
......@@ -33,40 +38,62 @@ int run_loop=1;
volatile int bl_raspi_iterations_nb;
int buffer_size;
blc_channel debug_channel;
int debug_index=0;
inline void update_time(){
struct timespec timestamp;
// current_ns_per_it;
float current_ns_per_it, error;
int32_t delta_ns;
clock_gettime(CLOCK_MONOTONIC, &timestamp);
current_ns=timestamp.tv_nsec;
delta_ns=DELTA_NS(current_ns, previous_ns);
if (it_since_clock){
ns_per_it=(float)delta_ns/(float)it_since_clock;
// ns_per_it+=(((float)delta_ns/(float)it_since_clock)-ns_per_it)/100.f;
current_ns_per_it=(delta_ns+1)/it_since_clock;
// 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;
}
previous_ns=current_ns;
DEBUG_VALUE(ns_per_it);
// DEBUG_VALUE(error*error);
// DEBUG_VALUE(var_ns_per_it);
for( auto &input : ohm_inputs){
// DEBUG_VALUE(input.activated);
if (input.activated){
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){
tau=300000;
}
inline void Ohm_input::rest(){
raspi.set_pin_dir(pin, raspi.INPUT);
update_time();
buffer[index]=DELTA_NS(current_ns, spike_ns);
buffer[index]=current_ns;
index++;
if (index==buffer_size) index=0;
rest_ns=current_ns;
......@@ -74,7 +101,6 @@ inline void Ohm_input::rest(){
}
inline void Ohm_input::spike(){
raspi.set_pin_dir(pin, raspi.OUTPUT);
tau=50000;
update_time();
spike_ns=current_ns;
buffer[index]=DELTA_NS(current_ns, rest_ns);
......@@ -116,7 +142,6 @@ void *thread_loop(void*){
*raspi.gpfset0|=input.flag; //Set pin high
raspi.set_pin_mode(input.pin, raspi.PULL_OFF);
raspi.set_pin_dir(input.pin, raspi.OUTPUT);
input.stop_ns=current_ns+input.tau*1000;
}
for(auto &output : volt_outputs){
*raspi.gpfset0|=output.flag; //Set pin high
......@@ -126,7 +151,7 @@ void *thread_loop(void*){
uint32_t results;
while(run_loop){
// results =*(raspi.gplev0);
// results =*(raspi.gplev0);
for(auto &input : ohm_inputs){
if (input.activated) {
input.activated--;
......@@ -142,37 +167,37 @@ void *thread_loop(void*){
}
//Output volts
// FOR (i, bl_raspi_volt_outputs_nb){
/*COmputing current ( not yet working)
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;
if (iterations_nb%100==0){
fwrite(&current_ns, 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;
}*/
/* delta_ns=current_ns-volt_outputs[i].us_timestamps;
if (delta_ns > 10){
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//){
raspi.set_pin_dir(volt_outputs[i].pin, raspi.OUTPUT);
volt_outputs[i].activated=1;
}
else{
if (volt_outputs[i].activated==1){
volt_outputs[i].activated_input=0;
volt_outputs[i].us_timestamps_input=current_ns;
raspi.set_pin_dir(volt_outputs[i].pin, raspi.INPUT);
volt_outputs[i].activated=0;
}
}
volt_outputs[i].us_timestamps=current_ns;
}
}*/
// FOR (i, bl_raspi_volt_outputs_nb){
/*COmputing current ( not yet working)
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;
if (iterations_nb%100==0){
fwrite(&current_ns, 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;
}*/
/* delta_ns=current_ns-volt_outputs[i].us_timestamps;
if (delta_ns > 10){
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//){
raspi.set_pin_dir(volt_outputs[i].pin, raspi.OUTPUT);
volt_outputs[i].activated=1;
}
else{
if (volt_outputs[i].activated==1){
volt_outputs[i].activated_input=0;
volt_outputs[i].us_timestamps_input=current_ns;
raspi.set_pin_dir(volt_outputs[i].pin, raspi.INPUT);
volt_outputs[i].activated=0;
}
}
volt_outputs[i].us_timestamps=current_ns;
}
}*/
bl_raspi_iterations_nb++;
it_since_clock++;
if (it_since_clock == 1024) update_time();
if (it_since_clock == 32) update_time();
}
//Stop voltages before living for security
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){
struct sched_param sched_param;
run_loop=1;
/* 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;*/
debug_channel.create_or_open("/raspinobo.debug", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, DEBUG_BUFFER_SIZE);
/* 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_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED), NULL);
......@@ -202,3 +230,4 @@ void bl_raspi_start_loop(uint32_t *RC_results, uchar const *voltage_commands){
void bl_raspi_stop_loop(){
run_loop=0;
}
raspinobo.cpp
View file @ 233a7ed7
......@@ -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);
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")
/*
......
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