int main(void) {
uint8_t data[] = {0, 1, 2};
/* Initialize system */
SystemInit();
/* Initialize I2C, custom pinout with 100kHt serial clock */
TM_I2C_Init(I2C1, TM_I2C_PinsPack_Custom, 100000);
/* Write "5" at location 0x00 to slave with address ADDRESS */
TM_I2C_Write(I2C1, ADDRESS, 0x00, 5);
/**
* Write multi bytes to slave with address ADDRESS
* Write to registers starting from 0x00, get data in variable "data" and write 3 bytes
*/
TM_I2C_WriteMulti(I2C1, ADDRESS, 0x00, data, 3);
/* Read single byte from slave with 0xD0 (1101 000 0) address and register location 0x00 */
data[0] = TM_I2C_Read(I2C1, ADDRESS, 0x00);
/**
* Read 3 bytes of data from slave with 0xD0 address
* First register to read from is at 0x00 location
* Store received data to "data" variable
*/
TM_I2C_ReadMulti(I2C1, 0xD0, 0x00, data, 3);
while (1) {
}
}
void show_i2c(void) {
I2C_DeInit(I2C3);
TM_I2C_Init(I2C3, TM_I2C_PinsPack_1, 200000);
for (int i=0;i<0xFF;i++)
if (TM_I2C_IsDeviceConnected(I2C3, i))
printf("Device available at address: %02X\n\r",i);
}
bool SFP_Init() {
I2C_DeInit(SFP_I2C_PORT);
TM_I2C_Init(SFP_I2C_PORT, SFP_I2C_PINS, SFP_I2C_SPEED);
if (TM_I2C_IsDeviceConnected(SFP_I2C_PORT, SFP_I2C_ADDRESS)) {
return true;
}
return false;
}
bool SI7021_begin() {
_si_exists = false;
I2C_DeInit(SI7021_I2C_PORT);
TM_I2C_Init(SI7021_I2C_PORT, SI7021_I2C_PINS, SI7021_I2C_SPEED);
if (TM_I2C_IsDeviceConnected(SI7021_I2C_PORT, SI7021_ADDRESS)) {
_si_exists = true;
}
return _si_exists;
}
int main(void) {
/* Init system clock for maximum system speed */
TM_RCC_InitSystem();
/* Init HAL layer */
HAL_Init();
/* Init leds */
TM_DISCO_LedInit();
/* Init delay */
TM_DELAY_Init();
/* Init I2C, use custom pins, callback function will be caleed */
/* For STM32F4xx and STM32F7xx lines */
TM_I2C_Init(I2C1, TM_I2C_PinsPack_Custom, 100000);
while (1) {
}
}
int main(void) {
uint16_t i;
uint8_t devices_found = 0;
char buffer[20];
/* Initialize system */
SystemInit();
/* Initialize delay */
TM_DELAY_Init();
/* Initialize leds on board */
TM_DISCO_LedInit();
/* Init LCD on STM32F429-Discovery board */
TM_ILI9341_Init();
TM_ILI9341_Rotate(TM_ILI9341_Orientation_Portrait_2);
/* Set background color */
TM_ILI9341_Fill(0x1234);
/* Init I2C3 peripheral, PA8, PC9 */
TM_I2C_Init(I2C3, TM_I2C_PinsPack_1, 100000);
/* Make search and show on LCD */
for (i = 2; i < 0xFF; i += 2) {
/* Check if device detected */
if (TM_I2C_IsDeviceConnected(I2C3, i)) {
/* Format string */
sprintf(buffer, "Device: %02X", i);
/* Print to LCD */
TM_ILI9341_Puts(10, 10 + 12 * devices_found++, buffer, &TM_Font_7x10, 0x0000, 0x1234);
}
}
while (1) {
}
}
void TM_DS1307_Init(void) {
TM_I2C_Init(TM_DS1307_I2C, TM_DS1307_I2C_PINSPACK, 50000);
}
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
}
}
}
