Estoy haciendo un mouse giroscópico usando MPU6050, NRF24L01 + conectado a la placa STM32F103, todo soldado a un tablero de separación. El STM32 lee el sensor y envía datos a través de NRF24 a otro NRF24 conectado a una mochila USB (Arduino Leonardo). Este dongle realiza los comandos del ratón. Actualmente, solo estoy usando el giro y la inclinación para controlar los comandos dx y dy.
Los datos enviados (Yaw Pitch Roll) son estables solo por unos minutos, incluso unos pocos segundos dependen de cómo muevo el tablero. Después de eso, el trazador en serie mostró las señales con picos de ruido aleatorios entre los datos correctos enviados. Durante este tiempo, pude ver que el cursor del mouse también es más lento, lo que indica retrasos en el flujo de datos. Observé que cuanto más me alejo del dispositivo USB, es más probable que este flujo de datos se vuelva inestable. Sin embargo, este rango sería inusualmente corto, a veces solo 30-40 cm es suficiente para causar esto. También los picos casi siempre aparecían después de la pérdida de los primeros paquetes. Sospecho que el módulo NRF24 podría estar fallando ya que tengo los datos del MPU6050 impresos en el USART de STM32 y la gráfica no tenía picos en el lado del receptor.
Usé
Aquí está el código que usé para los dispositivos TX y RX:
TX:
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "delay.h"
#include "usart.h"
#include "mpu6050.h"
#include "nrf24l01.h"
#include "stm32f10x.h"
// Timeout counter (depends on the CPU speed)
// Used for not stuck waiting for IRQ
#define nRF24_WAIT_TIMEOUT (uint32_t)0x000FFFFF
// Result of packet transmission
typedef enum {
nRF24_TX_ERROR = (uint8_t)0x00, // Unknown error
nRF24_TX_SUCCESS, // Packet has been transmitted successfully
nRF24_TX_TIMEOUT, // It was timeout during packet transmit
nRF24_TX_MAXRT // Transmit failed with maximum auto retransmit count
} nRF24_TXResult;
// Configure TX PIPE
static const uint8_t nRF24_ADDR[] = { '4','2','F','R','n' };
// MPU interrupt flag
volatile bool mpuIntFlag = false;
volatile bool nrfIntFlag = false;
nRF24_TXResult nRF24_TransmitPacket(uint8_t *pBuf, uint8_t length);
void nRF24_Config(void);
void EXTI0_Config(void);
/**************************** MAIN PROGRAM ****************************************/
int main(void)
{
/*********************
Variable initialization
*********************/
char buf[50];
uint16_t fifoCount;
uint8_t mpuIntStatus;
Quaternion q;
VectorFloat g;
float ypr[3];
uint8_t nRF24_payload[32]; // Buffer to store a payload of maximum width
uint8_t payload_length; // Length of received payload
uint32_t packets_lost = 0; // global counter of lost packets
uint8_t otx;
uint8_t otx_plos_cnt; // lost packet count
uint8_t otx_arc_cnt; // retransmit count
// TX status return
nRF24_TXResult tx_res;
// DMP status return
bool dmpReady = false;
/*********************
Default startup: HSI (8MHz), HSE (25MHz) and PLL (72MHz) enabled.
System clock source is set to PLL = 72MHz.
High-speed peripherals PCLK2 = HCLK
Low-speed peripherals PCLK1 = HCLK/2
*********************/
RCC_HCLKConfig(RCC_SYSCLK_Div1);
RCC_PCLK1Config(RCC_HCLK_Div2);
RCC_PCLK2Config(RCC_HCLK_Div1);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
/*Initialize SysTick module*/
SysTick_Init();
/*Initialize USART1 peripheral*/
USART1_Init();
/* Initialize SPI peripheral for nRF24 */
nRF24_SPI_Init();
USART_puts(USART1, "nRF24L01+ check: ");
if (!nRF24_Check()) {
USART_puts(USART1, "FAIL\r\n");
while (1);
}
USART_puts(USART1, "OK\r\n");
// Initialize the nRF24L01 to its default state
nRF24_Init();
// This is simple transmitter (to one logic address):
// - TX address: 'nRF24'
// - payload: 12 bytes
// - RF channel: 96 ()
// - data rate: 2Mbps
// - CRC scheme: 2 byte
// - AutoACK: PIPE0 receive ACK payload, 15 retries with 250us in-between
// The transmitter sends a 12-byte packets to the address 'nRF24' with Auto-ACK (ShockBurst enabled)
nRF24_Config();
nRF24_DumpConfig();
SPI_CE(SET);
/* MPU6050 Initialization */
MPU6050_I2C_Init();
EXTI0_Config();
if(!MPU6050_DMPInitialize()) {
MPU6050_setDMPEnabled(true);
// Read to clear all interrupts
MPU6050_getIntStatus();
USART_puts(USART1, "Initialization success\r\n");
dmpReady = true;
}
//Variable values
payload_length = 12;
nrfIntFlag = false;
PC13_debugSuccess();
while (1)
{
// Prevent further action if initialization failed
while(!dmpReady) USART_puts(USART1, "DMP failed\r\n");;
// Wait for MPU interrupt
while(!mpuIntFlag) {
// Other program code here
}
mpuIntFlag = false;
mpuIntStatus = MPU6050_getIntStatus();
fifoCount = MPU6050_getFIFOCount();
if((mpuIntStatus & 0x10) || fifoCount == 1024) {
MPU6050_resetFIFO();
USART_puts(USART1, "FIFO Overflow!\r\n");
}
else if(mpuIntStatus & 0x02) {
while(fifoCount < MPU6050_DMP_PACKET_SIZE) fifoCount = MPU6050_getFIFOCount();
sprintf(buf, "fifo = %d\r\n", fifoCount);
USART_puts(USART1, buf);
MPU6050_getFIFOBytes(fifoBuf, MPU6050_DMP_PACKET_SIZE);
fifoCount -= MPU6050_DMP_PACKET_SIZE;
MPU6050_DMPGetQuaternion(&q, fifoBuf);
MPU6050_DMPGetGravity(&g, &q);
MPU6050_DMPGetYawPitchRoll(ypr, &q, &g);
sprintf(buf, "%.2f %.2f %.2f\r\n", ypr[0], ypr[1], ypr[2]);
USART_puts(USART1, buf);
// Transmit a packet
tx_res = nRF24_TransmitPacket((unsigned char*)ypr, payload_length);
otx = nRF24_GetRetransmitCounters();
otx_plos_cnt = (otx & nRF24_MASK_PLOS_CNT) >> 4; // packets lost counter
otx_arc_cnt = (otx & nRF24_MASK_ARC_CNT); // auto retransmissions counter
switch (tx_res) {
case nRF24_TX_SUCCESS:
USART_puts(USART1,"OK");
break;
case nRF24_TX_TIMEOUT:
USART_puts(USART1,"TIMEOUT");
//MPU6050_resetFIFO();
break;
case nRF24_TX_MAXRT:
USART_puts(USART1,"MAX RETRANSMIT");
MPU6050_resetFIFO();
packets_lost += otx_plos_cnt;
break;
default:
USART_puts(USART1,"ERROR");
break;
}
USART_puts(USART1,"\r\n");
sprintf(buf, "ARC=%u \t LOST=%u\r\n", otx_arc_cnt, packets_lost);
USART_puts(USART1, buf);
}
}
}
/************************************ FUNCTION DEFINITIONS ***********************************************/
void nRF24_Config(void) {
// Set RF channel
nRF24_SetRFChannel(96);
// Set data rate
nRF24_SetDataRate(nRF24_DR_2Mbps);
// Set CRC scheme
nRF24_SetCRCScheme(nRF24_CRC_2byte);
// Set address width, its common for all pipes (RX and TX)
nRF24_SetAddrWidth(5);
nRF24_SetAddr(nRF24_PIPETX, nRF24_ADDR); // program TX address
nRF24_SetAddr(nRF24_PIPE0, nRF24_ADDR);
// Set TX power (maximum)
nRF24_SetTXPower(nRF24_TXPWR_0dBm);
//Set auto retransmit: 10 retries for 2500us
nRF24_SetAutoRetr(nRF24_ARD_250us, 15);
nRF24_EnableAA(nRF24_PIPE0);
// Set operational mode (PTX == transmitter)
nRF24_SetOperationalMode(nRF24_MODE_TX);
// Wake the transceiver
nRF24_SetPowerMode(nRF24_PWR_UP);
// Clear any pending IRQ flags
nRF24_ClearIRQFlags();
}
void EXTI0_Config(void) {
EXTI_InitTypeDef EXTI_InitStruct;
NVIC_InitTypeDef NVIC_InitStruct;
// EXTI pin for MPU6050 interrupt
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource0);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource12);
EXTI_InitStruct.EXTI_Line = EXTI_Line0;
EXTI_InitStruct.EXTI_LineCmd = ENABLE;
EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising;
EXTI_Init(&EXTI_InitStruct);
EXTI_InitStruct.EXTI_Line = EXTI_Line12;
EXTI_InitStruct.EXTI_LineCmd = ENABLE;
EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Falling;
EXTI_Init(&EXTI_InitStruct);
NVIC_InitStruct.NVIC_IRQChannel = EXTI0_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00;
NVIC_Init(&NVIC_InitStruct);
NVIC_InitStruct.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00;
NVIC_Init(&NVIC_InitStruct);
}
// Function to transmit data packet
// input:
// pBuf - pointer to the buffer with data to transmit
// length - length of the data buffer in bytes
// return: one of nRF24_TX_xx values
nRF24_TXResult nRF24_TransmitPacket(uint8_t *pBuf, uint8_t length) {
char strBuff[50];
volatile uint32_t wait = nRF24_WAIT_TIMEOUT;
uint8_t status;
// Deassert the CE pin (in case if it still high)
SPI_CE(RESET);
// Transfer a data from the specified buffer to the TX FIFO
nRF24_WritePayload(pBuf, length);
// Start a transmission by asserting CE pin (must be held at least 10us)
SPI_CE(SET);
// Poll the INT pin and check the status:
// TX_DS - means the packet has been transmitted
// MAX_RT - means the maximum number of TX retransmits happened
do {
if(nrfIntFlag) {
nrfIntFlag = false;
status = nRF24_GetStatus();
// if (status & (nRF24_FLAG_TX_DS | nRF24_FLAG_MAX_RT)) {
// break;
// }
break;
}
} while (wait--);
// Deassert the CE pin (Standby-II --> Standby-I)
SPI_CE(RESET);
if (!wait) {
// Timeout
return nRF24_TX_TIMEOUT;
}
// Clear pending IRQ flags
nRF24_ClearIRQFlags();
if (status & nRF24_FLAG_MAX_RT) {
// Auto retransmit counter exceeds the programmed maximum limit (FIFO is not removed)
return nRF24_TX_MAXRT;
}
if (status & nRF24_FLAG_TX_DS) {
// Successful transmission
return nRF24_TX_SUCCESS;
}
// Some banana happens, a payload remains in the TX FIFO, flush it
nRF24_FlushTX();
return nRF24_TX_ERROR;
}
void EXTI0_IRQHandler(void)
{
if(EXTI_GetITStatus(EXTI_Line0)) {
mpuIntFlag = true;
}
EXTI_ClearITPendingBit(EXTI_Line0);
}
void EXTI15_10_IRQHandler(void)
{
if(EXTI_GetITStatus(EXTI_Line12)) {
nrfIntFlag = true;
GPIOC->ODR ^= 1ul << 13;
}
EXTI_ClearITPendingBit(EXTI_Line12);
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t* file, uint32_t line)
{
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* Infinite loop */
while (1)
{
}
}
#endif
/**
* @}
*/
/**
* @}
*/
/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/
RX:
#include <nRF24L01.h>
#include <printf.h>
#include <RF24.h>
#include <RF24_config.h>
#include <Mouse.h>
#define XSF 35
#define YSF -38
RF24 radio(9,10);
byte addresses[][6] = {"nRF24","Node1"};
float YawPitchRoll[3], prevYawPitchRoll[3];
int dx = 0, dy = 0;
typedef enum {cal_state, main_state} STATES;
byte pipeNo, gotByte[12];
uint32_t init_timer = 0;
uint32_t sample_cnt = 0;
STATES prog_state;
void readYPR(byte* pipeNum, byte* bytePayload, float* ypr) {
while(!radio.available(pipeNum)); //Check which pipe has data available
radio.read(bytePayload, 12); //Read 12 byte payload
for(int i = 0; i < 12; i += 4) {
byte fBuffer[4] = {bytePayload[i],bytePayload[i+1],bytePayload[i+2],bytePayload[i+3]};
if(i == 0) {
ypr[0] = *( (float*) fBuffer);
} else
if(i == 4) {
ypr[1] = *( (float*) fBuffer);
} else
if(i == 8) {
ypr[2] = *( (float*) fBuffer);
}
}
return ypr[3];
}
void setup() {
//Initialize peripherals
Serial.begin(115200);
printf_begin();
radio.begin();
Mouse.begin();
//NRF24 settings
radio.setAutoAck(true);
radio.setPayloadSize(12); //Set RX payload size = TX payload size
radio.setAddressWidth(5); //Longer address width and CRC bits ensure
radio.setCRCLength(RF24_CRC_16); //ensure more reliable transmission and reception
radio.setPALevel(RF24_PA_MAX); //Modules TX/RX close together operating at high power
//run the risk of saturating
radio.setChannel(96);
radio.setDataRate(RF24_2MBPS);
radio.openReadingPipe(0, addresses[0]); //Set address of RX_PIPE0 = TX_PIPE
radio.startListening();
radio.printDetails();
Serial.println("Start calibrating, wait 9s");
init_timer = millis();
}
void loop() {
// put your main code here, to run repeatedly:
switch(prog_state) {
case cal_state:
readYPR(&pipeNo, gotByte, prevYawPitchRoll);
prog_state = (millis()-init_timer >= 9000) ? main_state : cal_state;
break;
case main_state:
//init_timer = millis();
readYPR(&pipeNo, gotByte, YawPitchRoll);
dx += (YawPitchRoll[0]-prevYawPitchRoll[0])*XSF;
dy += (YawPitchRoll[1]-prevYawPitchRoll[1])*YSF;
prevYawPitchRoll[0] = YawPitchRoll[0];
prevYawPitchRoll[1] = YawPitchRoll[1];
Serial.print(YawPitchRoll[0]);
Serial.print(", ");
Serial.print(YawPitchRoll[1]);
Serial.print(", ");
Serial.println(YawPitchRoll[2]);
Mouse.move(dx, dy, 0);
dx = 0; dy = 0;
break;
}
}