Trouble programming sensor reading

[PapercutSmothie]

I'm trying to get the accelerometer read by my custom code and right now, it is outputting bad, not-updated data despite looking like it should be updating.

Here is the code

static void dbgMessagerTask(void* parameters)
{
    Axis3f acc;
    while (true)
    {
        if (sensorsAreCalibrated())
        {
            // Read accelerometer values 
            sensorsReadAcc(&acc);

            consolePrintf("Acc: (%f,%f,%f) \n", 
                (double) acc.x, 
                (double) acc.y,
                (double) acc.z
            );
            sensorsResume();

        }

        //delay
        vTaskDelay(50);
    }
}

What am I missing that will make it work?

[ToveRumar]

Hi @PapercutSmothie !

Its looking like the queue where sensor data is stored might be empty when you fetch data from it.
If the queue is empty it the function will return false, and the struct will not be filled with data.
You will then just print junk.
From the stabilizer task the sensors are continuously read, so it looks like you thread is getting priority after that thread, if you are constantly getting the same junk data.
I would suggest checking the return value first to see if its false.

If so you could take several different approaches.
You could change the code so that the stabilizer task just peeks the data and this one reads, or print after the stabilizer task reads the data, if its only the prints you are looking for.

I hope you got the clarification you needed to make this work!

[PapercutSmothie]

I've so far updated the code to this:

#include "config.h"
#include "debug.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "static_mem.h"
#include "task.h"
#include "sensors.h"

static xQueueHandle inputQueue;
STATIC_MEM_QUEUE_ALLOC(inputQueue, 1, sizeof(Axis3f));

static void dbgMessagerTask(void*);
STATIC_MEM_TASK_ALLOC(dbgMessagerTask, DEBUG_MESSAGER_TASK_STACKSIZE);

static bool isInit = false;

void dbgMessagerTaskInit()
{
    inputQueue = STATIC_MEM_QUEUE_CREATE(inputQueue);
    //TODO
    STATIC_MEM_TASK_CREATE(dbgMessagerTask, dbgMessagerTask,DEBUG_MESSAGER_TASK_NAME,NULL,DEBUG_MESSAGER_TASK_PRI);
    isInit = true;
}

bool dbgMessagerTaskTest()
{
    return isInit;
}

static void dbgMessagerTask(void* parameters)
{
    Axis3f acc;
    while (true)
    {
        //wait for values
        sensorsWaitDataReady();

        // Read accelerometer values 
        if (sensorsReadAcc(&acc))
        {
            consolePrintf("Acc: (%f,%f,%f) \n", 
                (double) acc.x, 
                (double) acc.y,
                (double) acc.z
            );
        }
        else
        {
            consolePrintf("No new acceleration data found\n");
        }
        sensorsResume();
    }
}

void dbgMessagerTaskEnqueueInput(int value)
{
    xQueueOverwrite(inputQueue, &value);
}

The priority is 1, which is well below the sensor's of 4.

When I get the log, this is the data I get for a brief while

SYS: Crazyflie 2.1 is up and running!
SYS: Build 9:7c64c98060db (2024.10.2 +9) MODIFIED
SYS: I am 0x373732373433510C004F0040 and I have 1024KB of flash!
CFGBLK: v1, verification [OK]
DECK_CORE: 0 deck(s) found
IMU: BMI088: Using I2C interface.
IMU: BMI088 Gyro connection [OK].
IMU: BMI088 Accel connection [OK]
IMU: BMP388 I2C connection [OK]
ESTIMATOR: Using Complementary (1) estimator
CONTROLLER: Using PID (1) controller
MTR-DRV: Using brushed motor driver
SYS: About to run tests in system.c.
SYS: NRF51 version: 2024.2 (CF21)
EEPROM: I2C connection [OK].
STORAGE: Storage check [OK].
IMU: BMI088 gyro self-test [OK]
SYS: Self test passed!
STAB: Wait for sensor calibration...
SYS: Free heap: 17936 bytes
Acc: (-0.000131,-0.000120,0.007835) 
Acc: (-0.000618,-0.000563,0.037098) 
Acc: (-0.001486,-0.001338,0.089696) 
Acc: (-0.002655,-0.002285,0.158510) 
Acc: (-0.004086,-0.003247,0.237549) 
Acc: (-0.005701,-0.004113,0.321873) 
Acc: (-0.007384,-0.004824,0.407573) 
Acc: (-0.009050,-0.005373,0.491680) 
Acc: (-0.010650,-0.005757,0.571960) 
Acc: (-0.012127,-0.005980,0.646768) 
Acc: (-0.013417,-0.006084,0.715015) 
Acc: (-0.014506,-0.006134,0.776103) 
Acc: (-0.015437,-0.006186,0.829824) 
Acc: (-0.016257,-0.006279,0.876290) 
Acc: (-0.016978,-0.006406,0.915797) 
Acc: (-0.017578,-0.006514,0.948714) 
Acc: (-0.018043,-0.006554,0.975502) 
Acc: (-0.018391,-0.006517,0.996724) 
Acc: (-0.018643,-0.006462,1.013000) 
Acc: (-0.018814,-0.006470,1.024984) 
Acc: (-0.018924,-0.006572,1.033358) 
Acc: (-0.018983,-0.006753,1.038795) 
Acc: (-0.018996,-0.006984,1.041894) 
Acc: (-0.018972,-0.007252,1.043081) 
Acc: (-0.018917,-0.007580,1.042617) 
Acc: (-0.018838,-0.007984,1.040756) 
Acc: (-0.018761,-0.008421,1.037835) 
Acc: (-0.018700,-0.008808,1.034229) 
Acc: (-0.018640,-0.009074,1.030285) 
Acc: (-0.018573,-0.009207,1.026320) 
Acc: (-0.018513,-0.009238,1.022572) 
Acc: (-0.018473,-0.009219,1.019108) 
Acc: (-0.018437,-0.009186,1.015843) 
Acc: (-0.018382,-0.009142,1.012690) 
Acc: (-0.018275,-0.009074,1.009675) 
Acc: (-0.018079,-0.008973,1.006912) 
Acc: (-0.017794,-0.008839,1.004515) 
Acc: (-0.017442,-0.008682,1.002569) 
Acc: (-0.017068,-0.008543,1.001111) 
Acc: (-0.016752,-0.008461,1.000112) 
Acc: (-0.016558,-0.008432,0.999513) 
Acc: (-0.016436,-0.008392,0.999222) 
Acc: (-0.016252,-0.008282,0.999096) 
Acc: (-0.015935,-0.008105,0.998991) 
Acc: (-0.015538,-0.007893,0.998826) 
Acc: (-0.015159,-0.007661,0.998585) 
Acc: (-0.014848,-0.007422,0.998305) 
Acc: (-0.014609,-0.007213,0.998039) 
Acc: (-0.014450,-0.007067,0.997825) 
Acc: (-0.014391,-0.006992,0.997722) 
Acc: (-0.014458,-0.006985,0.997810) 
Acc: (-0.014652,-0.007042,0.998121) 
Acc: (-0.014955,-0.007175,0.998616) 
Acc: (-0.015333,-0.007401,0.999238) 
Acc: (-0.015709,-0.007710,0.999907) 
Acc: (-0.015999,-0.008060,1.000484) 
Acc: (-0.016190,-0.008403,1.000811) 
Acc: (-0.016328,-0.008689,1.000823) 
Acc: (-0.016456,-0.008882,1.000557) 
Acc: (-0.016623,-0.008999,1.000123) 
Acc: (-0.016896,-0.009093,0.999652) 
Acc: (-0.017315,-0.009202,0.999238) 
Acc: (-0.017833,-0.009329,0.998907) 
Acc: (-0.018319,-0.009473,0.998641) 
Acc: (-0.018675,-0.009653,0.998391) 
Acc: (-0.018901,-0.009879,0.998134) 
Acc: (-0.019044,-0.010115,0.997867) 
Acc: (-0.019131,-0.010323,0.997573) 
Acc: (-0.019155,-0.010469,0.997232) 
Acc: (-0.019089,-0.010511,0.996867) 
Acc: (-0.018929,-0.010426,0.996534) 
Acc: (-0.018695,-0.010224,0.996305) 
Acc: (-0.018435,-0.009938,0.996244) 
Acc: (-0.018214,-0.009632,0.996358) 
Acc: (-0.018054,-0.009377,0.996586) 
Acc: (-0.017923,-0.009220,0.996848) 
Acc: (-0.017800,-0.009190,0.997077) 
Acc: (-0.017693,-0.009291,0.997232) 
Acc: (-0.017573,-0.009497,0.997304) 
Acc: (-0.017372,-0.009746,0.997287) 
Acc: (-0.017070,-0.009971,0.997220) 
Acc: (-0.016700,-0.010117,0.997184) 
Acc: (-0.016313,-0.010142<F>

As you can probably tell, it gets cut off for some reason and I'm assuming this is the problem now that I am looking at the problem again

[PapercutSmothie]

I've done some more testing and I noticed that the sensorsWaitDataReady() causes the program to wait indefinitely on the sensors and the sensorsReadAcc() always returns false. Could the sensors not be updating in a way my program can see it? I would assume it is set up by default to run.

[ToveRumar]

Hi!

I think that the stabilizer task reads out the sensor data from the queue before your task. However I agree that sensorsWaitDataReady() should give you what you want.

What I do doubt is that this is a feasible use case. If you manage to read out the sensor data from the queue there is no data for the stabilizer task to read. I highly doubt the Crazyflie will be able to fly without this data.

If you are only looking to print, could you print in the stabilizer task? Or put in the data in another queue or variable in the stabilizer task, for the dbgMessagerTask to read from?