ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

answered 2020-11-02 03:51:36 -0600

Gualor gravatar image

Hi @duck-development! Firstly thank you for your quick answear!

I moved the sendDataThread code inside the main loop since there wasn't any reason to do it. Then I tried using mutex to lock the SPI write function, but I had no luck with it and it always gives me the same error as before, which is:

Assert failed: g_spiHandle[1], file: /home/gualor/.platformio/packages/framework-gap_sdk/mbed-os/targets/TARGET_GWT/TARGET_GAP8

++ MbedOS Error Info ++

Error Status: 0x8001011D Code: 285 Module: 1

Error Message:

Pre main thread not created

The code now looks something like this:

#include <mbed.h>
#include "ADS1299.h"
#include "Biquad.h"
#include "Definitions.h"
#include "ros.h"
#include "ads1299/ChannelData.h"


// ADS1299
ADS1299 ads;

// ROS stuff
ros::NodeHandle nh;
ads1299::ChannelData ads_msg;
ros::Publisher gap8("channel_data", &ads_msg);

// Thread
Thread thread;
osThreadId threadID;

// Prototypes
void sendDataThread(void); 
void dataReadyISR(void);

// Interrupt pin
InterruptIn DRDY(GPIO_A5_B40);


// Signal data ready
void dataReadyISR(void)
{
    osSignalSet(threadID, 0x05);
}


int main(void)
{
    // Setup ROS node
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.advertise(gap8);

    // Start connection
    while(!nh.connected())
    {   
        nh.spinOnce();
        printf("Connecting...\n");
        Thread::wait(1000);
    }
    printf("Connection enstablished.\n");

    // Get thread ID
    threadID = osThreadGetId();
    uint8_t counter = 0;
    Mutex mutex;

    // Start data reayd ISR
    DRDY.fall(dataReadyISR);

    for(;;)
    {   
        // Wait for data ready signal
        osSignalWait(0x05, osWaitForever);

        // Read new data
        mutex.lock();
        ads.updateChannelData();
        mutex.unlock();

        // Publish message every 1s
        if (counter == 25)
        {
            // Insert data inside payload
            ads_msg.ch1 = ads.channelData[0];
            ads_msg.ch2 = ads.channelData[1];
            ads_msg.ch3 = ads.channelData[2];
            ads_msg.ch4 = ads.channelData[3];
            ads_msg.ch5 = ads.channelData[4];
            ads_msg.ch6 = ads.channelData[5];
            ads_msg.ch7 = ads.channelData[6];
            ads_msg.ch8 = ads.channelData[7];

            // Publish message
            gap8.publish(&ads_msg);
            nh.spinOnce();
            counter = 0;
        }
        counter++;
    }
}

I thought the error was due to SPI write being interrupted, but mutex lock should have solved at least this issue. Could it be that the mutex failed to lock properly and interruption occurred anyway? Do you have any idea how I could debug this?

Thank you again for the help!

Hi @duck-development! Firstly thank you for your quick answear!

I moved the sendDataThread code inside the main loop since there wasn't any reason to do it. Then I tried using mutex to lock the SPI write function, but I had no luck with it and it always gives me the same error as before, which is:

Assert failed: g_spiHandle[1], file: /home/gualor/.platformio/packages/framework-gap_sdk/mbed-os/targets/TARGET_GWT/TARGET_GAP8

++ MbedOS Error Info ++

Error Status: 0x8001011D Code: 285 Module: 1

Error Message:

Pre main thread not created

The code now looks something like this:

#include <mbed.h>
#include "ADS1299.h"
#include "Biquad.h"
#include "Definitions.h"
#include "ros.h"
#include "ads1299/ChannelData.h"


// ADS1299
ADS1299 ads;

// ROS stuff
ros::NodeHandle nh;
ads1299::ChannelData ads_msg;
ros::Publisher gap8("channel_data", &ads_msg);

// Thread
Thread thread;
osThreadId threadID;

// Prototypes
void sendDataThread(void); 
void dataReadyISR(void);

// Interrupt pin
InterruptIn DRDY(GPIO_A5_B40);


// Signal data ready
void dataReadyISR(void)
{
    osSignalSet(threadID, 0x05);
}


int main(void)
{
    // Setup ROS node
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.advertise(gap8);

    // Start connection
    while(!nh.connected())
    {   
        nh.spinOnce();
        printf("Connecting...\n");
        Thread::wait(1000);
    }
    printf("Connection enstablished.\n");

    // Get thread ID
    threadID = osThreadGetId();
    uint8_t counter = 0;
    Mutex mutex;

    // Start data reayd ISR
    DRDY.fall(dataReadyISR);

    for(;;)
    {   
        // Wait for data ready signal
        osSignalWait(0x05, osWaitForever);

        // Read new data
        mutex.lock();
        ads.updateChannelData();
        mutex.unlock();

        // Publish message every 1s
        if (counter == 25)
        {
            // Insert data inside payload
            ads_msg.ch1 = ads.channelData[0];
            ads_msg.ch2 = ads.channelData[1];
            ads_msg.ch3 = ads.channelData[2];
            ads_msg.ch4 = ads.channelData[3];
            ads_msg.ch5 = ads.channelData[4];
            ads_msg.ch6 = ads.channelData[5];
            ads_msg.ch7 = ads.channelData[6];
            ads_msg.ch8 = ads.channelData[7];

            // Publish message
            gap8.publish(&ads_msg);
            nh.spinOnce();
            counter = 0;
        }
        counter++;
    }
}

I thought the error was due to SPI write being interrupted, but mutex lock should have solved at least this issue. Could it be that the mutex failed to lock properly and interruption occurred anyway? Do you have any idea how I could debug this?

Thank you again for the help!

Hi @duck-development! Firstly thank you for your quick answear!

I moved the sendDataThread code inside the main loop since there wasn't any reason to do it. Then I tried using mutex to lock the SPI write function, but I had no luck with it and it always gives me the same error as before, which is:

Assert failed: g_spiHandle[1], file: /home/gualor/.platformio/packages/framework-gap_sdk/mbed-os/targets/TARGET_GWT/TARGET_GAP8

++ MbedOS Error Info ++

Error Status: 0x8001011D Code: 285 Module: 1

Error Message:

Pre main thread not created

The code now looks something like this:

#include <mbed.h>
#include "ADS1299.h"
#include "Biquad.h"
#include "Definitions.h"
#include "ros.h"
#include "ads1299/ChannelData.h"


// ADS1299
ADS1299 ads;

// ROS stuff
ros::NodeHandle nh;
ads1299::ChannelData ads_msg;
ros::Publisher gap8("channel_data", &ads_msg);

// Thread
osThreadId threadID;

// Prototypes
void dataReadyISR(void);

// Interrupt pin
InterruptIn DRDY(GPIO_A5_B40);


// Signal data ready
void dataReadyISR(void)
{
    osSignalSet(threadID, 0x05);
}


int main(void)
{
    // Setup ROS node
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.advertise(gap8);

    // Start connection
    while(!nh.connected())
    {   
        nh.spinOnce();
        printf("Connecting...\n");
        Thread::wait(1000);
    }
    printf("Connection enstablished.\n");

    // Get thread ID
    threadID = osThreadGetId();
    uint8_t counter = 0;
    Mutex mutex;

    // Start data reayd ISR
    DRDY.fall(dataReadyISR);

    for(;;)
    {   
        // Wait for data ready signal
        osSignalWait(0x05, osWaitForever);

        // Read new data
        mutex.lock();
        ads.updateChannelData();
        mutex.unlock();

        // Publish message every 1s
        if (counter == 25)
        {
            // Insert data inside payload
            ads_msg.ch1 = ads.channelData[0];
            ads_msg.ch2 = ads.channelData[1];
            ads_msg.ch3 = ads.channelData[2];
            ads_msg.ch4 = ads.channelData[3];
            ads_msg.ch5 = ads.channelData[4];
            ads_msg.ch6 = ads.channelData[5];
            ads_msg.ch7 = ads.channelData[6];
            ads_msg.ch8 = ads.channelData[7];

            // Publish message
            gap8.publish(&ads_msg);
            nh.spinOnce();
            counter = 0;
        }
        counter++;
    }
}

I thought the error was due to SPI write being interrupted, but mutex lock should have solved at least this issue. Could it be that the mutex failed to lock properly and interruption occurred anyway? Do you have any idea how I could debug this?

Thank you again for the help!

UPDATE:

As suggested I tried having 2 threads running, one for acquiring data via SPI and one for publishing ROS messaging and calling spin once. Saddly it seems to behave exactly like before even with the use of mutexes inside the code:

#include "mbed.h"
#include "ADS1299.h"
#include "Definitions.h"
#include "ros.h"
#include "ads1299/ChannelData.h"

// ADS1299
ADS1299 ads;

// ROS stuff
ros::NodeHandle nh;
ads1299::ChannelData ads_msg;
ros::Publisher gap8("channel_data", &ads_msg);

// Threads
osThreadId id;
Thread thread1;
Thread thread2(osPriorityAboveNormal1);
void acquireThread(void);
void publishThread(void);
Mutex mutex;

// Interrupt
InterruptIn DRDY(GPIO_A5_B40);  
void dataReadyISR(void);


// Data ready interrupt
void dataReadyISR(void)
{
    osSignalSet(id, 0x01);
}

// Acquire data from ADS1299
void acquireThread(void)
{
    id = ThisThread::get_id();
    for(;;)
    {
        osSignalWait(0x01, osWaitForever);
        mutex.lock();
        ads.updateChannelData();
        mutex.unlock();
    }
}

// Publish ROS messages
void publishThread(void)
{
    for(;;)
    {
        // Update message data
        ads_msg.ch1 = ads.channelData[0];
        ads_msg.ch2 = ads.channelData[1];
        ads_msg.ch3 = ads.channelData[2];
        ads_msg.ch4 = ads.channelData[3];
        ads_msg.ch5 = ads.channelData[4];
        ads_msg.ch6 = ads.channelData[5];
        ads_msg.ch7 = ads.channelData[6];
        ads_msg.ch8 = ads.channelData[7];

        // Publish message        
        mutex.lock();
        gap8.publish(&ads_msg);
        nh.spinOnce();
        mutex.unlock();

        ThisThread::sleep_for(100);
    }
}

int main() 
{
    // ROS setup
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.advertise(gap8);

    // Start threads
    thread1.start(callback(&acquireThread));
    thread2.start(callback(&publishThread));

    // Attach data ready ISR
    DRDY.fall(callback(&dataReadyISR));

    Thread::wait(osWaitForever);
}
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
about | faq | help | privacy policy | terms of service
Powered by Askbot version 0.10.2