69 lines
1.8 KiB
C
69 lines
1.8 KiB
C
#include <alloca.h>
|
|
//
|
|
// Created by xemon on 23.11.22.
|
|
//
|
|
|
|
#include <SystemDelayInterface.h>
|
|
#include "Accel.h"
|
|
#include "CmsisRtosThreadUtils.h"
|
|
|
|
bool Accel_GetNext(tAccel *env, vector3 *value, uint32_t timeout) {
|
|
return osMessageQueueGet(env->dataQueue, value, 0, timeout) == osOK;
|
|
}
|
|
|
|
void Accel_Clear(tAccel *env) {
|
|
osMessageQueueReset(env->dataQueue);
|
|
}
|
|
|
|
void Accel_Init(tAccel *env, tSpiPortIO *accelIO) {
|
|
|
|
AccelQma6100P_Init(&env->accel, accelIO);
|
|
InitThreadAtrStatic(&env->thread.attr, "Accel", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
|
|
env->thread.id = 0;
|
|
env->dataQueue = osMessageQueueNew(128, sizeof(vector3), NULL);
|
|
|
|
env->accelDataFlow = (tAccelDataFlowInterface) {
|
|
.env = env,
|
|
.getNext = (accelDataGetNextCall) Accel_GetNext,
|
|
.clear = (accelDataClearCall) Accel_Clear,
|
|
};
|
|
}
|
|
|
|
|
|
void Accel_StartDevice(tAccel *env) {
|
|
AccelQma6100P_ConfigureDefaults(&env->accel);
|
|
}
|
|
|
|
bool Accel_WaitReady(tAccel *env, uint32_t timeout) {
|
|
|
|
uint32_t time_end = SystemGetMs() + timeout;
|
|
|
|
float len;
|
|
do {
|
|
len = vector3Len(env->current);
|
|
} while ((time_end > SystemGetMs()) && len < 0.1f);
|
|
|
|
return len > 0.1f;
|
|
}
|
|
|
|
|
|
static _Noreturn void Accel_Thread(tAccel *env) {
|
|
//todo нужно переделать под таймер и семафор
|
|
for (;;) {
|
|
if (AccelQma6100P_Read(&env->accel, &env->current)) {
|
|
osMessageQueuePut(env->dataQueue, &env->current, 0, 0);
|
|
env->errorDataFlag = true;
|
|
} else {
|
|
env->errorDataFlag = false;
|
|
}
|
|
SystemDelayMs(1);
|
|
}
|
|
}
|
|
|
|
void Accel_StartThread(tAccel *env) {
|
|
if (!env->thread.id) {
|
|
env->thread.id = osThreadNew((osThreadFunc_t) (Accel_Thread), (void *) (env), &env->thread.attr);
|
|
}
|
|
}
|
|
|