[Feature Request]: Make Micro-ROS publish magnetic sensor information on interrupt

[Legohead259]

Is your feature request related to a problem? Please describe. The polling method for getting and publishing sensor data is slow and results in a slight time lag between the when the data is recorded and when it is reported. This could cause issues with precise logging applications.

Describe the solution you'd like The magnetic sensor should trigger an interrupt whenever new data is ready. The microcontroller can tie the interrupt to an ISR function that gets and publishes data accordingly. This will reduce polling calls and have the sensor drive capturing and reporting, rather than a timer on the microcontroller which can be subject to inconsistency.

1. The sensor takes a reading and converts it according to its internal memory.
2. The sensor pulls the INT pin LOW once conversions are completed
3. The microcontroller senses the transition from HIGH to LOW and executes an ISR
4. The ISR gets the sensor position, and calculates the angular velocity, angular acceleration, and error* values

Function angularPositionPublishISR():
    Get angular position from sensor
    Calculate angular velocity
    Calculate angular acceleration 
    Calculate errors (?)
    Publish an angularStats message

Describe alternatives you've considered The polling method is simple, but limited for precise applications.

Additional context For the TMAG5273 used on the Sparkfun IOT Brushless Driver board, the INT_CONFIG_1 Register should have the following configurations:

Bit	Field	Value	Description
7	RSLT_INT	1	Interrupt is asserted when a set of conversions is completed
6	THRSLD_INT	0	Interrupt is asserted when a threshold is crossed
5	INT_STATE	1	Interrupt is pulsed for 10 us
4-2	INT_MODE	1	Interrupt through !INT line
1	RESERVED	0
0	MASK_INTB	0	INT pin is enabled

Follow Section 8.2.3 Angle Measurement in the TMAG5273 datasheet for more information.

[Legohead259]

I'm at the point in this feature where I curse the person who decides to make rocks do math.....

So, publishing from an interrupt is not feasible. Takes to long to execute. Need to create a shared sensor data object that can be accessed by the FOC algorithm and publisher simultaneously. The ISR can update this shared value. Then, just need a muted to control access to this value.

First problem, the ESP32 crashes when the ISR is active. And for the life of me I don't know why

[Legohead259]

Alright, fixed the crashing problem. I changed the mutex variable to the std::mutex type, not the one provided by FreeRTOS. In the motorControlCallback() function, the lock guard was still being handled as if the mutex was by FreeRTOS, causing the system to crash. I removed the call for the publisher, which is why the system is stable now.

But, I have introduced the mutex guards in the TMAG5273 read callback for FOC control and in the sensor data publisher. Right now, they should be accessing the same sensor object, but with a mutex guard.

• Reintroduced publisher timer.
  • System crashed when connected to ROS2 agent, even with mutex's present.
• Made publisher call the global variable shaftAngle instead of the sensor object. The FOC algorithm is not accessing this variable so it should work if its a race condition problem, and not a mutex problem
  • Same result, system crash on ROS2 connection. The mutex guards are maybe not being released?
• Removed the mutex guards
  • System functions as expected. So it appears to be something with the mutex guards.

[Legohead259]

Changed the implementation of mutex lock to:

sensorFOCMutex.lock() // Locks the mutex - blocks until it is available
// ... important code related to sensorFOC
sensorFOCMutex.unlock() // Explicitly unlock the mutex

• Same result, system locks when ROS2 agent connects.
• Gonna try adding some debug print statements
  • Well now it works as expected... That's weird!

Example output from debug

unlocking mutex in angularMeasurementCallback()
Locking mutex in readTMAG5273Callback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Locking mutex in angularMeasurementCallback()
Locking mutex in readTMAG5273Callback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Locking mutex in angularMeasurementCallback()
Locking mutex in readTMAG5273Callback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Locking mutex in angularMeasurementCallback()
Locking mutex in readTMAG5273Callback()

This follows expected results. angularMeasurementCallback() reaches the mutex first and locks it. readTMAG5273Callback() gets to it and is blocked. angularMeasurementCallback() unlocks the mutex and finishes. readTMAG5273callback() then executes and finishes, releasing the mutex for the next call of angularMeasurementCallback()

[Legohead259]

That being said, it just crashed for some reason. Last messages:

Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Locking mutex in readTMAG5273Callback()
Unlocking mutex in readTMAG5273Callback()
Locking mutex in angularMeasurementCallback()
readTMAG5273Callback() finished!
Locking mutex in readTMAG5273Callback()Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!

Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Locking mutex in readTMAG5273Callback()
Locking mutex in angularMeasurementCallback()

[Legohead259]

Gonna change the debug messages.

Example debug output without ROS2 agent connected:

5273Callback() finished!
Trying to lock mutex in readTMAG5273Callback()
Locked mutex in readTMAG5273Callback()
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Trying to lock mutex in readTMAG5273Callback()
Locked mutex in readTMAG5273Callback()
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Trying to lock mutex in readTMAG5273Callback()
Locked mutex in readTMAG5273Callback()
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Trying to lock mutex in readTMAG5273Callback()
Locked mutex in readTMAG5273Callback()
Unlocking mutex in readTMAG5273Callback()

Debug output with ROS2 agent connected:

Locked mutex in angularMeasurementCallback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in readTMAG5273Callback()
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Trying to lock mutex in angularMeasurementCallback()
Trying to lock mutex in readTMAG5273Callback()
Locked mutex in angularMeasurementCallback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in readTMAG5273Callback()
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Trying to lock mutex in angularMeasurementCallback()
Trying to lock mutex in readTMAG5273Callback()
Locked mutex in angularMeasurementCallback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!

Topic is publishing as expected. Though data rate for publisher is significantly slower (~3Hz). This is probably due to all the debug messages, but something to consider. There will be in an impact since the sensorFOCMutex.lock() call is blocking without a timeout.

[Legohead259]

This example was having the functions access different memory variables. Gonna try having them both access sensor now.

• Board boots as expected
• System stable on agent connect
  • Topic published still at ~3 Hz

Example output:

Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in readTMAG5273Callback()
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Trying to lock mutex in angularMeasurementCallback()
Locked mutex in angularMeasurementCallback()
Trying to lock mutex in readTMAG5273Callback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in readTMAG5273Callback()
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Trying to lock mutex in readTMAG5273Callback()
Locked mutex in readTMAG5273Callback()
Unlocking mutex in readTMAG5273Callback()
readTMAG5273Callback() finished!
Trying to lock mutex in angularMeasurementCallback()
Locked mutex in angularMeasurementCallback()
Trying to lock mutex in readTMAG5273Callback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in readTMAG5273Callback()

Both functions have the same call to get sensor data: sensor.getAngleResult() / 180 * PI;

[Legohead259]

Backing up this stage into ebe5169.

Let's shift to a global variable for the shaftAngle that is updated with the interrupt.

• System crashes on startip. Debgu output:

  Starting LED state machine...Starting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Done!
  Initializing FOC sensor...
  Trying to lock mutex in readTMAG5273CStarting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Done!
  Initializing FOC sensor...
  Trying to lock mutex in readTMAG5273CStarting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Starting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Starting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Done!
  Starting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Starting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Done!
  Initializing FOC sensor...
  Trying to lock mutex in readTMAG5273CStarting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Done!
  Initializing FOC sensor...
  Trying to lock mutex in readTMAG5273CStarting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Starting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Done!
  Initializing FOC sensor...
  Trying to lock mutex in readTMAG5273CStarting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Starting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Starting TMAG5273...Done!
  Attaching ISR...Done!
  Starting LED state machine...Done!

  It could be something with the ISR. Let me try to put some debug statements in there. For this test, I removed the FOC algorithm and microROS functions for simplicity.

[Legohead259]

System still crashes with ISR. That appears to be a problem...

void IRAM_ATTR positionISR() {
    shaftReadTime = millis();
    shaftAngle = sensor.getAngleResult() / 180 * PI;
    Serial1.printf("%lu | %0.3f\r\n", shaftReadTime, shaftAngle); // DEBUG
}

Debug output:

aching ISR...Done!
Starting LED state machine...Done!
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
Starting TMAG5273...Done!

[Legohead259]

Changing the ISR to just be a simple print statement:

void IRAM_ATTR positionISR() {
    // shaftReadTime = millis();
    // shaftAngle = sensor.getAngleResult() / 180 * PI;
    Serial1.printf("%lu | %0.3f\r\n", shaftReadTime, shaftAngle); // DEBUG
}

Interesting result:

Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
0 | 0.000
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...0 | 0.000
Done!
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
0 | 0.000
0 | 0.000
0 | 0.000
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
0 | 0.000
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...0 | 0.000
Done!
0 | 0.000
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
0 | 0.000
0 | 0.000

[Legohead259]

New ISR:

bool intTriggered = false;
uint32_t triggerTime = 0;
uint32_t lastTriggerTime = 0;

void IRAM_ATTR positionISR() {
    // shaftReadTime = millis();
    // shaftAngle = sensor.getAngleResult() / 180 * PI;
    // Serial1.printf("%lu | %0.3f\r\n", shaftReadTime, shaftAngle); // DEBUG
    intTriggered = true;
    triggerTime = micros();
}

void loop() {
    if (intTriggered) {
        Serial1.printf("Interrupt triggered at %lu which was %lu us after the last\r\n", triggerTime, triggerTime - lastTriggerTime);
        intTriggered = false;
        lastTriggerTime = triggerTime;
    }
}

This code works as expected. Output:

upt triggered at 12892847 which was 3242 us after the last
Interrupt triggered at 12896087 which was 3240 us after the last
Interrupt triggered at 13019218 which was 3242 us after the last
Interrupt triggered at 13022457 which was 3239 us after the last
Interrupt triggered at 13145592 which was 3242 us after the last
Interrupt triggered at 13268709 which was 3240 us after the last
Interrupt triggered at 13271948 which was 3239 us after the last
Interrupt triggered at 13395065 which was 3241 us after the last
Interrupt triggered at 13398305 which was 3240 us after the last
Interrupt triggered at 13521427 which was 3241 us after the last
Interrupt triggered at 13524667 which was 3240 us after the last
Interrupt triggered at 13647794 which was 3241 us after the last
Interrupt triggered at 13651036 which was 3242 us after the last
Interrupt triggered at 13774164 which was 3242 us after the last
Interrupt triggered at 13777403 which was 3239 us after the last
Interrupt triggered at 13900537 which was 3240 us after the last
Interrupt triggered at 14023681 which was 3243 us after the last
Interrupt triggered at 14026921 which was 3240 us after the last
Interrupt triggered at 14150044 which was 3241 us after the last
Interrupt triggered at 14153285 which was 3241 us after the last

[Legohead259]

Let's throw in the sensor read now, but not do anything with the result.

bool intTriggered = false;
uint32_t triggerTime = 0;
uint32_t lastTriggerTime = 0;

void IRAM_ATTR positionISR() {
    shaftReadTime = millis();
    shaftAngle = sensor.getAngleResult() / 180 * PI;
    // Serial1.printf("%lu | %0.3f\r\n", shaftReadTime, shaftAngle); // DEBUG
    intTriggered = true;
    triggerTime = micros();
}

void loop() {
    if (intTriggered) {
        Serial1.printf("Interrupt triggered at %lu which was %lu us after the last\r\n", triggerTime, triggerTime - lastTriggerTime);
        intTriggered = false;
        lastTriggerTime = triggerTime;
    }
}

Crashes with new code. Output shows it never makes it into the loop().

Attaching ISR...Done!
Starting LED state machine...Done!
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Starting TMAG5273...Done!
Attaching ISR...Done!
Starting LED state machine...Done!
Starting TMAG5273...Done!
Attaching ISR...Done!

[Legohead259]

Seems like the ESP32 cannot handle I2C or other interrput-based tasks (e.g. UART) within an ISR unlike other microcontrollers. See:

• Recent forum post
• ESP32 forum post 1
• ESP32 forum post 2

This is most likely because the ESP32 runs a RTOS which balances all of the other peripherals. To prevent blocking features like WiFi, BT, etc. the ESP-IDF does not implement an IRQ-friendly I2C interface. Other microcontrollers may have this capability. Suggested solutions are to flip a flag and poll the data. I think this definitively closes this issue in terms of the interrupt. We will just have to deal with the slower sample rates for now. We won't need the flag because the sensor will be updating itself (~400 Hz) faster than we can poll it (<100 Hz).

So, let's just go back to having both the readTMAG5273Callback() and angularMeasurementCallback() functions poll the sensor when they're ready and use the mutex to prevent simultaneous access.

[Legohead259]

Code:

void angularMeasurementCallback(rcl_timer_t * timer, int64_t last_call_time) {
    RCLC_UNUSED(last_call_time);
    if (timer != NULL) {
        // std::lock_guard<std::mutex> lock(sensorFOCMutex);
        Serial1.println("Trying to lock mutex in angularMeasurementCallback()");
        sensorFOCMutex.lock();
        Serial1.println("Locked mutex in angularMeasurementCallback()");
        RCSOFTCHECK(rcl_publish(&angularPositionPublisher, &angularMeasurementMsg, NULL));
        // angularMeasurementMsg.timestamp = shaftReadTime;
        // angularMeasurementMsg.angular_position = shaftAngle;
        // angularMeasurementMsg.angular_velocity = shaftAngularVelocity;

        sensorFOC.update();
        angularMeasurementMsg.timestamp = millis();
        angularMeasurementMsg.angular_position = sensorFOC.getAngle() - sensorFOC.getFullRotations();
        angularMeasurementMsg.angular_velocity = sensorFOC.getVelocity();

        // angularMeasurementMsg.timestamp = millis();
        // angularMeasurementMsg.angular_position = sensor.getAngleResult() / 180 * PI;
        // angularMeasurementMsg.angular_velocity = sensorFOC.getVelocity();

        Serial1.println("Unlocking mutex in angularMeasurementCallback()");
        sensorFOCMutex.unlock();
        Serial1.println("angularMeasurementCallback() finished!");

    }
}

...

float readTMAG5273Callback() {
    // return shaftAngle;
    // Serial1.println("Trying to lock mutex in readTMAG5273Callback()");
    // sensorFOCMutex.lock();
    // Serial1.println("Locked mutex in readTMAG5273Callback()");
    float _angle = sensor.getAngleResult() / 180 * PI;
    // Serial1.println("Unlocking mutex in readTMAG5273Callback()");
    // sensorFOCMutex.unlock();
    // Serial1.println("readTMAG5273Callback() finished!");
    return _angle;
}

...

void controlMotorTask( void * parameter) {
    Serial1.printf("Motor control on Core %d\r\n", xPortGetCoreID());
    for(;;) {
        uint32_t _startMicros = micros();
        Serial1.println("Trying to lock mutex in controlMotorTask()");
        sensorFOCMutex.lock();
        Serial1.println("Locked mutex in controlMotorTask()");

        motor.loopFOC();
        // Serial1.printf("Time to finish loopFOC(): %lu us\r\n", micros() - _startMicros);
        motor.move(target);

        Serial1.println("Unlocking mutex in controlMotorTask()");
        sensorFOCMutex.unlock();
        Serial1.println("controlMotorTask() finished!");
    }
}

Debug output:

Trying to lock mutex in angularMeasurementCallback()
controlMotorTask() finished!Locked mutex in angularMeasurementCallback()

Trying to lock mutex in controlMotorTask()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in controlMotorTask()
Unlocking mutex in controlMotorTask()
controlMotorTask() finished!Trying to lock mutex in angularMeasurementCallback()

Locked mutex in angularMeasurementCallback()
Trying to lock mutex in controlMotorTask()Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!

Locked mutex in controlMotorTask()
Unlocking mutex in controlMotorTask()
controlMotorTask() finished!
Trying to lock mutex in controlMotorTask()Trying to lock mutex in angularMeasurementCallback()

Locked mutex in angularMeasurementCallback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in controlMotorTask()
Unlocking mutex in controlMotorTask()
Trying to lock mutex in angularMeasurementCallback()
controlMotorTask() finished!Locked mutex in angularMeasurementCallback()

Trying to lock mutex in controlMotorTask()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in controlMotorTask()
Unlocking mutex in controlMotorTask()
Trying to lock mutex in angularMeasurementCallback()
controlMotorTask() finished!Locked mutex in angularMeasurementCallback()

Trying to lock mutex in controlMotorTask()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in controlMotorTask()
Unlocking mutex in controlMotorTask()
controlMotorTask() finished!
Trying to lock mutex in controlMotorTask()Trying to lock mutex in angularMeasurementCallback()

Locked mutex in angularMeasurementCallback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in controlMotorTask()
Trying to lock mutex in angularMeasurementCallback()
Unlocking mutex in controlMotorTask()
controlMotorTask() finished!Locked mutex in angularMeasurementCallback()

Trying to lock mutex in controlMotorTask()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in controlMotorTask()
Unlocking mutex in controlMotorTask()
Trying to lock mutex in angularMeasurementCallback()
controlMotorTask() finished!Locked mutex in angularMeasurementCallback()

Trying to lock mutex in controlMotorTask()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in controlMotorTask()
Unlocking mutex in controlMotorTask()
controlMotorTask() finished!Trying to lock mutex in angularMeasurementCallback()

Locked mutex in angularMeasurementCallback()
Trying to lock mutex in controlMotorTask()Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!

Locked mutex in controlMotorTask()
Unlocking mutex in controlMotorTask()
controlMotorTask() finished!
Trying to lock mutex in controlMotorTask()Trying to lock mutex in angularMeasurementCallback()

Locked mutex in angularMeasurementCallback()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in controlMotorTask()
Trying to lock mutex in angularMeasurementCallback()
Unlocking mutex in controlMotorTask()
controlMotorTask() finished!Locked mutex in angularMeasurementCallback()

Trying to lock mutex in controlMotorTask()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in controlMotorTask()
Unlocking mutex in controlMotorTask()
Trying to lock mutex in angularMeasurementCallback()
controlMotorTask() finished!Locked mutex in angularMeasurementCallback()

Trying to lock mutex in controlMotorTask()
Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!
Locked mutex in controlMotorTask()
Unlocking mutex in controlMotorTask()
controlMotorTask() finished!Trying to lock mutex in angularMeasurementCallback()

Locked mutex in angularMeasurementCallback()
Trying to lock mutex in controlMotorTask()Unlocking mutex in angularMeasurementCallback()
angularMeasurementCallback() finished!

Trying to lock mutex in angularMeasurementCallback()

So it works, but randomly crashes after awhile. Could it be that both process are trying to access and lock the mutex simultaneously???

[Legohead259]

I changed the priority of the FOC algorithm task to 1 instead of 0. This seems to have made the system more stable. Publisher updating with information at ~2 Hz. I'm increasing the serial bus speed to see if that has an effect.

• Weird! At the high baudrate, the MicroROS task does not run... Is the FOC algorithm running to fast and hogging the mutex?
• Putting in an artificial delay delay(10); to the FOC task and increasing baudrate again
  • That worked. There needs to be a window of time where the MicroROS task can grab the mutex from the FOC task
  • Publish rate up to ~31 Hz
• Gonna remove the debug print statements now
  • Seems to be working. Publisher messages are updating at ~55 Hz now

Gonna commit this stage: dacacfb9

[Legohead259]

Closing as NOT POSSIBLE ON ESP32 ARCHITECTURE