RTOS locking

[mike_durian]

I'll be using the sensor fusion library with an RTOS (uC/OS-II, not FreeRTOS, but I don't think that is relevant). The sensor fusion user's guide discusses using an RTOS and gives an example showing two tasks: one for reading data from the sensors (read_task) and another to process the data (fusion_task).

I don't see any locks (semaphore, mutex, etc) to serialize access to the raw sensor data. Depending what other tasks are running in the RTOS, I think it would be theoretically possible for the read_task to modify sensor data while the fusion_task is still processing it.

I think it might be enough to put a lock around

   sfg.readSensor()

in read_task and around

   sfg.conditionSensorReadings()

in fusion_task. Is this correct? Is that sufficient locking to serialize data shared between the two tasks?

Thanks,

mike

[michaelestanley]

Mike,

The FreeRTOS variant that is part of the SDK uses FreeRTOS events to coordinate between tasks. Lower frequency tasks use xEventGroupWaitBits() to stall until  the appropriate control bit has been set by the next higher frequency task.   So there's never a chance of lockup.

And I agree you will need to do something similar with whatever RTOS implementation you chose.

Regards,

Mike Stanley

[mike_durian]

I don't think we can trust that the lower priority task, fusion_task, in this case, will complete its computation on the data before the higher priority task, read_task, needs to run again. This is especially true if this is a real-world application that has other non-sensor tasks that need to run at higher priorities. For example,

• read_task gets some data and sets a flag so fusion_task will run, then waits for the next sampling time
• fusion_task starts running, but gets interrupted by a higher priority task(s)
• higher priority task(s) run - taking longer than the read_task sampling interval - and then finish
• the RTOS scheduler picks the next task to run, which is read_task since it's done with its wait period and has a higher priority than fusion_task
• now read_task is modifying data that fusion_task hasn't finished processing

This sort of thing might be unlikely to occur, but since it can occur, we need to protect against it. In addition to the mechanism that read_task uses to signal fusion_task to run (this would be a flag in uC/OS-II), there also needs to be a semaphore or mutex to serialize access to the data shared between the two tasks.

I think the only shared data between read_task and fusion_task is the raw sensor data and I think fusion_task is done with the raw data after its call to conditionSensorReadings(), but I don't know the code well enough to say for certain that that is true.

It would look something like

read_task:

   get lock

   sfg.readSensors()

   release lock

fusion_task:

   get lock

   sfg.conditionSensorReadings()

   release lock

mike

[michaelestanley]

Good points and we always appreciate suggestions.  I double checked the code.  Accel and Mag raw data ARE consumed in conditionSensorReadings.  Gyro raw data is consumed both there as well as near the top of the Kalman filters.  We've never encountered problems because these operations typically occur well within our FAST_LOOP_HZ interval, which is fairly long relatively speaking.  If this wasn't sufficient, we would really need (in addition to your suggestions) to add extra FIFO buffers, increasing RAM utilization, as we wouldn't want to risk losing sensor samples.

Regards,

Mike

[mike_durian]

Mike,

Thanks for checking on this. Can you provide a little more details on where the raw gyro data is used in the Kalman filters? I might try to break it out so I can lock it without having to lock the entire Kalman computation, though I'll do that if it gets too messy.

I've got another computation heavy task that needs to run at a higher priority than the sensor fusion stuff. I also have some high priority tasks that need to be low latency. I've found that even if I think something is unlikely, it I can imagine a failure scenario, it will come back to bite me, so I'm going to add the locking. I'll keep my eye on the FIFO usage and see if I need to enlarge that later.

I'm also changing the behavior of the read_task by slowing down how often it reads data. Instead of reading about once for every sensor sample, I'm going to wait for the smallest hardware FIFO to get 1/2 - 3/4 full before reading. I think fewer, larger sensor reads might help. I'm going to uncouple the fusion_task so it runs on its own schedule, instead of running after every read_task read. With proper locking, I don't think this will pose a problem. Ideally, the read_task would act in response to FIFO interrupts from the sensors, but my quick check didn't turn up a configurable watermark interrupt for the magnetometer part of the FXOS8700.

One last thing. Unless there is good locking in the I2C driver, the calls to put the gyro in stand-by mode should probably happen in the read_task, not the fusion_task. That way all I2C access happens in the read_task. You wouldn't want both the fusion_task and read_task trying to use the I2C bus at the same time. The motion check call can still happen in the fusion_task - it just needs to signal the read_task about the stationary status.

mike