Skip to content
GitLab
Commit 6e124a1b authored by ARnaud Blanchard's avatar ARnaud Blanchard
Browse files

Nig improvement but not yet working

parent 6c573609
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 6e124a1b
......@@ -22,7 +22,7 @@ add_executable(raspinobo raspinobo.cpp bl_raspi.cpp)
target_link_libraries(raspinobo ${BL_LIBRARIES})
if (NOT APPLE)
#Give the raw capbility to the target /dev/mem
#Give the raw capability to the target /dev/mem
add_custom_command(TARGET raspinobo POST_BUILD COMMAND sudo setcap "cap_dac_override+ep cap_sys_rawio+ep cap_sys_nice=eip" $<TARGET_FILE:raspinobo>)
endif()
README.md
View file @ 6e124a1b
......@@ -4,6 +4,15 @@ Setup the raspberry has an access point
More details here in the original tuatorial ( https://github.com/SurferTim/documentation/blob/6bc583965254fa292a470990c40b145f553f6b34/configuration/wireless/access-point.md )
Check the frequency:
`sudo cat /sys/devices/system/cpu/cpufreq/policy0/cpuinfo_cur_freq`
Check temperature:
`/opt/vc/bin/vcgencmd measure_temp`
Video tracking
......
bl_raspi.cpp
View file @ 6e124a1b
......@@ -15,15 +15,23 @@
#define GPIO_MAX 32
#define FLAG(pos) (1<<pos)
typedef struct
typedef struct Ohm_input
{
uint32_t us_timestamps;
uint32_t flag;
uint32_t tau;
uint32_t rest_ns, spike_ns;
uint8_t pin;
uint8_t activated;
FILE *file;
}ohm_input_type;
uint32_t activated;
uint32_t *buffer;
uint32_t previous_iteration;
int32_t index;
int64_t stop_ns;
ohm_input_type();
inline void rest();
inline void spike();
}Ohm_input;
typedef struct
{
......@@ -38,10 +46,10 @@ typedef struct
}volt_output_type;
typedef struct bl_raspi{
static const unsigned int system_timers_base_addr = 0x3F003000;
static const unsigned int gpio_base_addr = 0x3F200000;// gpio registers base address for raspberry 2 et 3 0x20200000 sinon
//Three pulling resistor (~50KOhm modes in input direction
static const unsigned int PULL_OFF = 0;
static const unsigned int PULL_DOWN = 1;
......@@ -52,58 +60,103 @@ typedef struct bl_raspi{
// two possible states for output pins
static const unsigned int LOW = 0;
static const unsigned int HIGH = 1;
volatile uint32_t *system_timers_map, *gpio_map;
volatile uint32_t *ptime_us;
volatile uint32_t *gpfsels, *gpfset0, *gpclr0, *gplev0, *gppud, *gppudclk0; //1 could be used if more than 32 pins
bl_raspi();
~bl_raspi();
void set_pin_dir(int pin, int dir); //input or output
void set_pin_mode(int pin, int mode); //pulling resistance
}bl_raspi;
int bl_raspi_RC_inputs_nb=0;
int bl_raspi_volt_outputs_nb=0;
uint32_t *bl_raspi_RC_inputs=NULL;
uchar const *bl_raspi_volt_outputs=NULL;
int bl_raspi_iterations_nb=0;
static const int CHUNK_ITERATIONS = 128;
uint32_t it_since_clock=0;
uint32_t current_ns, previous_ns;
float ns_per_it=1;
bl_raspi raspi;
Ohm_input *ohm_inputs=nullptr;
static int run_loop;
static ohm_input_type *ohm_inputs=NULL;
static volt_output_type *volt_outputs=NULL;
inline void update_time(){
struct timespec timestamp;
int32_t current_ns_per_it;
int32_t delta_ns;
int i;
clock_gettime(CLOCK_MONOTONIC, &timestamp);
current_ns=timestamp.tv_nsec;
delta_ns=current_ns-previous_ns;
ns_per_it+=(((float)delta_ns/(float)it_since_clock)-ns_per_it)/10.f;
previous_ns=current_ns;
it_since_clock=0;
FOR( i, bl_raspi_RC_inputs_nb){
if (ohm_inputs[i].activated){
if (ohm_inputs[i].stop_ns - current_ns> 0) ohm_inputs[i].activated = (float)(ohm_inputs[i].stop_ns - current_ns)/ns_per_it;
else ohm_inputs[i].activated=1;
}
}
}
ohm_input_type::ohm_input_type():index(0){
}
static bl_raspi raspi;
inline void ohm_input_type::rest(){
raspi.set_pin_dir(pin, raspi.INPUT);
update_time();
buffer[index]=rest_ns-spike_ns;
index++;
if (index==buffer_size) index=0;
rest_ns=current_ns;
// fprintf(stderr, "%lld %f\n", current_ns-spike_ns, ns_per_it);
}
inline void ohm_input_type::spike(){
raspi.set_pin_dir(pin, raspi.OUTPUT);
tau=10000;
update_time();
spike_ns=current_ns;
buffer[index]=current_ns-rest_ns;
index++;
if (index==buffer_size) index=0;
stop_ns=current_ns + tau*1000;
activated=(float)(tau*1000)/ns_per_it;
}
volatile uint32_t *map_memory(uint32_t base_addr, size_t size){
int mem_fd;
volatile uint32_t *mapped_addr;
SYSTEM_ERROR_CHECK(mem_fd = open("/dev/mem", O_RDWR|O_SYNC), 0, "Maybe you need to be sudo or use set_cap on the binary" );
/* mmap IO */
SYSTEM_ERROR_CHECK(mapped_addr = (volatile unsigned *)mmap(NULL, //Any adddress in our space will do
size, //Map length
PROT_READ|PROT_WRITE|PROT_EXEC,// Enable reading & writting to mapped memory
#ifdef MAP_LOCKED
#ifdef MAP_LOCKED
MAP_SHARED | MAP_LOCKED,//Shared with other processes
#else
#else
MAP_SHARED, // Darwin (OSX) misses MAP_LOCKED
#endif
#endif
mem_fd, //File to map
base_addr //Offset to base address
), MAP_FAILED, "Mapping physical memory. You are not on raspberry or you do not have the rights. ( use setcap or sudo).");
SYSTEM_SUCCESS_CHECK(close(mem_fd), 0, "We cannot close fd_meme (i.e. we can close /dev/mem)");
return mapped_addr;
}
bl_raspi::bl_raspi(){
//maps timers
system_timers_map=map_memory(system_timers_base_addr, 0x1C);
ptime_us=system_timers_map+1; //+1 is 4bytes because ptimes is 32(4*8)bits step
//map gpio
gpio_map=map_memory(gpio_base_addr, 0xB4);
gpfsels=gpio_map;
......@@ -139,14 +192,13 @@ void bl_raspi::set_pin_dir(int pinnum, int dir){
}
}
void bl_raspi_add_RC_input(int pin, int us_tau, char const *filename){
void bl_raspi_add_RC_input(int pin, int us_tau, uint32_t *buffer){
ohm_input_type input;
input.flag=FLAG(pin);
input.tau=us_tau;
input.pin=pin;
input.activated=1;
if (filename) SYSTEM_ERROR_CHECK(input.file=fopen(filename, "w"), NULL, NULL);
else input.file=NULL;
input.buffer=buffer;
APPEND_ITEM(&ohm_inputs, &bl_raspi_RC_inputs_nb, &input);
}
......@@ -154,30 +206,33 @@ void bl_raspi_add_voltage_output(int pin, char const *filename, char const* file
volt_output_type output;
output.flag=FLAG(pin);
output.pin=pin;
if (filename) SYSTEM_ERROR_CHECK(output.file=fopen(filename, "w"),NULL, NULL);
if (filename_input) SYSTEM_ERROR_CHECK(output.file_input=fopen(filename_input, "w"),NULL, NULL);
APPEND_ITEM(&volt_outputs, &bl_raspi_volt_outputs_nb, &output);
}
void *thread_loop(void*){
static const unsigned int coeff=5;
static const unsigned int coeff=1;
struct timespec timestamp;
uint32_t result;
int i;
struct timespec delay_sleep;
uint32_t initial_us, relative_us, current_us, delta_us;
delay_sleep.tv_sec=0;
delay_sleep.tv_nsec=1000;
uint32_t initial_ns, delta_ns;
int32_t previous_ns, current_ns;
int32_t var_ns_per_iteration;
int32_t error;
*raspi.gpfset0=0;
initial_us=3000000*(int)(*raspi.ptime_us/3000000)-3000000;
// initial_us=3000000*(int)(*raspi.ptime_us/3000000)-3000000;
update_time();
FOR(i, bl_raspi_RC_inputs_nb){
*raspi.gpfset0|=ohm_inputs[i].flag; //Set pin high
raspi.set_pin_mode(ohm_inputs[i].pin, raspi.PULL_OFF);
raspi.set_pin_dir(ohm_inputs[i].pin, raspi.OUTPUT);
ohm_inputs[i].us_timestamps=*raspi.ptime_us;
ohm_inputs[i].activated=1;
ohm_inputs[i].stop_ns=current_ns+ohm_inputs[i].tau*1000;
ohm_inputs[i].spike_ns=current_ns;
ohm_inputs[i].rest_ns=current_ns;
}
FOR(i, bl_raspi_volt_outputs_nb) {
......@@ -188,49 +243,20 @@ void *thread_loop(void*){
}
while(run_loop){
result=*(raspi.gplev0);
relative_us=*(raspi.ptime_us);
if (initial_us > current_us) current_us=UINT32_MAX-initial_us+relative_us;
else current_us=relative_us-initial_us;
// Input RC
FOR(i, bl_raspi_RC_inputs_nb){
if (ohm_inputs[i].activated) {
delta_us=current_us-ohm_inputs[i].us_timestamps;
if ( delta_us > coeff*ohm_inputs[i].tau){
raspi.set_pin_dir(ohm_inputs[i].pin, raspi.INPUT);
ohm_inputs[i].activated=0;
if (ohm_inputs[i].file) {
fwrite(&current_us, sizeof(uint32_t), 1, ohm_inputs[i].file);
fwrite(&delta_us, sizeof(uint32_t), 1, ohm_inputs[i].file);
}
ohm_inputs[i].us_timestamps=current_us;
// if (i==0) fprintf(stderr, "end %uld delta_ns %f\n", (*us_time), delta_ns);
ohm_inputs[i].activated--;
if (ohm_inputs[i].activated==0){
ohm_inputs[i].rest();
}
}
else {
if ((result & ohm_inputs[i].flag)==0){
raspi.set_pin_dir(ohm_inputs[i].pin, raspi.OUTPUT);
delta_us=current_us-ohm_inputs[i].us_timestamps;
bl_raspi_RC_inputs[i]=delta_us;
ohm_inputs[i].activated=1;
// if (i==0) fprintf(stderr, "start %lu ", (*ptime));
// clock_gettime(CLOCK_MONOTONIC, &timer);
// current_ns=(timer.tv_sec-initial_s)*(1000000000.f)+timer.tv_nsec;
ohm_inputs[i].us_timestamps=current_us;
if (ohm_inputs[i].file){
fwrite(&current_us, sizeof(uint32_t), 1, ohm_inputs[i].file);
fwrite(&(delta_us), sizeof(uint32_t), 1, ohm_inputs[i].file);
}
if ((*(raspi.gplev0) & ohm_inputs[i].flag)==0){
ohm_inputs[i].spike();
}
}
}
//Output volts
FOR (i, bl_raspi_volt_outputs_nb){
/*COmputing current ( not yet working)
......@@ -242,9 +268,9 @@ void *thread_loop(void*){
}
volt_outputs[0].activated_input=1;
}*/
delta_us=current_us-volt_outputs[i].us_timestamps;
if (delta_us > 10){
volt_outputs[i].average+=(volt_outputs[i].activated-volt_outputs[i].average)*(delta_us/100);
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;
......@@ -252,15 +278,17 @@ void *thread_loop(void*){
else{
if (volt_outputs[i].activated==1){
volt_outputs[i].activated_input=0;
volt_outputs[i].us_timestamps_input=current_us;
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_us;
volt_outputs[i].us_timestamps=current_ns;
}
}
bl_raspi_iterations_nb++;
it_since_clock++;
if (it_since_clock == (1<<16)) update_time();
}
//Stop voltages before living for security
FOR(i, bl_raspi_volt_outputs_nb) raspi.set_pin_dir(volt_outputs[i].pin, raspi.INPUT);
......@@ -270,7 +298,7 @@ void *thread_loop(void*){
void bl_raspi_start_loop(uint32_t *RC_results, uchar const *voltage_commands){
pthread_t thread;
pthread_attr_t attr;
pthread_attr_t attr={0};
struct sched_param sched_param;
run_loop=1;
......@@ -278,12 +306,12 @@ void bl_raspi_start_loop(uint32_t *RC_results, uchar const *voltage_commands){
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);
BLC_PTHREAD_CHECK(pthread_attr_setschedpolicy(&attr, SCHED_FIFO), NULL);
sched_param.sched_priority=20;
BLC_PTHREAD_CHECK(pthread_attr_setschedparam(&attr, &sched_param), NULL);
/* 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_setschedpolicy(&attr, SCHED_FIFO), NULL);
sched_param.sched_priority=18;
BLC_PTHREAD_CHECK(pthread_attr_setschedparam(&attr, &sched_param), NULL);*///
BLC_PTHREAD_CHECK(pthread_create(&thread, &attr, thread_loop, NULL), "Creating bl_raspi thread");
}
......
bl_raspi.h
View file @ 6e124a1b
......@@ -7,11 +7,12 @@ extern uint32_t *bl_raspi_RC_inputs;
extern unsigned char const *bl_raspi_volt_outputs;
extern int bl_raspi_RC_inputs_nb;
extern int bl_raspi_volt_outputs_nb;
extern int buffer_size;
/** Number of iterations of the loop. Useful to check the rate of the refreshing loop */
extern int bl_raspi_iterations_nb;
extern volatile int bl_raspi_iterations_nb;
void bl_raspi_add_RC_input(int pin, int us_tau, char const *filename);
void bl_raspi_add_RC_input(int pin, int us_tau, uint32_t *buffer);
void bl_raspi_add_voltage_output(int pin, char const *filename, char const* filename_input);
void bl_raspi_start_loop(uint32_t *RC_results, unsigned char const* voltage_commands);
......
raspinobo.cpp
View file @ 6e124a1b
......@@ -7,14 +7,17 @@
#include <math.h>
#include <iostream>
#include <fstream>
#include <chrono>
#include <thread>
#include <chrono>
#include <time.h>
#include "blc_program.h"
#include "blc_channel.h"
#include "bl_raspi.h"
static const int SOUND_PIN=13;
static const int LEFT_POT_PIN=6;
static const int RIGHT_POT_PIN=12;
static const int LEFT_MOTOR_PIN=26;
......@@ -34,7 +37,7 @@ static uchar gaussian_cdf(uint32_t s, float mu, float var){
blc_channel motor_channel, sensor_channel, neurons_channel;
int main(int argc, char **argv){
blc_channel gain_channel, target_channel;
blc_channel gain_channel, target_channel, buffer_channel;
char const *gpio_channel_name, *period_str, *volts_channel_name, *target_channel_name, *tracking, *gain_name;
int us_period, previous_iterations_nb;
char const *values_debug;
......@@ -62,11 +65,16 @@ int main(int argc, char **argv){
if (gpio_channel_name==NULL) EXIT_ON_ERROR("The ouput channel (option -o) must not be null");
// 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);
bl_raspi_add_RC_input(SOUND_PIN, 1, buffer_channel.uints32);//"left_pot_file.bin")
/*
bl_raspi_add_RC_input(LEFT_POT_PIN, 333, NULL);//"left_pot_file.bin");
bl_raspi_add_RC_input(RIGHT_POT_PIN, 333, NULL); //"right_pot_file.bin");
bl_raspi_add_voltage_output(LEFT_MOTOR_PIN, NULL, NULL);
bl_raspi_add_voltage_output(RIGHT_MOTOR_PIN, NULL, NULL);
bl_raspi_add_voltage_output(RIGHT_MOTOR_PIN, NULL, NULL);*/
motor_channel.create_or_open(volts_channel_name, BLC_CHANNEL_WRITE, 'UIN8', 'NDEF', 1, 2);
sensor_channel.create_or_open(gpio_channel_name, BLC_CHANNEL_WRITE, 'UI32', 'NDEF', 1, bl_raspi_RC_inputs_nb);
......@@ -74,11 +82,12 @@ int main(int argc, char **argv){
neurons_channel.create_or_open("/pinobo.neurons", BLC_CHANNEL_WRITE, 'UIN8', 'NDEF', 1, 3);
target_channel.create_or_open(target_channel_name, BLC_CHANNEL_READ, 'UIN8', 'NDEF', 1, 2);
gain_channel.create_or_open("/pinobo.gain", BLC_CHANNEL_READ, 'UIN8', 'NDEF', 1, 1);
gain_channel.create_or_open("/pinobo.gain", BLC_CHANNEL_READ, 'UIN8', 'NDEF', 1, 1);
motor_channel.uchars[0]=0;
motor_channel.uchars[1]=0;
bl_raspi_start_loop(sensor_channel.uints32, motor_channel.uchars);
previous_iterations_nb=0;
......@@ -109,53 +118,22 @@ int main(int argc, char **argv){
uchar right_motor;
}line;
SYSTEM_ERROR_CHECK(record_file=fopen("pinobo.data", "w"), NULL, "Opening 'pinobo.data'");
chrono::high_resolution_clock::time_point tp = chrono::high_resolution_clock::now();
//SYSTEM_ERROR_CHECK(record_file=fopen("pinobo.data", "w"), NULL, "Opening 'pinobo.data'");
struct timespec timestamp;
uint32_t previous_ns, delay_ns;
previous_ns=0;
BLC_COMMAND_LOOP(us_period){
if (blc_loop_iteration!=0) blc_eprint_cursor_up(1);
FOR(i, 2){
neurons_channel.uchars[i]=neurons[i].update(sensor_channel.uints32[i]);
}
fprintf(stderr, "Freq:%fHz\n n0:%d n0mu:%f n0var:%f", (bl_raspi_iterations_nb-previous_iterations_nb)/((float)us_period/1000000.f), neurons_channel.uchars[2], neurons[0].mu, neurons[0].var);
int tmp=(255-neurons_channel.uchars[1]);
int avg=128+(neurons_channel.uchars[0]-neurons_channel.uchars[1])/2;
int dist_avg=abs(avg-neurons_channel.uchars[2]);
int dist_tmp=abs(tmp-neurons_channel.uchars[2]);
int dist_0=abs(neurons_channel.uchars[0]-neurons_channel.uchars[2]);
if (avg<=dist_tmp){
if (avg<=dist_0) neurons_channel.uchars[2] = avg;
else neurons_channel.uchars[2] = neurons_channel.uchars[0];
}
else if (dist_0 < dist_tmp) neurons_channel.uchars[2] = neurons_channel.uchars[0];
else neurons_channel.uchars[2]=tmp;
clock_gettime(CLOCK_MONOTONIC, &timestamp);
if (timestamp.tv_nsec > previous_ns) delay_ns = timestamp.tv_nsec-previous_ns;
else delay_ns = 1000000000 - previous_ns + timestamp.tv_nsec;
previous_ns=timestamp.tv_nsec;
if (gain_channel.uchars[0]!=0)
{
if (gain_channel.uchars[0]>30) error = (target_channel.uchars[0]-neurons_channel.uchars[2])*gain_channel.uchars[0]/128;
else error = (target_channel.uchars[0]-128)*gain_channel.uchars[0]/128 + error/2;
if ( neurons_channel.uchars[2]==0) error=-20;
else if ( neurons_channel.uchars[2]==255) error=20;
if (abs(error)>1){
motor_channel.uchars[0]+=(CLIP_UCHAR(-error)- motor_channel.uchars[0]);
motor_channel.uchars[1]+=(CLIP_UCHAR(+error)-motor_channel.uchars[1]);
}
else{
motor_channel.uchars[0]=0;
motor_channel.uchars[1]=0;
}
}
line.time=chrono::duration_cast<chrono::microseconds>(chrono::high_resolution_clock::now()-tp).count();
line.left_pot=sensor_channel.uints32[0];
line.right_pot=sensor_channel.uints32[1];
line.left_motor=motor_channel.uchars[0];
line.right_motor=motor_channel.uchars[1];
fwrite(&line, sizeof(line), 1, record_file);
fprintf(stderr, "%f, Freq:%fMHz\n n0:%d n0mu:%f n0var:%f", (float)delay_ns, (bl_raspi_iterations_nb-previous_iterations_nb)/((float)(delay_ns/1000)), neurons_channel.uchars[2], neurons[0].mu, neurons[0].var);
previous_iterations_nb=bl_raspi_iterations_nb;
}
......
Supports Markdown
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