Hi Gabor, Hoffman and all,
First, i'd like to apologize come back to this point now but in the last
days i've been trying to understand (trough your e-mails) and testing my
program to learn how can i use FP in my own programs, but to be honest some
things, even now, aren't clear for me.
Before talk about my doubts i'd like to thank Gabor for his help
about rtl_printf: I changed all printk with rtl_printf and my system began
work better although it still freeze sometimes. I'd like to thank Hoffman
too and everybody that sended e-mails about this point.
After my mail talking about my "system freezing" Hoffman asked me if the
floating point reference ("10.0") in my init_module() code couldn't be
causing the crash. Well, when he asked this i really didn't know the
answer, now (after many mails) seems to me that it could be the problem but
my practice results are bring me doubts.
To test my module i removed all the rtl_printf calls. Now my program seems
work very well, without crashs. Important points: I'm still using FP in my
init_module() and fifo handlers and i don't call pthread_setfp_np (or
something like that) in any point of my code.
Hoping you can help me to understand this,
Daniela.
P.S.: My module is attached. Sorry but i didn't have time to translate the
comments.
/* Driver para controle de um duplo integrador. Daniela Almeida 16/05/2000 */
/* This program makes the interface between a GUI (not RT) and AD/DA boards. It looks
for work with
2 kinds of AD/DA boards and V1.1 and V2.2 of RTL, both choosed through #define */
#ifndef MODULE
#define MODULE
#endif
#ifndef __KERNEL__
#define __KERNEL__
#endif
#ifndef __RT__
#define __RT__
#endif
// Define se o drive sera para a versao 1.1 ou 2.2 do RTLinux e se sera para a placa
DAS1600 ou AIO (STD)
//#define V1 //Escolhe entre as versoes 1.1 e 2.2 do RTL
#define V2
//#define AIO //Escolhe entre as placas AD/DA AIO(STD) e DAS1601
#define DAS1600
#define OSC //Define se havera uma onda quadrada na porta paralela
para teste
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/version.h>
#include <linux/errno.h>
#include <linux/cons.h>
#include <linux/ioport.h>
#include <linux/string.h>
#include <pthread.h>
#include <asm/io.h>
#include <rtl_sched.h>
#include <rtl_fifo.h>
#include "declaracoes.h"
#ifdef AIO
#include "placa_ai.h" // header com os enderecos da placa AD/DA do STD
#endif
#ifdef DAS1600
#include "placa16.h" // header com os enderecos da placa AD/DA DAS1600
#endif
#ifdef V1
RT_TASK tarefa[2];
#endif
#ifdef V2
pthread_t tarefa[2];
#endif
//Definicao das variaveis de tempo
#ifdef V2
hrtime_t RT_TICKS_CTRL; //Variavel utilizada para determinar o tempo de amostragem
para o controle
hrtime_t RT_TICKS_SAVE; //Variavel utilizada para determinar o tempo de amostragem
para salvar os dados nas fifos
hrtime_t ONDA_QUADRADA;
hrtime_t ONDA_QUADRADA2;
#endif
#ifdef V1
RTIME RT_TICKS_CTRL; //Variavel utilizada para determinar o tempo de amostragem para
o controle
RTIME RT_TICKS_SAVE; //Variavel utilizada para determinar o tempo de amostragem para
salvar os dados nas fifos
RTIME ONDA_QUADRADA;
RTIME ONDA_QUADRADA2;
#endif
parametros_PID PID; // Ganhos do PID
mensagens MSG; // Mensagens de inicializacao e mudanca de periodo
float uf, vf;
int ub;
int valor;
float tensao;
float h;
float ek, pk, dk, otensao, ref;
float ik;
float N=20.0;
float Nh;
float escala; //escala de medicao: VADPP/4095.0 (converte para volts)
int a=1;
int range; // Range de enderecos ocupados pelo AD
int dispara_ad; //String que dipara o AD
int controle_ad; //String que controla o funcionamento da placa AD/DA
float FREQHZ;
float VADPP;
int contador; //Contador que controla a onda quadrada
int sinal; // Sinal da onda quadrada
int count=0;
#ifdef AIO
static int AIO_port_base=0x0300; //Se zero a base sera lida do arquivo .h
#endif
#ifdef DAS1600
static int das1601_port_base=0; //Se zero a base sera lida do arquivo .h
#endif
void *lei_controle(void *t)
{
while (1) {
if (count < 100) //rotina para teste
{
//rtl_printf("Lei de controle Chamada,count= %i\n",count);
count = count+1;
}
#ifdef DAS1600
valor = ( (inb(AD0) & 0x0F0) >> 4 | ((inb(AD1) & 0x0FF) << 4)); // Aquisicao da
posicao
tensao = ((float) (valor - AD_POL))*escala;
#endif
#ifdef AIO
valor = ( (inb(AD0) & 0x0FF) | ((inb(AD1) & 0x00F) << 8)); // Aquisicao da posicao
tensao = ((float) (valor - AD_POL))*escala;
#endif
// Rotina para gerar a onda quadrada
if (contador > ONDA_QUADRADA2 && contador <= ONDA_QUADRADA) // Gera um onda
quadrada ou retangular?
sinal = -1;
else if (contador > ONDA_QUADRADA) {
sinal = 1;
contador=0;
}
contador++;
// Lei de controle
ref = PID.ref + PID.sw_amplitud*sinal; // A referencia eh uma constante mais a
amplitude
// da onda quadrada * sinal aplicado
ek = ref - tensao; //tensao e a leitura atual da posicao (convertida em valores de
tensao)
pk = PID.kp*ek; //termo proporcional do controle
dk = PID.kd/(PID.kd+Nh)*(dk - PID.kp*N*(tensao-otensao)); //termo derivativo do
controle
vf = pk + ik + dk; //tensao de controle
// Saturacao do controle ub \in [0,4095]
if( vf > 1.0)
uf=1.0;
else if (vf < -1.0)
uf=-1.0;
else
uf=vf;
ub = ((int)(uf*2047.0)) + 2048; // Sinal de Controle (Limites setados na placa
AD/DA. Verificar Header)
// Escreve o controle no DA
#ifdef DAS1600
outb((((ub&0x00F) << 4) & 0xF0), DA0LB);
outb((((ub&0xFF0) >> 4) & 0xFF), DA0HB);
#endif
#ifdef AIO
outb(((ub&0x0FF) & 0xFF), DA0LSB);
outb((((ub&0xF00) >> 8)& 0x0F),DA0MSB);
#endif
// Calculo dos estados do PID
ik += PID.kp*PID.ki*h*ek; //acao integral
otensao = tensao; // prepara a variavel de tensao anterior
#ifdef OSC //Produz uma onda quadrada na porta paralela (para teste)
outb(a, LPT);
a = ~a & 0x01;
#endif OSC
// outb(0, AD0); //Dispara o AD para a proxima conversao
//Dispara o AD para a proxima conversao
#ifdef AIO
dispara_ad = (((AD_GANHO << 5)|AD_CANAL)& 0xFF);
outb(dispara_ad,AD_MUX);
#endif
#ifdef DAS1600
dispara_ad = ((AD_CANAL)& 0x0F);
outb(dispara_ad,AD0);
#endif
#ifdef V2
pthread_wait_np(); // Espera a proxima interrupcao
#endif
#ifdef V1
rt_task_wait(); // Espera a proxima interrupcao
#endif
}
}
void *salvar_dados(void *t)
{
while (1) {
if (count < 100) //rotina para teste
{
//rtl_printf("Salvar Dados Chamado, count= %i\n",count);
count = count+1;
}
//ref = 1.0;
/* Salva posicao e controle nas FIFO */
rtf_put(0, &tensao, sizeof(float));
rtf_put(1, &ref, sizeof(float));
rtf_put(3, &uf, sizeof(float));
rtf_put(5, &count, sizeof(float));
#ifdef V2
pthread_wait_np(); // Espera a proxima interrupcao
#endif
#ifdef V1
rt_task_wait(); // Espera a proxima interrupcao
#endif
}
}
int mod_pid(unsigned int fifo)
{
//rtl_printf("mod_pid chamado\n");
rtf_get(fifo, &PID, sizeof(parametros_PID)); //Atualiza a struct PID com os novos
dados
ik=0.0;
return(0);
}
int comando(unsigned int fifo)
{
int err;
#ifdef V1
RTIME now;
#endif
#ifdef V2
// struct sched_param sched_param;
//pthread_wakeup_np(tarefa[MSG.tarefa]);
hrtime_t now;
#endif
//rtl_printf("Handler Comando Chamado\n");
//while ((err= rtf_get(fifo, &MSG, sizeof(MSG)))== sizeof(MSG)) { //Atualiza a struct
MSGS com os novos dados
err= rtf_get(fifo, &MSG, sizeof(MSG));
switch (MSG.command){
case START_TASK:
//rtl_printf("Comando: %d, Tarefa: %d, Periodo: %g\n",
MSG.command,MSG.tarefa,MSG.period);
#ifdef V2
now = gethrtime();
pthread_wakeup_np(tarefa[MSG.tarefa]);
err=pthread_make_periodic_np(tarefa[MSG.tarefa], now,
HRTICKS_PER_SEC*MSG.period);
//if (err<0) { rtl_printf("Erro fazendo a tarefa %d periodica, err=
%i\n",MSG.tarefa,err);} else {//rtl_printf("Sucesso fazendo a tarefa %d periodica,
err= %i\n",MSG.tarefa,err);}
#endif
#ifdef V1
now = rt_get_time();
err=rt_task_make_periodic(&tarefa[MSG.tarefa], now,MSG.period);
//if (err<0) { rtl_printf("Erro fazendo a tarefa %d periodica, err=
%i\n",MSG.tarefa,err);} else {//rtl_printf("Sucesso fazendo a tarefa %d periodica,
err= %i\n",MSG.tarefa,err);}
#endif
break;
case STOP_TASK:
//rtl_printf("Comando: %d, Tarefa: %d, Periodo: %g\n",
MSG.command,MSG.tarefa,MSG.period);
#ifdef V2
err=pthread_suspend_np(tarefa[MSG.tarefa]);
//if (err<0) { rtl_printf("Erro suspendedo a tarefa %d periodica, err=
%i\n",MSG.tarefa,err);} else {//rtl_printf("Sucesso suspendendo a tarefa %d periodica,
err= %i \n",MSG.tarefa,err);}
#endif
#ifdef V1
err=rt_task_suspend(&tarefa[MSG.tarefa]);
//if (err<0) { rtl_printf("Erro suspendedo a tarefa %d periodica, err=
%i\n",MSG.tarefa,err);} else {//rtl_printf("Sucesso suspendendo a tarefa %d periodica,
err= %i \n",MSG.tarefa,err);}
#endif
count =0;
break;
default:
//rtl_printf("Dados errados\n");
//rtl_printf("Comando: %d, Tarefa: %d, Periodo: %d\n",
MSG.command,MSG.tarefa,MSG.period);
return -EINVAL;
// }
}
// if (err != 0){
// return -EINVAL;
// }
//rtl_printf("Handler Comando Finalizado\n");
return 0;
}
int init_ad(unsigned int port, unsigned int range)
{
int err=0;
// Verifica se os enderecos necessarios para o AD estao disponiveis
// Nao entendi check_region e request_region
if ((err=check_region(port,range)) < 0) return err; /* busy */
request_region(port,range,"Placa AD/DA"); /* always succeeds */
// Programacao do AD
// Programacao do AD para a placa DAS1601
#ifdef DAS1600
controle_ad = (((AD_INTE & 0x01) << 7)| ((AD_LEVEL & 0x07) << 4) | ((AD_DMAE &
0x01) << 2) | (AD_TRIG & 0x03));
outb(controle_ad, AD_CTRL); /* 0 INT
xxx INT LEVEL = HardInt
x Doesn't matter
0 DMAE = Desabled
0x Trigger Source = Timer */
controle_ad = (((AD_BRATE & 0x1F) << 2) | (AD_GANHO & 0x03));
outb(controle_ad, AD_GAIN); /* Seta ganhos */
controle_ad = (((AD_BLEN & 0x0F) << 4) | (AD_CLOCK & 0x03));
outb(controle_ad, AD_CNTEN); /* Contador 1 e 2 habilitados */
controle_ad = (((AD_FIM & 0x0F) << 4)| (AD_INICIO & 0x0F));
outb(controle_ad, AD_MUX); /* Conversao do Canal 0 somente */
//outb(0x11, AD_MUX); /*Conversao do Canal 1 somente */
outb(AD_HAB, AD_DISARM); /* Habilita Conversao */
#endif
//Programacao do AD para a placa AD do STD
#ifdef AIO
controle_ad = (((AD_GANHO << 5)|AD_CANAL)& 0xFF);
outb(controle_ad,AD_MUX);
#endif
return(err);
}
int init_ctrl(void)
{
FREQHZ = 50.0; //Base para a frequencia de amostragem
h = 1.0/FREQHZ;
Nh = N*h;
escala = VADPP/4095; // Ambas as placas tem 12 bits de resolucao
ik=0.0;
contador=0; //Contador que controla a onda quadrada
sinal = 1; // Sinal inicial da onda quadrada
//Inicializa as variaveis de tempo
#ifdef V2
RT_TICKS_CTRL = HRTICKS_PER_SEC/ FREQHZ; // Periodo em nanosegundos
RT_TICKS_SAVE = 1 * RT_TICKS_CTRL; // Periodo em nanosegundos
ONDA_QUADRADA = 10 * FREQHZ;
ONDA_QUADRADA2 = 10 * FREQHZ / 2;
#endif
#ifdef V1
RT_TICKS_CTRL = RT_TICKS_PER_SEC/ FREQHZ; //Periodo em Ticks do 8254
RT_TICKS_SAVE = 4 * RT_TICKS_CTRL; //Periodo em Ticks do 8254
ONDA_QUADRADA = 20 * FREQHZ;
ONDA_QUADRADA2 = 20 * FREQHZ / 2;
#endif
// Inicializa as referencias
PID.ref=0.0;
PID.sw_amplitud=0.0;
return 0;
}
int init_module(void)
{
int adstatus;
int err;
int c[6];
#ifdef V1
RTIME now;
#endif
#ifdef V2
struct sched_param sched_param;
hrtime_t now;
# endif
#ifdef DAS1600
int base = das1601_port_base ? das1601_port_base : AD_BASE; //Determina se o endereco
base sera o mesmo do arquivo .h
range=0x20;
#endif
#ifdef AIO
int base = AIO_port_base ? AIO_port_base : AD_BASE; //Determina se o endereco
base sera o mesmo do arquivo .h
range=0x20;
#endif
if((err=init_ad(base, range)) != 0 ) { //Se o AD nao puder ser inicializado
retorna uma mensagem de erro
/* //rtl_printf(KERN_INFO "I/O Address already used\n"); */
//rtl_printf("I/O Address already used\n");
return(-1);
}
/* //rtl_printf(KERN_INFO "di.c:2.0 09/05/00 [EMAIL PROTECTED]\n"); */
/*#ifdef DEBUG
//rtl_printf(KERN_INFO "di.c: Sampling time: %i*nanoseg \n", RT_TICKS_CTRL);
//rtl_printf(KERN_INFO "di.c: Saving interval: %i*nanoseg \n", RT_TICKS_SAVE);
#endif*/
// Nao exporta as variaveis globais para o kernel
//register_symtab(0);
// Cria as FIFOs
c[0] = rtf_create(0, 1000); // y
c[1] = rtf_create(1, 1000); // status ad
c[2] = rtf_create(2, 1000); // parametros PID
c[3] = rtf_create(3, 1000); // u
c[4] = rtf_create(4, 1000); //comandos para o tarefa0 (lei_controle) e para o
tarefa1 (salvar_dados)
c[5] = rtf_create(5, 1000); //Teste de Funcionamento guarda o valor de count
//rtl_printf("Fifo return 0=%d 1=%d 2=%d 3=%d 4=%d
5=%d\n",c[0],c[1],c[2],c[3],c[4],c[5]);
// Le o status do AD
#ifdef DAS1600
adstatus = inb(AD_STATUSA);
AD_POL= ((adstatus & 0x40) >> 6);
if (AD_POL==0) {
VADPP = 20.0;
AD_POL = 2048;} // a placa do DAS esta ajustada para trabalhar de -10 a +10 volts
else if (AD_POL==1){
VADPP = 10.0;
AD_POL = 0;} // a placa do DAS esta ajustada para trabalhar de 0 a +10 volts
else{
//rtl_printf("Erro lendo o status do AD\n");
return (-1);
}
#endif
#ifdef AIO
VADPP = 5.0; // a placa AIO trabalha somente de 0 a +5 Volts (AD e DA)
adstatus = (inb(AD1));
adstatus = (((adstatus & 0xC0) >> 5)|AD_POL);
if (AD_POL==0) {
AD_POL = 2048;} // A planta de controle tem saida bipolar
else if (AD_POL==1){
AD_POL = 0;} // A planta de controle tem saida unipolar
else{
//rtl_printf("Erro lendo o status do AD\n");
return (-1);
}
#endif
rtf_put(1, &adstatus, sizeof(int)); //Grava o status do AD na rtf1
init_ctrl(); //Inicializa as variaveis de tempo, frequencia, escala, etc.
// Cria as threads
#ifdef V2
err=pthread_create(&tarefa[0], NULL,lei_controle, (void *)1);
//if (err<0) {rtl_printf("Erro iniciando a tarefa 0\n");} else {rtl_printf("Tarefa 0
inicializada corretamente\n");}
sched_param.sched_priority = 1;
pthread_setschedparam (tarefa[0], SCHED_FIFO, &sched_param);
err=pthread_create (&tarefa[1], NULL,salvar_dados, (void *)1);
//if (err<0) {rtl_printf("Erro iniciando a tarefa 1\n");} else {rtl_printf("Tarefa 1
inicializada corretamente\n");}
sched_param.sched_priority = 4;
pthread_setschedparam (tarefa[1], SCHED_FIFO, &sched_param);
now = gethrtime();
/* the 2 tasks run periodically with an offset of 100 time units -> Nao entendi o
comentario */
err=pthread_make_periodic_np(tarefa[0], now + 1 * NSECS_PER_SEC, RT_TICKS_CTRL);
//if (err<0) {rtl_printf("Erro fazendo a tarefa 0 periodica\n");} else
{rtl_printf("Sucesso fazendo a tarefa 0 periodica\n");}
err=pthread_make_periodic_np(tarefa[1], now + 2 * NSECS_PER_SEC, RT_TICKS_SAVE);
//if (err<0) {rtl_printf("Erro fazendo a tarefa 1 periodica\n");} else
{rtl_printf("Sucesso fazendo a tarefa 1 periodica\n");}
#endif
#ifdef V1
err = rt_task_init(&tarefa[0],(void *)lei_controle,1,3000,1);
//if (err<0) {rtl_printf("Erro iniciando a tarefa 0\n");} else {rtl_printf("Tarefa 0
inicializada corretamente\n");}
err = rt_task_init(&tarefa[1],(void *)salvar_dados,1,3000,4);
//if (err<0) {rtl_printf("Erro iniciando a tarefa 1\n");} else {rtl_printf("Tarefa 1
inicializada corretamente\n");}
//rtl_printf("Tarefas inicializadas\n");
now = rt_get_time();
err=rt_task_make_periodic(&tarefa[0], now, RT_TICKS_CTRL); // Ha 1193180 Ticks em 1
segundo (Era 120000)
//if (err<0) {rtl_printf("Erro fazendo a tarefa 0 periodica\n");} else
{rtl_printf("Sucesso fazendo a tarefa 0 periodica\n");}
err=rt_task_make_periodic(&tarefa[1], now + 3000,RT_TICKS_SAVE); // Ha 1193180 Ticks
em 1 segundo (Era 120000)
//if (err<0) {rtl_printf("Erro fazendo a tarefa 1 periodica\n");} else
{rtl_printf("Sucesso fazendo a tarefa 1 periodica\n");}
#endif
// Cria o handler de modificacao dos parametros do PID
err=rtf_create_handler(2,&mod_pid); //Toda vez que ha uma modificacao da fifo 2
mod_pid eh chamada
//if (err<0) {rtl_printf("Erro criando o handler 1\n");} else {rtl_printf("Sucesso
criando o handler 1\n");}
err=rtf_create_handler(4,&comando); //A rotina comando le a fifo 4 e determina o
inicio e o fim das tarefas
//if (err<0) {rtl_printf("Erro criando o handler 2\n");} else {rtl_printf("Sucesso
criando o handler 2\n");}
return 0;
}
void cleanup_module(void)
{
/* release adc region.
Before that write zero to the DA */
#ifdef DAS1600
outb(0x00,DA0LB);
outb(0x00,DA0HB);
#endif
#ifdef AIO
outb(0x00,DA0LSB);
outb(0x00,DA0MSB);
#endif
rtf_destroy(0);
rtf_destroy(1);
rtf_destroy(2);
rtf_destroy(3);
rtf_destroy(4);
#ifdef V2
pthread_delete_np(tarefa[0]);
pthread_delete_np(tarefa[1]);
#endif
#ifdef V1
rt_task_delete(&tarefa[0]);
rt_task_delete(&tarefa[1]);
#endif
release_region(AD_BASE, range);
}
//#define COMMAND_FIFO 4
#define START_TASK 1
#define STOP_TASK 2
typedef struct {
int command;
int tarefa;
float period;
}mensagens ;
typedef struct
{
float kd;
float kp;
float ki;
float ref;
float sw_amplitud;
}parametros_PID;
