143 lines
4.0 KiB
C
143 lines
4.0 KiB
C
//
|
|
// Created by ilya on 25.03.24.
|
|
//
|
|
#include <alloca.h>
|
|
#include <SystemDelayInterface.h>
|
|
#include "Accel.h"
|
|
#include "CmsisRtosThreadUtils.h"
|
|
#include "ext_telematica.h"
|
|
#include "at32f435_437_clock.h"
|
|
|
|
crm_clocks_freq_type crm_clocks_freq_struct = {0};
|
|
tAccel ACCEL;
|
|
|
|
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) {
|
|
|
|
env->errorDataFlag = true;
|
|
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)) {
|
|
|
|
if (EXT_ENV_ADR_TELE.META_EXT_ENV_TELE)
|
|
EXT_ENV_ADR_TELE.tele_func(&env->accel.xyz, TELE_MODULE_ACCEL_DATA);
|
|
|
|
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);
|
|
// }
|
|
}
|
|
|
|
void TMR1_Accel_TmrStart(tAccel *env){
|
|
/* get system clock */
|
|
crm_clocks_freq_get(&crm_clocks_freq_struct);
|
|
|
|
/* enable tmr1 clock */
|
|
crm_periph_clock_enable(CRM_TMR1_PERIPH_CLOCK, TRUE);
|
|
|
|
/* tmr1 configuration */
|
|
/* time base configuration */
|
|
uint32_t tmr = (crm_clocks_freq_struct.apb2_freq * 2 / 10000) - 1;
|
|
tmr_base_init(TMR1, 9998, tmr/1000);
|
|
tmr_cnt_dir_set(TMR1, TMR_COUNT_UP);
|
|
|
|
/* overflow interrupt enable */
|
|
tmr_interrupt_enable(TMR1, TMR_OVF_INT, TRUE);
|
|
|
|
/* tmr1 hall interrupt nvic init */
|
|
nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);
|
|
nvic_irq_enable(TMR1_OVF_TMR10_IRQn, 0x59, 0);
|
|
/* enable tmr1 */
|
|
tmr_counter_enable(TMR1, TRUE);
|
|
}
|
|
|
|
//uint16_t ms[1000];
|
|
//volatile uint16_t i = 0;
|
|
//volatile uint16_t o = 0;
|
|
//volatile uint32_t ttt = 0;
|
|
//volatile uint32_t aa = 0;
|
|
osStatus_t stat = osError;
|
|
|
|
void TMR1_OVF_TMR10_IRQHandler(void)
|
|
{
|
|
if(tmr_interrupt_flag_get(TMR1, TMR_OVF_FLAG) == SET)
|
|
{
|
|
if(ACCEL.errorDataFlag == true) {
|
|
if (AccelQma6100P_Read(&ACCEL.accel, &ACCEL.current)) {
|
|
|
|
if (EXT_ENV_ADR_TELE.META_EXT_ENV_TELE)
|
|
EXT_ENV_ADR_TELE.tele_func(&ACCEL.accel.xyz, TELE_MODULE_ACCEL_DATA);
|
|
|
|
stat = osMessageQueuePut(ACCEL.dataQueue, &ACCEL.current, 0, 0);
|
|
if (stat != osOK) {
|
|
asm("nop");
|
|
}
|
|
ACCEL.errorDataFlag = true;
|
|
|
|
// if(o == 1000){
|
|
// o=0;
|
|
// }
|
|
// aa = SystemGetMs();
|
|
// if(ttt > aa){
|
|
// ++i;
|
|
// } else {
|
|
// ms[o] = i;
|
|
// i=0;
|
|
// o++;
|
|
// ttt = SystemGetMs()+1000;
|
|
// }
|
|
|
|
} else {
|
|
ACCEL.errorDataFlag = false;
|
|
}
|
|
}
|
|
tmr_flag_clear(TMR1, TMR_OVF_FLAG);
|
|
}
|
|
} |