114 lines
1.9 KiB
C
114 lines
1.9 KiB
C
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
#include "freertos/portmacro.h"
|
|
|
|
#include <stdlib.h>
|
|
#include "esp_err.h"
|
|
|
|
#include "emu.h"
|
|
#include "tmeconfig.h"
|
|
|
|
#include "mpu6050.h"
|
|
#include "mpumouse.h"
|
|
|
|
|
|
|
|
portMUX_TYPE dxdyMux=portMUX_INITIALIZER_UNLOCKED;
|
|
static int dx, dy, btn;
|
|
|
|
void mpuMouseGetDxDyBtn(int *pDx, int *pDy, int *pBtn) {
|
|
printf("Mouse: %d %d\n", dx, dy);
|
|
portENTER_CRITICAL(&dxdyMux);
|
|
*pDx=dx;
|
|
*pDy=dy;
|
|
*pBtn=0;
|
|
dx=0;
|
|
dy=0;
|
|
portEXIT_CRITICAL(&dxdyMux);
|
|
}
|
|
|
|
|
|
#define RECAL_SAMPS 32
|
|
static int recal[RECAL_SAMPS][2];
|
|
static int recalPos=0;
|
|
static int idleAx=0, idleAy=0;
|
|
static int curDx, curDy;
|
|
|
|
//Add sample to correction
|
|
static void addToCorr(int ax, int ay) {
|
|
recalPos++;
|
|
if (recalPos>=RECAL_SAMPS) recalPos=0;
|
|
recal[recalPos][0]=ax;
|
|
recal[recalPos][1]=ay;
|
|
}
|
|
|
|
|
|
#define DIV_NOMOTION 5000
|
|
|
|
static void checkCorr() {
|
|
int avgax=0;
|
|
int avgay=0;
|
|
for (int i=0; i<RECAL_SAMPS; i++) {
|
|
avgax+=recal[i][0];
|
|
avgay+=recal[i][1];
|
|
}
|
|
avgax/=RECAL_SAMPS;
|
|
avgay/=RECAL_SAMPS;
|
|
|
|
int div=0;
|
|
for (int i=0; i<RECAL_SAMPS; i++) {
|
|
int dax=recal[i][0]-avgax;
|
|
int day=recal[i][1]-avgay;
|
|
div+=dax*dax+day*day;
|
|
}
|
|
div/=RECAL_SAMPS;
|
|
if (div<DIV_NOMOTION) {
|
|
//Accelerometer is not moving.
|
|
idleAx=avgax;
|
|
idleAy=avgay;
|
|
curDx=0;
|
|
curDy=0;
|
|
}
|
|
}
|
|
|
|
|
|
void mpuMouseEmu() {
|
|
int calCtr=0;
|
|
mpu6050_accel_tp a[128];
|
|
while(1) {
|
|
int r=mpu6050_read_fifo(a, 128);
|
|
if (r) {
|
|
int cax=0, cay=0;
|
|
for (int i=0; i<r; i++) {
|
|
cax+=a[i].accelx;
|
|
cay+=a[i].accely;
|
|
}
|
|
cax/=r;
|
|
cay/=r;
|
|
addToCorr(cax, cay);
|
|
|
|
//Subtract bias
|
|
cax-=idleAx;
|
|
cay-=idleAy;
|
|
|
|
//Integrate
|
|
curDx+=cax;
|
|
curDy+=cay;
|
|
|
|
portENTER_CRITICAL(&dxdyMux);
|
|
dx+=curDx/(1<<12);
|
|
dy+=curDy/(1<<12);
|
|
portEXIT_CRITICAL(&dxdyMux);
|
|
// printf("% 2d samp x % 6d y % 6d\n", r, dx, dy);
|
|
} else {
|
|
printf("Mpu: No samples?\n");
|
|
}
|
|
calCtr++;
|
|
if (calCtr>=10) {
|
|
calCtr=0;
|
|
checkCorr();
|
|
}
|
|
vTaskDelay(1);
|
|
}
|
|
}
|