ssize_t rtlx_read(int index, void *buff, size_t count, int user)
{
size_t fl = 0L;
struct rtlx_channel *lx;
if (rtlx == NULL)
return -ENOSYS;
lx = &rtlx->channel[index];
/* find out how much in total */
count = min(count,
(size_t)(lx->lx_write + lx->buffer_size - lx->lx_read)
% lx->buffer_size);
/* then how much from the read pointer onwards */
fl = min( count, (size_t)lx->buffer_size - lx->lx_read);
copy_to(buff, &lx->lx_buffer[lx->lx_read], fl, user);
/* and if there is anything left at the beginning of the buffer */
if ( count - fl )
copy_to (buff + fl, lx->lx_buffer, count - fl, user);
/* update the index */
lx->lx_read += count;
lx->lx_read %= lx->buffer_size;
return count;
}
bool ConcurrentCircularBuffer::bl_consume(uint size, char* dest, CommandInitiator* curr_thread)
{
uint capacity_mask = m_capacity_mask;
uint capacity = get_capacity();
uint chunk;
while (size > 0) {
chunk = size;
if (chunk > capacity) chunk = capacity;
uint rd = m_rd_ptr;
m_rd_wr_lock.lock();
if (m_size < chunk) {
m_blocked_consumer = curr_thread;
m_waited_bytes = chunk;
m_rd_wr_lock.unlock();
if (!curr_thread->yield()) return false;
m_rd_wr_lock.lock();
}
copy_to(dest, rd, chunk, capacity);
m_size -= chunk;
dest += chunk;
m_rd_ptr = (rd+chunk) & capacity_mask; // single reader ensured:no need to protect...
size -= chunk;
m_rd_wr_lock.unlock();
}
return true;
}
/**
* Parse EPSEM data into class
*
* @param data EPSEM data
*/
C1222_EPSEM *
C1222_EPSEM::parse(uint8_t * data)
{
uint8_t * ptr = (uint8_t *)data;
//first byte should be flag
uint8_t flag = *ptr;
int eclass = 0;
int ber_size = 0, serv_len = 0;
#ifdef DEBUG
printf("[*] EPSEM Parsing start... \n");
#endif
if(*ptr == 0x0)
{
#ifdef DEBUG
printf(" [-] Empty EPSEM, still valid\n");
#endif
return NULL;
}
if((flag >> 4) & 0x1) //if eclass flag is on
{
#ifdef DEBUG
printf(" [-] eclass flag copy... \n");
#endif
copy_to(&eclass, ptr+1, 4);
#ifdef DEBUG
printf(" [-] get data part... \n");
#endif
serv_len = ber_len_decode(ptr+5, &ber_size);
ptr = ptr + 1 + 4 + ber_size;
}
else //if e_class is not present
{
void VbaProject::toPPTY(NSBinPptxRW::CBinaryFileWriter* pWriter) const
{
pWriter->WriteBYTE(NSBinPptxRW::g_nodeAttributeStart);
pWriter->WriteString1(0, m_filename.GetFilename());
pWriter->WriteBYTE(NSBinPptxRW::g_nodeAttributeEnd);
copy_to(pWriter->m_pCommon->m_pMediaManager->m_strDstMedia);
}
DynamicAny::DynAny_ptr DynArrayImpl::copy()
throw(CORBA::SystemException)
{
if (_destroyed)
{
throw CORBA::OBJECT_NOT_EXIST("DynAny destroyed");
}
DynArrayImpl* new_dyn =
new DynArrayImpl(_factory, _orb, _dyn_type, _base_type);
copy_to(*new_dyn);
return new_dyn;
}
static TEE_Result alloc_and_copy_to(void **p, struct elf_load_state *state,
size_t offs, size_t len)
{
TEE_Result res;
void *buf;
buf = malloc(len);
if (!buf)
return TEE_ERROR_OUT_OF_MEMORY;
res = copy_to(state, buf, len, 0, offs, len);
if (res == TEE_SUCCESS)
*p = buf;
else
free(buf);
return res;
}
DynamicAny::DynAny_ptr DynExceptImpl::copy()
throw(CORBA::SystemException)
{
if (_destroyed)
{
throw CORBA::OBJECT_NOT_EXIST("DynAny destroyed");
}
DynExceptImpl* new_dyn =
new DynExceptImpl(_factory, _orb, _dyn_type, _base_type);
copy_to(*new_dyn);
if (new_dyn->_next_value)
{
new_dyn->_next_value->skip_string();
}
return new_dyn;
}
std::tuple<aku_Status, size_t> CombineGroupAggregateOperator::read(aku_Timestamp *destts, AggregationResult *destval, size_t size) {
if (size == 0) {
return std::make_tuple(AKU_EBAD_ARG, 0);
}
return copy_to(destts, destval, size);
}
int main( int argc, char *argv[] )
{
unsigned iter;
FILE *infile, *resfile;
char *resfilename;
// algorithmic parameters
algoparam_t param;
int np;
double runtime, flop;
double residual=0.0;
// check arguments
if( argc < 2 )
{
usage( argv[0] );
return 1;
}
// check input file
if( !(infile=fopen(argv[1], "r")) )
{
fprintf(stderr,
"\nError: Cannot open \"%s\" for reading.\n\n", argv[1]);
usage(argv[0]);
return 1;
}
// check result file
resfilename= (argc>=3) ? argv[2]:"heat.ppm";
if( !(resfile=fopen(resfilename, "w")) )
{
fprintf(stderr,
"\nError: Cannot open \"%s\" for writing.\n\n",
resfilename);
usage(argv[0]);
return 1;
}
// check input
if( !read_input(infile, ¶m) )
{
fprintf(stderr, "\nError: Error parsing input file.\n\n");
usage(argv[0]);
return 1;
}
print_params(¶m);
if( !initialize(¶m) )
{
fprintf(stderr, "Error in Solver initialization.\n\n");
usage(argv[0]);
return 1;
}
// full size (param.resolution are only the inner points)
np = param.resolution + 2;
#if _EXTRAE_
Extrae_init();
#endif
// starting time
runtime = wtime();
iter = 0;
while(1) {
switch( param.algorithm ) {
case 0: // JACOBI
residual = relax_jacobi(param.u, param.uhelp, np, np);
copy_to(¶m.uhelp, ¶m.u);
break;
case 1: // GAUSS
residual = relax_gauss(param.u, np, np);
break;
}
iter++;
// solution good enough ?
if (residual < 0.00005) break;
// max. iteration reached ? (no limit with maxiter=0)
if (param.maxiter>0 && iter>=param.maxiter) break;
}
// Flop count after iter iterations
flop = iter * 11.0 * param.resolution * param.resolution;
// stopping time
runtime = wtime() - runtime;
#if _EXTRAE_
Extrae_fini();
#endif
fprintf(stdout, "Time: %04.3f ", runtime);
fprintf(stdout, "(%3.3f GFlop => %6.2f MFlop/s)\n",
flop/1000000000.0,
flop/runtime/1000000);
fprintf(stdout, "Convergence to residual=%f: %d iterations\n", residual, iter);
// for plot...
coarsen( param.u, np, np,
param.uvis, param.visres+2, param.visres+2 );
write_image( resfile, param.uvis,
param.visres+2,
param.visres+2 );
finalize( ¶m );
return 0;
}
TEE_Result elf_load_body(struct elf_load_state *state, vaddr_t vabase)
{
TEE_Result res;
size_t n;
void *p;
uint8_t *dst = (uint8_t *)vabase;
Elf32_Ehdr *ehdr = state->ehdr;
Elf32_Phdr *phdr = state->phdr;
/*
* Zero initialize everything to make sure that all memory not
* updated from the ELF is zero (covering .bss and eventual gaps).
*/
memset(dst, 0, state->vasize);
/*
* Copy the segments
*/
memcpy(dst, state->ta_head, state->ta_head_size);
res = copy_to(state, dst, state->vasize,
phdr[0].p_vaddr + state->ta_head_size,
phdr[0].p_offset + state->ta_head_size,
phdr[0].p_filesz - state->ta_head_size);
if (res != TEE_SUCCESS)
return res;
for (n = 1; n < ehdr->e_phnum; n++) {
if (phdr[n].p_type != PT_LOAD)
continue;
res = copy_to(state, dst, state->vasize, phdr[n].p_vaddr,
phdr[n].p_offset, phdr[n].p_filesz);
if (res != TEE_SUCCESS)
return res;
}
/*
* We have now loaded all segments into TA memory, now we need to
* process relocation information. To find relocation information
* we need to locate the section headers. The section headers are
* located somewhere between the last segment and the end of the
* ELF.
*/
if (ehdr->e_shoff) {
/* We have section headers */
res = alloc_and_copy_to(&p, state, ehdr->e_shoff,
ehdr->e_shnum * sizeof(Elf32_Shdr));
if (res != TEE_SUCCESS)
return res;
state->shdr = p;
}
/* Hash until end of ELF */
res = advance_to(state, state->nwdata_len);
if (res != TEE_SUCCESS)
return res;
if (state->shdr) {
/* Process relocation */
for (n = 0; n < ehdr->e_shnum; n++) {
if (state->shdr[n].sh_type == SHT_RELA)
return TEE_ERROR_NOT_IMPLEMENTED;
else if (state->shdr[n].sh_type == SHT_REL) {
res = elf_process_rel(state, n, vabase);
if (res != TEE_SUCCESS)
return res;
}
}
}
return TEE_SUCCESS;
}
int main(void)
{
iniciar_leds_debug();
//teste_filtro_de_kalman();
//setar_parametros_PID(1500, 1000, 60, 126, 2500, 0, 0, 126); //Ajusta as constantes do PID para Roll e Pitch.
setar_parametros_PID(1400, 25, 50, 188, 15, 0, 0, 188); //Ajusta as constantes do PID para Roll e Pitch.
//setar_parametros_PID(900, 1200, 20, 188, 10, 0, 0, 188); //Ajusta as constantes do PID para Roll e Pitch.
//Melhores parametros obtidos até o momento (05/01/2015) 5e-10 1e-45 1e-45 0.005 0.35 1e-6
//Qang, QbiasAcel, Qbiasmag, Racel, Rmag, Rorth
setar_parametros_Kalman(2.8e-8, 1e-15, 1e-15, 5e-15, 1e-2, 1e-1, 5e-1); //Ajusta as covariâncias do filtro de Kalman. //Melhores parametreos testados até o momento - 2e-9, 5e-8, 5e-12, 2.e-2, 2e-1, 1e-10, 1e-10
uint16_t counter_recebidos = 0; //Variável para contagem do número de mensagens recebidas.
uint8_t status_RF = 0; //Variável que aloca o estado do link RF.
NVIC_SetPriorityGrouping(5);
SysTick_Config(168e6/10000); //Frequência de 100uS no systick.
iniciar_ESC(); //Inicia PWM para controle dos ESCS.
ajustar_velocidade(15,0); //15 -> 0b1111 -> todos os motores -> Velocidade 0 -> Motores parados.
//blinkDebugLeds(); //Pisca os leds
delay_startup(); //Delay de inicialização dos ESCS.
iniciar_RF(); //Inicar a placa nRF24L01p
// configurar_I2C(); //Configurar o periférico I²C da placa.
TM_I2C_Init(I2C3, TM_I2C_PinsPack_1, 400000);
iniciarMPU6050Imu();
configurar_bussola(); //Inicia o magnetômetro.
iniciar_timer_controle(); //Timer responsável por checar se houve recepção de controel nos últimos 2 segundos.
escrita_dados(SPI2, (uint8_t *)"Iniciado.", 32); //Mensagem que informa que o procimento de inicialização foi concluído.
modo_rx(SPI2); //Habilita recepção de novas mensagens.
configurar_timers_PWM_I(); //Configura os timers utilizados para PWMinput do controle.
iniciar_estado_Kalman();
iniciar_timer_processamento(); //Iniciar o timer responsável pelo controle do PID -> TIM6.
while (1) //Processo contínuo de checagem do transmissor RF.
{
if (!Checar_IRQ()) //Checa eventos do transmissor. -> 0 Novo evento | 1 Não há novos eventos.
{
status_RF = retornar_status(SPI2); //Registrador de Status do transmissor -> Causa da interrupção.
if (MSG_Recebida()) //Checa o 6º bit -> Mensagem recebida
{
GPIO_ToggleBits(GPIOD, GPIO_Pin_13); //Toggle no LED que indica a recepção de novas mensagens.
TIM_SetCounter(TIM7,0); //Reseta o contador do timer -> Inicia nova contagem de 2 segundos.
/*Recupera as msgs enquanto houverem dados à serem recuperados*/
while((status_RF & 0x0E) != 0x0E) //Bits 1,2 e 3 -> Número do duto que recebeu mensagem no RF.
{ //0b1110 -> Sem dados do duto. Repete o processo enquanto tiver dados.
counter_recebidos++; //Contador para teste - Comparação entre número de msg enviadas / recebidas.
limpar_buffer(buffer_dados_tx, 33); //Limpa os buffers para troca de dados.
limpar_buffer(buffer_dados_rx, 33);
leitura_dados(SPI2, buffer_dados_rx, 32);
buffer_dados_tx[0] = status_RF; //Limpa o FLAG de msg recebida no registrador de status.
buffer_dados_tx[0] |= RX_DR;
escrever_registrador(SPI2, STATUS, buffer_dados_tx, 1);
status_RF = retornar_status(SPI2); //Checagem se há mais dados nos disponíveis para serem tratados na próxima iteração.
switch(buffer_dados_rx[0]) //Primeiro caractér do vetor recebido determina a operação à ser realizada.
{
case 't':
GPIO_ToggleBits(GPIOD, GPIO_Pin_15);
break;
//Função de teste do LINK RF -> Retorna o número de payloads recebidos
//Utilizada para checagem no número de pacotes perdidos.
case 'C':
numToASCII(counter_recebidos, buffer_dados_tx);
escrita_dados(SPI2, buffer_dados_tx, 32);
break;
case 'R':
buffer_dados_tx[0] = 'R';
conversor.flutuante_entrada = dc_ch_1;
copy_to(buffer_dados_tx, conversor.bytes, 1, 4);
conversor.flutuante_entrada = dc_ch_2;
copy_to(buffer_dados_tx, conversor.bytes, 5, 4);
conversor.flutuante_entrada = dc_ch_3;
copy_to(buffer_dados_tx, conversor.bytes, 9, 4);
conversor.flutuante_entrada = dc_ch_4;
copy_to(buffer_dados_tx, conversor.bytes, 13, 4);
escrita_dados(SPI2, buffer_dados_tx, 32);
break;
case 'S':
if(start_logging_sensores == 0)
start_logging_sensores = 1;
else if(start_logging_sensores == 1)
start_logging_sensores = 0;
break;
case 'L':
if(start_logging_final == 0)
start_logging_final = 1;
else if(start_logging_final == 1)
start_logging_final = 0;
break;
//Retorna as constantes utilizadas no PID.
case 'Y':
limpar_buffer(buffer_dados_tx,33);
retornar_parametros_pid(&kp, &ki, &kd);
buffer_dados_tx[0] = '%';
conversor.flutuante_entrada = kp;
copy_to(buffer_dados_tx, conversor.bytes, 1, 4);
conversor.flutuante_entrada = ki;
copy_to(buffer_dados_tx, conversor.bytes, 5, 4);
conversor.flutuante_entrada = kd;
copy_to(buffer_dados_tx, conversor.bytes, 9, 4);
escrita_dados(SPI2, buffer_dados_tx, 32);
break;
//Retorna as convariâncias e variância utilizadas no filtro de Kalman.
case 'J':
limpar_buffer(buffer_dados_tx,33);
//FIXME: Consertar método para envio dos parâmetros do filtro de Kalman
//retornar_parametros_Kalman(&Q_acelerometro, &Q_magnetometro, &Q_bias, &R_acelerometro, &R_magnetometro);
buffer_dados_tx[0] = '^';
conversor.flutuante_entrada = Q_acelerometro;
copy_to(buffer_dados_tx, conversor.bytes, 1, 4);
conversor.flutuante_entrada = Q_magnetometro;
copy_to(buffer_dados_tx, conversor.bytes, 5, 4);
conversor.flutuante_entrada = Q_bias;
copy_to(buffer_dados_tx, conversor.bytes, 9, 4);
conversor.flutuante_entrada = R_acelerometro;
copy_to(buffer_dados_tx, conversor.bytes, 13, 4);
conversor.flutuante_entrada = R_magnetometro;
copy_to(buffer_dados_tx, conversor.bytes, 17, 4);
escrita_dados(SPI2, buffer_dados_tx, 32);
break;
//Altera as constantes utilizadas no PID.
case 'U':
conversor.bytes[0] = buffer_dados_rx[1];
conversor.bytes[1] = buffer_dados_rx[2];
conversor.bytes[2] = buffer_dados_rx[3];
conversor.bytes[3] = buffer_dados_rx[4];
kp = conversor.flutuante_entrada;
conversor.bytes[0] = buffer_dados_rx[5];
conversor.bytes[1] = buffer_dados_rx[6];
conversor.bytes[2] = buffer_dados_rx[7];
conversor.bytes[3] = buffer_dados_rx[8];
kd = conversor.flutuante_entrada;
conversor.bytes[0] = buffer_dados_rx[9];
conversor.bytes[1] = buffer_dados_rx[10];
conversor.bytes[2] = buffer_dados_rx[11];
conversor.bytes[3] = buffer_dados_rx[12];
ki = conversor.flutuante_entrada;
conversor.bytes[0] = buffer_dados_rx[13];
conversor.bytes[1] = buffer_dados_rx[14];
conversor.bytes[2] = buffer_dados_rx[15];
conversor.bytes[3] = buffer_dados_rx[16];
kp_yaw = conversor.flutuante_entrada;
conversor.bytes[0] = buffer_dados_rx[17];
conversor.bytes[1] = buffer_dados_rx[18];
conversor.bytes[2] = buffer_dados_rx[19];
conversor.bytes[3] = buffer_dados_rx[20];
kd_yaw = conversor.flutuante_entrada;
conversor.bytes[0] = buffer_dados_rx[21];
conversor.bytes[1] = buffer_dados_rx[22];
conversor.bytes[2] = buffer_dados_rx[23];
conversor.bytes[3] = buffer_dados_rx[24];
ki_yaw = conversor.flutuante_entrada;
//setar_parametros_PID(kp,ki,kd, 126, kp_yaw, ki_yaw, kd_yaw, 126); //Insere os parametros no processo de controle.
setar_parametros_PID(kp,ki,kd, 126, 0, 0, 0, 126); //Insere os parametros no processo de controle.
break;
//Altera as constantes utilizadas no filtro de Kalman.
case 'T':
conversor.bytes[0] = buffer_dados_rx[1];
conversor.bytes[1] = buffer_dados_rx[2];
conversor.bytes[2] = buffer_dados_rx[3];
conversor.bytes[3] = buffer_dados_rx[4];
Q_acelerometro = conversor.flutuante_entrada;
conversor.bytes[0] = buffer_dados_rx[5];
conversor.bytes[1] = buffer_dados_rx[6];
conversor.bytes[2] = buffer_dados_rx[7];
conversor.bytes[3] = buffer_dados_rx[8];
Q_magnetometro = conversor.flutuante_entrada;
conversor.bytes[0] = buffer_dados_rx[9];
conversor.bytes[1] = buffer_dados_rx[10];
conversor.bytes[2] = buffer_dados_rx[11];
conversor.bytes[3] = buffer_dados_rx[12];
Q_bias = conversor.flutuante_entrada;
conversor.bytes[0] = buffer_dados_rx[13];
conversor.bytes[1] = buffer_dados_rx[14];
conversor.bytes[2] = buffer_dados_rx[15];
conversor.bytes[3] = buffer_dados_rx[16];
R_acelerometro = conversor.flutuante_entrada;
conversor.bytes[0] = buffer_dados_rx[17];
conversor.bytes[1] = buffer_dados_rx[18];
conversor.bytes[2] = buffer_dados_rx[19];
conversor.bytes[3] = buffer_dados_rx[20];
R_magnetometro = conversor.flutuante_entrada;
//Qang, Qbias, Qbiasmag, Racel, Rmag
//setar_parametros_Kalman(Q_acelerometro, Q_magnetometro, R_acelerometro, R_magnetometro, 1e-10); //Insere os parametros no processo de controle.
break;
//Se a sequência de caracteres recebida não possui um comando reconhecido, retorna o número de caracteres
//contidos no pacote.
default:
numToASCII((strlen((char *) buffer_dados_rx)), buffer_dados_tx);
escrita_dados(SPI2, buffer_dados_tx, 32);
break;
}
}
//Retorna ao modo de recepção.
modo_rx(SPI2);
}
}
//Função para envio dos dados da telemetria : Se a telemetria estiver ativada e o tempo entre o envio se passou.
if(variavel_delay_100ms == 0)
{
buffer_dados_tx[0] = 'L';
retornar_estado(telemetria_kalman, telemetria_pid);
conversor.flutuante_entrada = telemetria_kalman[0];
copy_to(buffer_dados_tx, conversor.bytes, 1, 4);
conversor.flutuante_entrada = telemetria_kalman[1];
copy_to(buffer_dados_tx, conversor.bytes, 5, 4);
conversor.flutuante_entrada = telemetria_kalman[2];
copy_to(buffer_dados_tx, conversor.bytes, 9, 4);
conversor.flutuante_entrada = telemetria_pid[0];
copy_to(buffer_dados_tx, conversor.bytes, 13, 4);
conversor.flutuante_entrada = telemetria_pid[1];
copy_to(buffer_dados_tx, conversor.bytes, 17, 4);
conversor.flutuante_entrada = telemetria_pid[2];
copy_to(buffer_dados_tx, conversor.bytes, 21, 4);
escrita_dados(SPI2, buffer_dados_tx, 32);
//Retorna ao modo de recepção para habilitar a chegada de novos pacotes.
// modo_rx(SPI2);
//Variável para controle de tempo entre os envios dos dados da telemetria.
//variavel_delay_100ms = 500; //500* 100 uS -> 1 Ponto a cada 50 mS
//}else if(variavel_delay_100ms == 0 && start_logging_sensores == 1 && start_logging_final == 0)
//{
buffer_dados_tx[0] = 'S';
retornar_estado_sensores(telemetria_acelerometro, telemetria_giroscopio, telemetria_magnetometro);
conversor.flutuante_entrada = telemetria_acelerometro[0];
copy_to(buffer_dados_tx, conversor.bytes, 1, 4);
conversor.flutuante_entrada = telemetria_acelerometro[1];
copy_to(buffer_dados_tx, conversor.bytes, 5, 4);
conversor.flutuante_entrada = telemetria_acelerometro[2];
copy_to(buffer_dados_tx, conversor.bytes, 9, 4);
conversor.flutuante_entrada = telemetria_giroscopio[0];
copy_to(buffer_dados_tx, conversor.bytes, 13, 4);
conversor.flutuante_entrada = telemetria_giroscopio[1];
copy_to(buffer_dados_tx, conversor.bytes, 17, 4);
conversor.flutuante_entrada = telemetria_giroscopio[2];
copy_to(buffer_dados_tx, conversor.bytes, 21, 4);
conversor.flutuante_entrada = telemetria_magnetometro[0];
copy_to(buffer_dados_tx, conversor.bytes, 25, 4);
//conversor.flutuante_entrada = telemetria_giroscopio[1];
//copy_to(buffer_dados_tx, conversor.bytes, 29, 4);
escrita_dados(SPI2, buffer_dados_tx, 32);
//Retorna ao modo de recepção para habilitar a chegada de novos pacotes.
modo_rx(SPI2);
variavel_delay_100ms = 250; //500* 100 uS -> 1 Ponto a cada 50 mS
}
}
}
