I'm currently trying to acquire data from an inertial measurement unit and to publish those data through mqtt.
To do this, I have one periodic task launched on sensor interrupt (at 119Hz) which acquire data from the sensor and send it into a queue and a publish task which gets data from the queue and send it through mqtt.
The acquire task (T1) is high priority and the second task (T2) is lower priority.
The acquisition works well as the publication. My problem is that when I integrate both tasks, the publishing tasks makes the sensor to crash.
I explain my logic :
When the program starts, it initialise all the ressources (mongoose mqtt, gpio, queue, etc...) then it define an ISR on a gpio to launch the acquisition task each time it receives a rising edge.
At the end of the first execution of T1, T2 is started, the only thing this task do is getting data from queue, encode it and send it through mqtt inside an infinite loop.
The program works for few periods and then crash. What is strange is that when it crashes, I have no more interruption on my gpio so my acquisition task is not called again.
If I don't launch the publishing task at the end of the first T1 execution, the program runs indefinitely without crashing (except when the queue is full but that's normal). So I think that the publish task is the problem but I don't see why and how it could stop my sensor sending me interrupts.
I have checked with an oscilloscope and indeed, my sensor doesn't send any interrupt.
I'm programming on ESP32DevKitC and my sensor is a LSM9DS1 embedded on mikroe 9DoF click breackout board.
I give you my code :
Code: Select all
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "esp_wifi.h"
#include "esp_system.h"
#include "esp_event.h"
#include "esp_event_loop.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "driver/i2c.h"
#include "driver/gpio.h"
#include "mongoose.h"
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include "lwip/inet.h"
#include "lwip/ip4_addr.h"
#include "esp_event.h"
#include "mpu.h"
#include "LSM9DS1_Reg.h"
#include "base64.h"
// Defines for WiFi
#define SSID "<SSID>"
#define PASSWORD "<PASSWORD>"
//Define for publish event
#define PUBLISH_BIT 0x01
//Queue message struct
struct sampleMessage
{
int messageId;
float gyrData[3];
float accData[3];
};
// Task functions
void acqTask();
void publishTask();
//Networking functions
esp_err_t wifi_event_cb(void *ctx, system_event_t *event);
void mongooseTask(void *data);
void mongoose_event_cb(struct mg_connection *nc, int ev, void *evData);
char *mongoose_eventToString(int ev);
char *mgStrToStr(struct mg_str mgStr);
//Encoding functions
void encodeMessage(struct sampleMessage msg, char * buffer);
//Broker informations
const char *s_topic = "/test/slot1";
const char * s_serverAddr = "IP:PORT";
struct mg_mqtt_topic_expression s_topic_expr = {"/test/slot1", 0};
struct mg_connection * mqttConnection = NULL;
//Wifi network informations
ip4_addr_t net_ip;
ip4_addr_t net_gw;
ip4_addr_t net_msk;
bool b_isConnected = false;
//I2C Bus
i2c_port_t i2cBus = I2C_NUM_0;
//GPIO pins
int acqPin = GPIO_NUM_4;
//Task handlers
static TaskHandle_t acqTask_handler = NULL;
static TaskHandle_t publishTask_handler = NULL;
//Sample Queue
QueueHandle_t sampleQueue_handler = NULL;
//Sensors scale values
float accScale, gyrScale, magScale;
void gpio_isr_acq_handler(void * arg)
{
xTaskCreate(&acqTask, "acqTask", 2048, NULL, configMAX_PRIORITIES-1, &acqTask_handler);
}
void app_main(void)
{
ESP_ERROR_CHECK(esp_event_loop_init(wifi_event_cb, NULL));
/////////////////////// Wifi connection //////////////////////
nvs_flash_init();
tcpip_adapter_init();
wifi_init_config_t wifiConfig = WIFI_INIT_CONFIG_DEFAULT();
ESP_ERROR_CHECK(esp_wifi_init(&wifiConfig));
ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM));
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
wifi_config_t sta_config = {
.sta =
{
.ssid = SSID,
.password = PASSWORD,
.bssid_set = 0
}
};
ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &sta_config));
ESP_ERROR_CHECK(esp_wifi_start());
ESP_ERROR_CHECK(esp_wifi_connect());
while(!b_isConnected)
{}
// GPIO config
gpio_config_t io_conf;
//Acq pin
io_conf.intr_type = GPIO_PIN_INTR_POSEDGE;
io_conf.pin_bit_mask = (1 << acqPin);
io_conf.mode = GPIO_MODE_INPUT;
io_conf.pull_up_en = 0;
io_conf.pull_down_en = 1;
gpio_config(&io_conf);
// I2C config
i2c_master_init(i2cBus, 18, 19);
accSetDataRate(i2cBus, A_ODR_119_HZ);
accScale = accSetScale(i2cBus, A_SCALE_2G);
accEnableOutputs(i2cBus);
gyrSetDataRate(i2cBus, G_ODR_119_HZ);
gyrScale = gyrSetScale(i2cBus, G_SCALE_245DPS);
gyrEnableOutputs(i2cBus);
enableMultipleByteAccess(i2cBus);
setFifoThreshold(i2cBus, 2);
enableFifoThreshold(i2cBus);
setFifoMode(i2cBus, FIFO_MODE_CONTINUOUS);
enableFifo(i2cBus);
setInt1Source(i2cBus, INT1_SRC_FTH);
setInt2Source(i2cBus, INT2_SRC_OVR);
//Create communication queue
sampleQueue_handler = xQueueCreate(100, sizeof(struct sampleMessage));
//ISR callbacks
gpio_install_isr_service(0);
gpio_isr_handler_add(acqPin, gpio_isr_acq_handler, NULL);
clearFifo(i2cBus);
}
void acqTask()
{
float acc_xyz[3];
float gyr_xyz[3];
static int sampleCount = 0;
readGyr(i2cBus, gyrScale, &gyr_xyz[0]);
readAcc(i2cBus, accScale, &acc_xyz[0]);
struct sampleMessage msg;
msg.messageId = sampleCount;
memcpy(&msg.gyrData, gyr_xyz, sizeof(gyr_xyz));
memcpy(&msg.accData, acc_xyz, sizeof(acc_xyz));
if(sampleQueue_handler != 0)
{
// Send an unsigned long. Wait for 10 ticks for space to become available if necessary.
if(xQueueSend(sampleQueue_handler, (void *) &msg, ( TickType_t ) 10) != pdPASS)
{
printf("[Error] Failed to send message to queue\n");
}
else
{
sampleCount += 1;
}
}
if(publishTask_handler == NULL)
{
xTaskCreate(&publishTask, "publishTask", 10000, NULL, configMAX_PRIORITIES-2, &publishTask_handler);
}
vTaskDelete(NULL);
}
void publishTask()
{
char encodingBuffer[500];
char b64_message[2400];
struct sampleMessage receivedMessage;
while(true)
{
if(sampleQueue_handler != 0)
{
// Receive a message on the created queue. Block indefinitely until a message is available.
if(xQueueReceive(sampleQueue_handler, &receivedMessage, ( TickType_t ) 10))
{
//Format and encode message to base64
encodeMessage(receivedMessage, encodingBuffer);
mg_mqtt_publish(mqttConnection, s_topic, 50, 0, encodingBuffer, strlen(encodingBuffer));
}
else
{
printf("[Error] Failed to receive message from queue\n\n");
}
}
}
vTaskDelete(NULL);
}
esp_err_t wifi_event_cb(void *ctx, system_event_t *event)
{
if(event->event_id == SYSTEM_EVENT_STA_GOT_IP)
{
net_ip = event->event_info.got_ip.ip_info.ip;
net_gw = event->event_info.got_ip.ip_info.gw;
net_msk = event->event_info.got_ip.ip_info.netmask;
b_isConnected = true;
printf("Connected to %s\n", SSID);
printf("\tIP: %s\n", inet_ntoa(net_ip));
printf("\tNetmask: %s\n", inet_ntoa(net_msk));
printf("\tGateway: %s\n\n", inet_ntoa(net_gw));
xTaskCreatePinnedToCore(&mongooseTask, "mongooseTask", 20000, NULL, 5, NULL,0);
}
return ESP_OK;
}
// FreeRTOS task to start Mongoose.
void mongooseTask(void *data)
{
printf("Mongoose task starting\n");
struct mg_mgr mgr;
printf("Mongoose: Starting setup\n");
mg_mgr_init(&mgr, NULL);
printf("Mongoose: Succesfully inited\n");
printf("Mongoose: Starting connection to %s\n", s_serverAddr);
mqttConnection = mg_connect(&mgr, s_serverAddr, mongoose_event_cb);
if (mqttConnection == NULL)
{
printf("Mongoose: Impossible to connect to %s\n", s_serverAddr);
vTaskDelete(NULL);
return;
}
printf("Mongoose: Successfully connected\n");
mg_set_protocol_mqtt(mqttConnection);
mg_send_mqtt_handshake(mqttConnection, "dummy");
while(true)
{
mg_mgr_poll(&mgr, 1000);
}
}
void mongoose_event_cb(struct mg_connection *nc, int ev, void *evData)
{}
If someone already has such a problem I would be glad to have some advices
Thanks in advance.