Sorry, I can’t help optimizing your code. I don’t have the time to go through all your functions.
can you just verify this code. I have followed same for other tasks.
in setup() :
I’ll create semaphores
sem_accelroEvent = xSemaphoreCreateBinary();
xSemaphoreGive(sem_accelroEvent);
sem_battEvent = xSemaphoreCreateBinary();
xSemaphoreGive(sem_battEvent);
I’ll create timers
batteryTask.begin(getTimerInterval_ms(g_config_settings.batteryLevelFrequency), BatteryTask_TimerCallback);
batteryTask.start();
accelTask.begin(getTimerInterval_ms(g_config_settings.unitOrientationFrequency), AccelroTask_TimerCallback);
accelTask.start();
Scheduler.startLoop(Accelerometer_Task);
Callabck functions :
in timer callback I’ll release
void BatteryTask_TimerCallback(TimerHandle_t xtimer) {
// timestamp_flag = true;
xSemaphoreGiveFromISR(sem_battEvent, pdFALSE);
}
void AccelroTask_TimerCallback(TimerHandle_t xtimer) {
xSemaphoreGiveFromISR(sem_accelroEvent, pdFALSE);
}
Task :
In RTOS task I’ll take and release semaphore
void Accelerometer_Task() {
if (xSemaphoreTake(sem_accelroEvent, portMAX_DELAY) == pdTRUE)
{
static int index = 0;
sBmx160SensorData_t Oaccel;
bmx160.getAllData(NULL, NULL, &Oaccel);
float ax = Oaccel.x;
float ay = Oaccel.y;
float az = Oaccel.z;
// Calculate current roll and pitch
float currentRoll = atan2(ay, sqrt(ax * ax + az * az)) * 57.2958; // Degrees
float currentPitch = atan2(-ax, sqrt(ay * ay + az * az)) * 57.2958; // Degrees
// Calculate the angle difference from calibrated vertical
float rollDifference = abs(currentRoll - calibratedRoll);
float pitchDifference = abs(currentPitch - calibratedPitch);
// Combine roll and pitch differences to a single effective angle
float angleDifference = sqrt(rollDifference * rollDifference + pitchDifference * pitchDifference);
Serial.print("Unit Orientation : ");
Serial.print(angleDifference);
Serial.println("°");
hubdata[index % MAX_SAMPLES].timestamp = timestamp;
hubdata[index % MAX_SAMPLES].orientation = angleDifference;
index++;
if (angleDifference > g_config_settings.trigger_orientation) {
TipoverInterrupt = true;
xSemaphoreGiveFromISR(reportInterrupt, pdFALSE);
}
xSemaphoreTake(sem_accelroEvent, 10);
}
}