///////////////////////////////////////////////////////////////////////////////
/// \brief static thread function passed to pthread. opens the device and
/// attempts to process inputs from it.
///
void* InputHandler :: thread_func ( void* in_ptr )
{
if ( in_ptr == NULL )
{
rt_printf ( "InputHandler :: thread_func: received invalide pointer" ) ;
return NULL ;
}
std :: string *filename = ( std :: string* ) in_ptr ;
int fd = -1 ;
if ( ( fd = open ( filename -> c_str () , O_RDONLY ) ) < 0 ) {
perror("evdev open");
return NULL ; //exit(1);
}
mqd_t output = mq_open ( BBT_EVENT_QUEUE_NAME
, O_WRONLY ) ;
if ( output < 0 )
{
rt_printf ( "InputHandler: failed to open queue" ) ;
}
struct input_event ev;
while ( 1 )
{
size_t rb = read ( fd , &ev , sizeof ( struct input_event ) ) ;
if ( rb > 0 )
processEvent ( ev , output ) ;
}
mq_close ( output ) ;
close ( fd ) ;
return NULL ;
}
bool setup(BelaContext *context, void *userData)
{
// This project makes the assumption that the audio and digital
// sample rates are the same. But check it to be sure!
if(context->audioFrames != context->digitalFrames) {
rt_printf("Error: this project needs the audio and digital sample rates to be the same.\n");
return false;
}
for(int i = 0; i < NUM_PINS; i++) {
pinMode(context, 0, kPins[i], OUTPUT);
}
return true;
}
int main(int argc, char* argv[])
{
char str[10] ;
int i;
rt_print_auto_init(1);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_printf("start task\n");
for (i=0; i<5; i++)
{
sprintf(str,"hello-%d",i);
rt_task_create(&demo_task, str, 0, 50, 0);
rt_task_start(&demo_task, &demo, 0);
}
}
void rEnc(void *arg)
{
int r_last = LOW;
int inr = 0;
int inrB = 0;
int rtick = 0;
char rticks[40];
int fd = open("/dev/mem",O_RDWR | O_SYNC);
ulong* pinconf1 = (ulong*) mmap(NULL, 0x1000, PROT_READ | PROT_WRITE, MAP_SHARED, fd, GPIO1_ADDR);
//configure encoder pins as input
pinconf1[OE_ADDR/4] |= ((1<<13)|(1 << 12)); //P8_11, P8_12, P8_15, P8_16
rt_task_set_periodic(NULL, TM_NOW, period);
rt_printf("Reading right Encoder!\n");
while (1){
rt_task_wait_period(NULL);
if(pinconf1[GPIO_DATAIN/4] & (1 << 13)){
inr = HIGH;
}else{
inr = LOW;
}
if((r_last == LOW)&&(inr == HIGH)){
if(pinconf1[GPIO_DATAIN/4] & (1 << 12)) {
inrB = HIGH;
}else{
inrB = LOW;
}
if(inrB == LOW){
rtick--;
}else{
rtick++;
}
}
r_last = inr;
//rt_printf("Right ticks: %d \n", rtick);
//send to odometry task
sprintf(rticks, "%d", rtick);
rt_queue_write(&rqueue, rticks, sizeof(rticks), Q_NORMAL);
}
return;
}
void periodic_task (void * arg)
{
RTIME previous = 0;
RTIME now = 0;
RTIME period;
RTIME duration;
RTIME min = -1;
RTIME max = 0;
RTIME sum = 0;
RTIME max_max = 0;
long nb_measure_per_cycle;
long measure = 0;
period = * (RTIME *) arg;
nb_measure_per_cycle = 2000000 / period; // 2 seconds.
period = period * 1000; // us->ns
rt_task_set_periodic(NULL, TM_NOW, period);
for (;;) {
rt_task_wait_period(NULL);
now = rt_timer_read();
if (previous != 0) {
duration = now - previous;
sum = sum + duration;
if ((min < 0) || (duration < min))
min = duration;
if (max < duration) {
max = duration;
if (max > max_max)
max_max = max;
}
measure ++;
if (measure == nb_measure_per_cycle) {
rt_printf("Min.=%lld, Moy.=%lld, Max.=%lld, Max.Max.=%lld\n",
min/1000, sum/nb_measure_per_cycle/1000, max/1000, max_max/1000);
measure = 0;
min = -1;
max = 0;
sum = 0;
}
}
previous = now;
}
}
int main(int argc, char* argv[])
{
char str[10];
int err;
rt_print_auto_init(1);
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_printf("start task\n");
sprintf(str, "hello");
//rt_task_create(&demo_task, str, 0, 50, 0);
//rt_task_start(&demo_task, &demo, 0);
err = rt_task_spawn(&demo_task, str, TASK_STKSZ, TASK_PRIO, TASK_MODE, &demo, NULL);
if(!err)rt_task_delete(&demo_task);
printf("task finished\n");
return 0;
}
void LoopLayer::startRecording(uint64_t clockFrame)
{
if (recording) {
return;
}
recording = true;
startRecordingScheduled = false;
if (!recorded) {
recordingStartedFrame = clockFrame;
}
#ifdef DEBUG
rt_printf("starting recording - Layer %d\n", id);
#endif
}
int main(int argc, char* argv[])
{
char str[10] ;
int num1 = 1;
int num2 = 2;
int num3 = 3;
int num4 = 4;
int num5 = 5;
// Perform auto-init of rt_print buffers if the task doesn't do so
rt_print_auto_init(1);
// Lock memory : avoid memory swapping for this program
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_printf("start 5 tasks\n");
/*
* Arguments: &task,
* name,
* stack size (0=default),
* priority,
* mode (FPU, start suspended, ...)
*/
sprintf(str,"task1");
rt_task_create(&demo_task1, str, 0, 50, 0);
sprintf(str,"task2");
rt_task_create(&demo_task2, str, 0, 51, 0);
sprintf(str,"task3");
rt_task_create(&demo_task3, str, 0, 52, 0);
sprintf(str,"task4");
rt_task_create(&demo_task4, str, 0, 53, 0);
sprintf(str,"task5");
rt_task_create(&demo_task5, str, 0, 54, 0);
/*
* Arguments: &task,
* task function,
* function argument
*/
rt_task_start(&demo_task1, &demo, &num1);
rt_task_start(&demo_task2, &demo, &num2);
rt_task_start(&demo_task3, &demo, &num3);
rt_task_start(&demo_task4, &demo, &num4);
rt_task_start(&demo_task5, &demo, &num5);
}
bool EcMaster::start() throw(EcError)
{
#ifdef HRT
rt_task_set_priority(&master,0);
if (asprintf(&devnameOutput, "/proc/xenomai/registry/rtipc/xddp/%s", XDDP_PORT_OUTPUT) < 0)
throw(EcError(EcError::FAIL_OUTPUT_LABEL));
//open the socket for the outputs
fdOutput = open(devnameOutput, O_WRONLY);
free(devnameOutput);
if (fdOutput < 0)
{
perror("open");
throw(EcError(EcError::FAIL_OPENING_OUTPUT));
}
if (asprintf(&devnameInput, "/proc/xenomai/registry/rtipc/xddp/%s", XDDP_PORT_INPUT) < 0)
throw(EcError(EcError::FAIL_INPUT_LABEL));
//open the socket for the inputs
fdInput = open(devnameInput, O_RDONLY);
free(devnameInput);
if (fdInput < 0)
{
perror("open");
throw(EcError(EcError::FAIL_OPENING_INPUT));
}
/*
*Create a thread for update slave inputs periodically
*/
sched_param sch;
sch.sched_priority = 90;
pthread_setschedparam(updateThread.native_handle(), SCHED_OTHER, &sch);
updateThread = std::thread(&EcMaster::update_EcSlaves,this);
#endif
for (int i = 0 ; i < m_drivers.size() ; i++)
m_drivers[i] -> start();
//wait to start correctly drivers
usleep(50000);
rt_printf("Master started!!! \n");
return true;
}
static void task0(void *argpointer){
/* get task method */
int method = *((int *) argpointer);
free(argpointer);
int id = 0;
sync_threads(id);
/* get resource */
get_resource(method, id);
/* do work */
rt_printf("Task %i start\n", id);
busy_wait_ms(3);
/* release resource */
release_resource(method, id);
}
void Midi::writeOutputLoop(){
while(!gShouldStop){
usleep(1000);
int length = outputBytesWritePointer - outputBytesReadPointer;
if(length < 0){
length = outputBytes.size() - outputBytesReadPointer;
}
if(length == 0){ //nothing to be written
continue;
}
int ret;
ret = write(outputPort, &outputBytes[outputBytesReadPointer], sizeof(midi_byte_t)*length);
if(ret < 0){ //error occurred
rt_printf("error occurred while writing: %d\n", errno);
usleep(10000); //wait before retrying
continue;
}
}
}
//startup code
void startup()
{
int i;
char str[10] ;
void (*task_func[NTASKS]) (void *arg);
task_func[0]=taskOne;
task_func[1]=taskTwo;
rt_queue_create(&myqueue,"myqueue",QUEUE_SIZE,10,Q_FIFO);
rt_timer_set_mode(0);// set timer to tick in nanoseconds and not in jiffies
for(i=0; i < NTASKS; i++) {
rt_printf("start task : %d\n",i);
sprintf(str,"task%d",i);
rt_task_create(&task_struct[i], str, 0, 50, 0);
rt_task_start(&task_struct[i], task_func[i], &i);
}
}
int main(void)
{
int err;
RT_TASK task;
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_print_auto_init(1);
if ((err = rt_task_shadow(& task, "Hello World 02",
99, T_JOINABLE)) != 0) {
fprintf(stderr, "rt_task_shadow: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
while (1) {
rt_printf("Hello World 02\n");
rt_task_sleep(1000000000LL); // 1 sec.
}
return 0;
}
int main(int argc, char* argv[])
{
rt_print_auto_init(1);
signal(SIGTERM, catch_signal);
signal(SIGINT, catch_signal);
mlockall(MCL_CURRENT|MCL_FUTURE);
done=1;
int flag=0;
while (flag<sizeQ){
gyroQ[flag]=rand()%366;
flag++;
}
int err =rt_mutex_create(&a,"MyMutex");//creating mutex
err =rt_mutex_create(&g,NULL);
rt_task_create(&Accelerometer,NULL , 0, 0, T_JOINABLE);
rt_task_create(&gyroscope, NULL, 0, 0, T_JOINABLE);
rt_task_create(&fusion, NULL, 0, 0, T_JOINABLE);
rt_task_start(&gyroscope, &gyro,NULL);
rt_task_start(&Accelerometer, &Acc, NULL);
rt_task_start(&fusion, &fusionT, NULL);
rt_task_join(&gyroscope);
rt_task_join(&Accelerometer);
rt_task_join(&fusion);
rt_task_delete(&gyroscope);
rt_task_delete(&Accelerometer);
rt_task_delete(&fusion);
rt_mutex_delete(&a);
rt_mutex_delete(&g);
flag=0;
while (flag<sizeFinal){//print out result
rt_printf("Result%d: %d\n",flag,finalNum[flag]);
flag++;
}
}
void key_handler(void *arg)
{
int count = 0;
int nr_interrupts_waiting;
RT_TASK *curtask;
RT_TASK_INFO curtaskinfo;
while(1) {
//rt_printf("%d\n",count++);
nr_interrupts_waiting = rt_intr_wait(&keypress,TM_INFINITE);
if (nr_interrupts_waiting>0)
{
// inquire current task
curtask=rt_task_self();
rt_task_inquire(curtask,&curtaskinfo);
rt_printf("Current priority of handler task: %d \n", curtaskinfo.cprio);
}
}
}
static void task2(void *argpointer){
/* get method */
int method = *((int *) argpointer);
free(argpointer);
int id = 2;
sync_threads(id);
rt_task_sleep(2000000);
/* Get resource */
get_resource(method, id);
/* Simulate work */
rt_printf("Task %i start \n", id);
busy_wait_ms(2);
/* Release resource */
release_resource(method, id);
}
int main(int argc, char* argv[])
{
char str[10] ;
int i;
rt_print_auto_init(1);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_printf("start task\n");
rt_sem_create(&sem, "sem", 0, S_FIFO);
for (i=0; i<5; i++)
{
sprintf(str, "hello-%d", i);
rt_task_create(&task[i], str, 0, 50+i, 0);
rt_task_start(&task[i], &demo, &i);
}
// rt_sem_v(&sem);
rt_sem_broadcast(&sem);
}
void fonction_periodique (void * arg)
{
RTIME precedent = 0;
RTIME heure = 0;
RTIME periode;
RTIME duree;
periode = * (RTIME *) arg;
periode = periode * 1000; // en ns
rt_task_set_periodic(NULL, TM_NOW, periode);
while(1) {
rt_task_wait_period(NULL);
heure = rt_timer_read();
// ignorer le premier declenchement
if (precedent != 0) {
duree = heure - precedent;
rt_printf("%llu\n", duree/1000);
}
precedent = heure;
}
}
void Verif_Comm (int status) {
int compteur;
DMessage *message;
if (status != STATUS_OK) {
rt_mutex_acquire(&mutexCpt, TM_INFINITE);
cpt_perte_com ++;
compteur = cpt_perte_com;
rt_mutex_release(&mutexCpt);
if (compteur >= 6) {
// La communication est perdue
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
etatCommRobot = status;
rt_mutex_release(&mutexEtat);
message = d_new_message();
message->put_state(message, status);
rt_printf("tmove : Envoi message\n");
if (write_in_queue(&queueMsgGUI, message, sizeof (DMessage)) < 0) {
message->free(message);
}
//rt_sem_v(&semConnectedRobot);
// On réinitialise le compteur
rt_mutex_acquire(&mutexCpt, TM_INFINITE);
cpt_perte_com = 0 ;
rt_mutex_release(&mutexCpt);
compteur = 0 ;
// On reset le robot
//rt_mutex_acquire(&mutexRobot, TM_INFINITE);
robot->close_com(robot); // a verifier
// rt_mutex_release(&mutexRobot);
}
}
}
int main(int argc, char* argv[])
{
// Perform auto-init of rt_print buffers if the task doesn't do so
rt_print_auto_init(1);
// Lock memory : avoid memory swapping for this program
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_printf("starting tasks\n");
/*
* Arguments: &task,
* name,
* stack size (0=default),
* priority,
* mode (FPU, start suspended, ...)
*/
rt_task_create(&task1, "t1", 0, 50, 0);
rt_task_create(&task2, "t2", 0, 50, 0);
rt_task_create(&task3, "t3", 0, 50, 0);
rt_task_create(&task4, "t4", 0, 50, 0);
rt_task_create(&task5, "t5", 0, 50, 0);
int a1 = 1;
int a2 = 2;
int a3 = 3;
int a4 = 4;
int a5 = 5;
/*
* Arguments: &task,
* task function,
* function argument
*/
rt_task_start(&task1, &task, &a1);
rt_task_start(&task2, &task, &a2);
rt_task_start(&task3, &task, &a3);
rt_task_start(&task4, &task, &a4);
rt_task_start(&task5, &task, &a5);
}
int main(int argc, char* argv[])
{
char str[10] ;
// Perform auto-init of rt_print buffers if the task doesn't do so
rt_print_auto_init(1);
// Lock memory : avoid memory swapping for this program
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_printf("starting 5 tasks\n");
/*
* Arguments: &task,
* name,
* stack size (0=default),
* priority,
* mode (FPU, start suspended, ...)
*/
sprintf(str,"a");
rt_task_create(&demo_task_a, str, 0, 50, 0);
sprintf(str,"b");
rt_task_create(&demo_task_b, str, 0, 50, 0);
sprintf(str,"c");
rt_task_create(&demo_task_c, str, 0, 50, 0);
sprintf(str,"d");
rt_task_create(&demo_task_d, str, 0, 50, 0);
sprintf(str,"e");
rt_task_create(&demo_task_e, str, 0, 50, 0);
/*
* Arguments: &task,
* task function,
* function argument
*/
rt_task_start(&demo_task_a, &demo, 0);
rt_task_start(&demo_task_b, &demo, 0);
rt_task_start(&demo_task_c, &demo, 0);
rt_task_start(&demo_task_d, &demo, 0);
rt_task_start(&demo_task_e, &demo, 0);
}
///////////////////////////////////////////////////////////////////////////////
/// \brief default constructor, creates a message queue
///
InputHandler :: InputHandler ()
{
struct mq_attr attr ; // To store queue attributes
// First we need to set up the attribute structure
memset ( &attr , 0 , sizeof ( attr ) ) ;
attr . mq_maxmsg = BBT_EVENT_QUEUE_SIZE ;
attr . mq_msgsize = BBT_EVENT_MSG_SIZE ;
attr . mq_flags = 0 ;
out_queue = mq_open ( BBT_EVENT_QUEUE_NAME
, O_WRONLY | O_CREAT
, 0664
, &attr ) ;
if ( out_queue < 0 )
{
rt_printf ( "InputHandler failed to create queue %d\n" , errno ) ;
}
}
int main(int argc, char* argv[])
{
char str[10] ;
int i;
// Perform auto-init of rt_print buffers if the task doesn't do so
rt_print_auto_init(1);
// Lock memory : avoid memory swapping for this program
mlockall(MCL_CURRENT|MCL_FUTURE);
/* create semaphore */
rt_sem_create(&semGlobal,"semaphore",0,S_PRIO);
rt_printf("start task\n");
for (i=0; i<5; i++)
{
/*
* Arguments: &task,
* name,
* stack size (0=default),
* priority,
* mode (FPU, start suspended, ...)
*/
sprintf(str,"hello-%d",i);
rt_task_create(&demo_task, str, 0, 50+i, 0);
/*
* Arguments: &task,
* task function,
* function argument
*/
rt_task_start(&demo_task, &demo, &i);
}
rt_sem_broadcast(&semGlobal);
}
bool EcMaster::stop() throw(EcError)
{
#ifdef HRT
rt_task_set_priority(&master,98);
#endif
//Stops slaves
for (int i = 0 ; i < m_drivers.size() ; i++)
m_drivers[i] -> stop();
#ifdef HRT
//Stops the NRT thread
threadFinished = true;
updateThread.join();
threadFinished = false;
close(fdInput);
close(fdOutput);
#endif
rt_printf("Master stoped!!! \n");
return true;
}
void handler()
{
rt_printf("Begin interrupt handler of lightsensor\n");
int nr_waiting_interrupts = 0;
int counter = 0;
while(1)
{
nr_waiting_interrupts = rt_intr_wait(&lightsens_intr,TM_INFINITE);
if (nr_waiting_interrupts > 0)
{
//stap 1
char byte;
byte = byte | 0x63; //0110011
outb(byte, 0x378); //set D to first phase of X
rt_task_wait_period(NULL);
//stap 2
byte = 0x14;
outb(byte, 0x378);
rt_task_wait_period(NULL);
//stap 3
byte = 0x08;
outb(byte,0x378);
rt_task_wait_period(NULL);
//stap 4
byte = 0x14;
outb(byte,0x378);
rt_task_wait_period(NULL);
//stap 5
byte = 0x63;
outb(byte,0x378);
rt_task_wait_period(NULL);
//leegmaken
byte = 0x00;
outb(byte,0x378);
}
}
}
void recv_msg(void *arg)
{
int ret;
struct msghdr msg;
struct iovec iov[2];
unsigned short msgsize = size;
struct sockaddr_in addr;
while (1) {
iov[0].iov_base = &msgsize;
iov[0].iov_len = sizeof(msgsize);
iov[1].iov_base = buffer_in;
iov[1].iov_len = size;
memset(&msg, 0, sizeof(msg));
msg.msg_name = &addr;
msg.msg_namelen = sizeof(addr);
msg.msg_iov = iov;
msg.msg_iovlen = 2;
ret = recvmsg_rt(sock, &msg, 0);
if (ret <= 0) {
rt_printf(" recvmsg_rt() = %d\n", ret);
return;
} else {
unsigned long ip = ntohl(addr.sin_addr.s_addr);
rt_printf("received packet from %lu.%lu.%lu.%lu, length: %d+2, "
"encoded length: %d,\n flags: %X, content %s\n", ip >> 24,
(ip >> 16) & 0xFF, (ip >> 8) & 0xFF, ip & 0xFF,
ret-sizeof(msgsize), msgsize, msg.msg_flags,
(memcmp(buffer_in, buffer_out, ret-sizeof(msgsize)) == 0) ?
"ok" : "corrupted");
}
}
}
bool EcMaster::reset() throw(EcError)
{
if (m_useDC)
{
for (int i = 0; i < m_drivers.size(); i++)
m_drivers[i] -> setDC(false, m_cycleTime, 0);
}
taskFinished = true;
//Wait on the termination of task.
rt_task_join (task);
// delete the periodic task
rt_task_delete(task);
bool success = switchState (EC_STATE_INIT);
if (!success)
throw(EcError(EcError::FAIL_SWITCHING_STATE_INIT));
for (unsigned int i = 0; i < m_drivers.size(); i++)
delete m_drivers[i];
m_drivers.resize(0);
#ifdef HRT
delete[] outputBuf;
delete[] inputBuf;
#endif
delete[] m_ecPort;
delete task;
for (size_t i = 0; i < 4096; i++)
m_IOmap[i] = 0;
ec_close();
rt_printf("Master reseted!!! \n");
return true;
}
int main(int argc, char* argv[])
{
rt_print_auto_init(1);
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_printf("start task\n");
rt_task_create(&task1, "t1", 0, 50, 0);
rt_task_create(&task2, "t2", 0, 49, 0);
rt_task_create(&task3, "t3", 0, 48, 0);
rt_task_create(&task4, "t4", 0, 47, 0);
rt_task_create(&task5, "t5", 0, 46, 0);
int num1 = 42;
int num2 = 43;
int num3 = 44;
int num4 = 45;
int num5 = 46;
rt_task_start(&task1, &demo, (void *)&num1);
rt_task_start(&task2, &demo, (void *)&num2);
rt_task_start(&task3, &demo, (void *)&num3);
rt_task_start(&task4, &demo, (void *)&num4);
rt_task_start(&task5, &demo, (void *)&num5);
}
void setFilterCoefficients(float sampleRate, float cutoff)
{
float wc = 2 * M_PI * cutoff; // cutoff frequency in radians
float Q = sqrt(2) / 2; // Q of filter
float wca = tanf(wc * 0.5 / sampleRate); // warped analogue frequency
// set coeffiecients for butterworth filters high pass):
float a0 = 1 + wca/Q + pow(wca,2);
gB0 = 1 / a0;
gB1 = -2 / a0;
gB2 = 1 / a0;
gA1 = (-2 + 2 * pow(wca,2) ) / a0;
gA2 = (1 - wca/Q + pow(wca,2) ) / a0;
rt_printf("B0: %f\tB1:%f\tB2: %f\tA1:%f\tA2: %f", gB0, gB1, gB2, gA1, gA2);
gX1 = gX2 = gY1 = gY2 = 0;
}
void communiquer(void *arg) {
DMessage *msg = d_new_message();
int var1 = 1;
int num_msg = 0;
rt_printf("tserver : Début de l'exécution de serveur\n");
serveur->open(serveur, "8000");
rt_printf("tserver : Connexion\n");
rt_mutex_acquire(&mutexEtat, TM_INFINITE);
etatCommMoniteur = 0;
rt_mutex_release(&mutexEtat);
while (var1 > 0) {
rt_printf("tserver : Attente d'un message\n");
var1 = serveur->receive(serveur, msg);
num_msg++;
if (var1 > 0) {
switch (msg->get_type(msg)) {
case MESSAGE_TYPE_ACTION:
rt_printf("tserver : Le message %d reçu est une action\n",
num_msg);
DAction *action = d_new_action();
action->from_message(action, msg);
switch (action->get_order(action)) {
case ACTION_CONNECT_ROBOT:
rt_printf("tserver : Action connecter robot\n");
rt_sem_v(&semConnecterRobot);
break;
}
break;
case MESSAGE_TYPE_MOVEMENT:
rt_printf("tserver : Le message reçu %d est un mouvement\n",
num_msg);
rt_mutex_acquire(&mutexMove, TM_INFINITE);
move->from_message(move, msg);
move->print(move);
rt_mutex_release(&mutexMove);
break;
}
}
}
}
