Add... erm, I forgot. Attempt at mouse using an MPU, for one.

This commit is contained in:
Jeroen Domburg 2017-05-27 23:51:16 +08:00
parent e579a9b16d
commit c64c92a2e4
8 changed files with 307 additions and 65 deletions

View File

@ -3,7 +3,7 @@
# project subdirectory.
#
PROJECT_NAME := app-template
PROJECT_NAME := tme
include $(IDF_PATH)/make/project.mk

View File

@ -22,6 +22,7 @@
#include "tmeconfig.h"
#include "mpu6050.h"
#include "mpumouse.h"
unsigned char *romdata;
@ -35,6 +36,17 @@ void emuTask(void *pvParameters)
tmeStartEmu(ram, romdata);
}
void mouseTask(void *pvParameters)
{
printf("Starting mouse task...\n");
while(!mpu6050_init()) {
printf("Can't init MPU....\n");
vTaskDelay(100);
}
mpuMouseEmu();
}
void app_main()
{
@ -43,14 +55,12 @@ void app_main()
spi_flash_mmap_handle_t hrom;
esp_err_t err;
// mpu6050_init();
// while(1);
part=esp_partition_find_first(0x40, 0x1, NULL);
if (part==0) printf("Couldn't find bootrom part!\n");
err=esp_partition_mmap(part, 0, 128*1024, SPI_FLASH_MMAP_DATA, (const void**)&romdata, &hrom);
if (err!=ESP_OK) printf("Couldn't map bootrom part!\n");
printf("Starting emu...\n");
xTaskCreatePinnedToCore(&mouseTask, "mouse", 6*1024, NULL, 6, NULL, 0);
xTaskCreatePinnedToCore(&emuTask, "emu", 6*1024, NULL, 5, NULL, 0);
}

View File

@ -1,92 +1,145 @@
#include <stdio.h>
#include "driver/i2c.h"
#include "mpu6050.h"
#define I2C_MASTER_SCL_IO 4 /*!< gpio number for I2C master clock */
#define I2C_MASTER_SDA_IO 26 /*!< gpio number for I2C master data */
#define I2C_MASTER_SCL_IO 26 /*!< gpio number for I2C master clock */
#define I2C_MASTER_SDA_IO 4 /*!< gpio number for I2C master data */
#define I2C_MASTER_NUM I2C_NUM_0 /*!< I2C port number for master dev */
#define I2C_MASTER_TX_BUF_DISABLE 0 /*!< I2C master do not need buffer */
#define I2C_MASTER_RX_BUF_DISABLE 0 /*!< I2C master do not need buffer */
#define I2C_MASTER_FREQ_HZ 100000 /*!< I2C master clock frequency */
#define WRITEW_BIT 1
#define MPU_ADDR (0x68<<1)
/*
esp_err_t i2c_master_read_slave(i2c_port_t i2c_num, uint8_t* data_rd, size_t size)
{
if (size == 0) {
return ESP_OK;
}
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, ( ESP_SLAVE_ADDR << 1 ) | READ_BIT, ACK_CHECK_EN);
if (size > 1) {
i2c_master_read(cmd, data_rd, size - 1, ACK_VAL);
}
i2c_master_read_byte(cmd, data_rd + size - 1, NACK_VAL);
i2c_master_stop(cmd);
esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 1000 / portTICK_RATE_MS);
i2c_cmd_link_delete(cmd);
return ret;
}
*/
#define MPU_ADDR 0x68
static esp_err_t setreg(int i2c_num, int reg, int val) {
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, ( MPU_ADDR | I2C_MASTER_WRITE), true);
i2c_master_write_byte(cmd, ( (MPU_ADDR<<1) | I2C_MASTER_WRITE), true);
i2c_master_write_byte(cmd, reg, true);
i2c_master_write_byte(cmd, val, true);
i2c_master_stop(cmd);
esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 100 / portTICK_RATE_MS);
esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, 30 / portTICK_RATE_MS);
if (ret!=ESP_OK) printf("MPU6050: NACK setting reg 0x%X to val 0x%X\n", reg, val);
i2c_cmd_link_delete(cmd);
return ret;
}
static int getreg(int i2c_num, int reg) {
unsigned char byte;
esp_err_t ret;
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, ( MPU_ADDR | I2C_MASTER_WRITE), true);
i2c_master_write_byte(cmd, ( (MPU_ADDR<<1) | I2C_MASTER_WRITE), true);
i2c_master_write_byte(cmd, reg, true);
i2c_master_start(cmd);
i2c_master_write_byte(cmd, ( MPU_ADDR | I2C_MASTER_READ), true);
i2c_master_read_byte(cmd, &byte, false);
i2c_master_stop(cmd);
esp_err_t ret=i2c_master_cmd_begin(i2c_num, cmd, 100 / portTICK_RATE_MS);
ret=i2c_master_cmd_begin(i2c_num, cmd, 100 / portTICK_RATE_MS);
i2c_cmd_link_delete(cmd);
if (ret!=ESP_OK) return -1;
cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, ( (MPU_ADDR<<1) | I2C_MASTER_READ), true);
i2c_master_read_byte(cmd, &byte, true);
i2c_master_stop(cmd);
ret=i2c_master_cmd_begin(i2c_num, cmd, 100 / portTICK_RATE_MS);
i2c_cmd_link_delete(cmd);
if (ret!=ESP_OK) printf("MPU6050: NACK reading reg 0x%X\n", reg);
if (ret!=ESP_OK) {
return -1;
}
return byte;
}
esp_err_t mpu6050_start(int i2c_num, int samp_div) {
printf("Initializing MPU6050...\n");
setreg(i2c_num, 107, 0x80); //Reset
vTaskDelay(10);
int i=getreg(i2c_num, 0x75);
printf("Mpu: %x\n", i);
if (i!=0x68) {
printf("No MPU6050 detected at i2c addr %x! (%d) No mouse available.\n", MPU_ADDR, i);
return ESP_ERR_NOT_FOUND;
}
setreg(i2c_num, 107, 1); //Take out of sleep, gyro as clk
setreg(i2c_num, 108, 0); //Un-standby everything
setreg(i2c_num, 106, 0x0C); //reset more stuff
vTaskDelay(10);
setreg(i2c_num, 106, 0x0); //reset more stuff
setreg(i2c_num, 25, samp_div); //Sample divider
setreg(i2c_num, 26, (7<<3)|0); //fsync to accel_z[0], 260Hz bw (making Fsamp 8KHz)
setreg(i2c_num, 27, 0); //gyro def
setreg(i2c_num, 28, 0); //accel 2G
setreg(i2c_num, 35, 0x08); //fifo: emit accel data only
setreg(i2c_num, 36, 0x00); //no slave
setreg(i2c_num, 106, 0x40); //fifo: enable
printf("MPU6050 found and initialized.\n");
return ESP_OK;
}
void mpu6050_poll(int *x, int *y, int *z) {
int mpu6050_read_fifo(mpu6050_accel_tp *meas, int maxct) {
int i2c_num=I2C_MASTER_NUM;
int i;
int no;
uint8_t buf[6];
i2c_cmd_handle_t cmd;
esp_err_t ret;
no=(getreg(i2c_num, 114)<<8);
no|=getreg(i2c_num, 115);
if (no>0x8000) {
printf("Huh? Fifo has %x bytes.\n", no);
return 0; //huh?
}
// printf("FIFO: %d\n", no);
no=no/6; //bytes -> samples
if (no>maxct) no=maxct;
for (i=0; i<no; i++) {
cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, ( (MPU_ADDR<<1) | I2C_MASTER_WRITE), true);
i2c_master_write_byte(cmd, 116, true);
i2c_master_stop(cmd);
ret=i2c_master_cmd_begin(i2c_num, cmd, 100 / portTICK_RATE_MS);
i2c_cmd_link_delete(cmd);
cmd = i2c_cmd_link_create();
i2c_master_start(cmd);
i2c_master_write_byte(cmd, ( (MPU_ADDR<<1) | I2C_MASTER_READ), true);
i2c_master_read(cmd, buf, 5, 0);
i2c_master_read(cmd, buf+5, 1, 1);
i2c_master_stop(cmd);
ret=i2c_master_cmd_begin(i2c_num, cmd, 100 / portTICK_RATE_MS);
if (ret!=ESP_OK) {
printf("Error reading packet %d/%d\n", i, no);
return 0;
}
meas[i].accelx=(buf[0]<<8)|buf[1];
meas[i].accely=(buf[2]<<8)|buf[3];
meas[i].accelz=(buf[4]<<8)|buf[5];
i2c_cmd_link_delete(cmd);
}
return no;
}
//Actually initializes the i2c port, but atm there's nothing else connected to it... should put this
//into some io init routine or something tho'.
void mpu6050_init() {
int i2c_master_port = I2C_MASTER_NUM;
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = I2C_MASTER_SDA_IO;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_io_num = I2C_MASTER_SCL_IO;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = I2C_MASTER_FREQ_HZ;
i2c_param_config(i2c_master_port, &conf);
i2c_driver_install(i2c_master_port, conf.mode, 1024, 1024, 0);
int mpu6050_init() {
i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.scl_io_num = I2C_MASTER_SCL_IO;
conf.sda_io_num = I2C_MASTER_SDA_IO;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = 400000;
ESP_ERROR_CHECK(i2c_param_config(I2C_MASTER_NUM, &conf));
ESP_ERROR_CHECK(i2c_driver_install(I2C_MASTER_NUM, conf.mode, 1024, 1024, 0));
mpu6050_start(I2C_MASTER_NUM, 10);
//MPU has sample rate of 8KHz. We have an 1K fifo where we put 6byte samples --> about 128 samples.
//We want to grab the data at about 10Hz -> sample rate of about 1KHz.
esp_err_t r=mpu6050_start(I2C_MASTER_NUM, 8);
if (r!=ESP_OK) {
i2c_driver_delete(I2C_MASTER_NUM);
return 0;
}
return 1;
}

View File

@ -1,2 +1,15 @@
#include <stdint.h>
typedef struct {
int16_t accelx;
int16_t accely;
int16_t accelz;
} mpu6050_accel_tp;
int mpu6050_read_fifo(mpu6050_accel_tp *meas, int maxct);
void mpu6050_poll(int *x, int *y, int *z);
void mpu6050_init();
int mpu6050_init();

View File

@ -0,0 +1,113 @@
#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);
}
}

View File

@ -0,0 +1,2 @@
void mpuMouseGetDxDyBtn(int *pDx, int *pDy, int *pBtn);
void mpuMouseEmu();

View File

@ -16,10 +16,11 @@
#include "soc/gpio_struct.h"
#include "driver/gpio.h"
#include "esp_heap_alloc_caps.h"
#include "mpumouse.h"
#include "mouse.h"
#define PIN_NUM_MISO 25
#define PIN_NUM_MOSI 27
//#define PIN_NUM_MOSI 23
#define PIN_NUM_MOSI 23
#define PIN_NUM_CLK 19
#define PIN_NUM_CS 22
@ -217,7 +218,7 @@ void IRAM_ATTR displayTask(void *arg) {
.quadhd_io_num=-1
};
spi_device_interface_config_t devcfg={
.clock_speed_hz=40000000, //Clock out at 40 MHz. Yes, that's heavily overclocked.
.clock_speed_hz=26000000, //Clock out at 40 MHz. Yes, that's heavily overclocked.
.mode=0, //SPI mode 0
.spics_io_num=PIN_NUM_CS, //CS pin
.queue_size=10, //We want to be able to queue 7 transactions at a time
@ -291,8 +292,11 @@ void IRAM_ATTR displayTask(void *arg) {
void dispDraw(uint8_t *mem) {
int dx, dy, btn;
currFbPtr=mem;
xSemaphoreGive(dispSem);
mpuMouseGetDxDyBtn(&dx, &dy, &btn);
mouseMove(dx, dy, btn);
}

View File

@ -81,13 +81,13 @@ CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv"
CONFIG_PARTITION_TABLE_CUSTOM_APP_BIN_OFFSET=0x10000
CONFIG_PARTITION_TABLE_FILENAME="partitions.csv"
CONFIG_APP_OFFSET=0x10000
CONFIG_PHY_DATA_OFFSET=
# CONFIG_OPTIMIZATION_LEVEL_DEBUG is not set
CONFIG_OPTIMIZATION_LEVEL_RELEASE=y
CONFIG_OPTIMIZATION_LEVEL_DEBUG=y
# CONFIG_OPTIMIZATION_LEVEL_RELEASE is not set
#
# Component config
#
# CONFIG_AWS_IOT_SDK is not set
# CONFIG_BT_ENABLED is not set
CONFIG_BT_RESERVE_DRAM=0
@ -100,15 +100,21 @@ CONFIG_ESP32_DEFAULT_CPU_FREQ_240=y
CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ=240
CONFIG_MEMMAP_SMP=y
# CONFIG_MEMMAP_TRACEMEM is not set
# CONFIG_MEMMAP_TRACEMEM_TWOBANKS is not set
# CONFIG_ESP32_TRAX is not set
CONFIG_TRACEMEM_RESERVE_DRAM=0x0
# CONFIG_ESP32_ENABLE_COREDUMP_TO_FLASH is not set
# CONFIG_ESP32_ENABLE_COREDUMP_TO_UART is not set
CONFIG_ESP32_ENABLE_COREDUMP_TO_NONE=y
# CONFIG_ESP32_ENABLE_COREDUMP is not set
# CONFIG_ESP32_APPTRACE_DEST_TRAX is not set
# CONFIG_ESP32_APPTRACE_DEST_UART is not set
CONFIG_ESP32_APPTRACE_DEST_NONE=y
# CONFIG_ESP32_APPTRACE_ENABLE is not set
# CONFIG_MEMMAP_SPIRAM_ENABLE is not set
# CONFIG_TWO_MAC_ADDRESS_FROM_EFUSE is not set
CONFIG_FOUR_MAC_ADDRESS_FROM_EFUSE=y
CONFIG_NUMBER_OF_MAC_ADDRESS_GENERATED_FROM_EFUSE=4
# CONFIG_TWO_UNIVERSAL_MAC_ADDRESS is not set
CONFIG_FOUR_UNIVERSAL_MAC_ADDRESS=y
CONFIG_NUMBER_OF_UNIVERSAL_MAC_ADDRESS=4
CONFIG_SYSTEM_EVENT_QUEUE_SIZE=32
CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE=4096
CONFIG_MAIN_TASK_STACK_SIZE=4096
@ -133,10 +139,44 @@ CONFIG_ESP32_TIME_SYSCALL_USE_RTC_FRC1=y
# CONFIG_ESP32_TIME_SYSCALL_USE_FRC1 is not set
# CONFIG_ESP32_TIME_SYSCALL_USE_NONE is not set
CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_RC=y
# CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_CRYSTAL is not set
CONFIG_ESP32_RTC_CLK_CAL_CYCLES=1024
CONFIG_ESP32_DEEP_SLEEP_WAKEUP_DELAY=0
# CONFIG_ESP32_XTAL_FREQ_40 is not set
# CONFIG_ESP32_XTAL_FREQ_26 is not set
CONFIG_ESP32_XTAL_FREQ_AUTO=y
CONFIG_ESP32_XTAL_FREQ=0
# CONFIG_WIFI_ENABLED is not set
# CONFIG_ETHERNET is not set
#
# FAT Filesystem support
#
CONFIG_FATFS_CODEPAGE_ASCII=y
# CONFIG_FATFS_CODEPAGE_437 is not set
# CONFIG_FATFS_CODEPAGE_720 is not set
# CONFIG_FATFS_CODEPAGE_737 is not set
# CONFIG_FATFS_CODEPAGE_771 is not set
# CONFIG_FATFS_CODEPAGE_775 is not set
# CONFIG_FATFS_CODEPAGE_850 is not set
# CONFIG_FATFS_CODEPAGE_852 is not set
# CONFIG_FATFS_CODEPAGE_855 is not set
# CONFIG_FATFS_CODEPAGE_857 is not set
# CONFIG_FATFS_CODEPAGE_860 is not set
# CONFIG_FATFS_CODEPAGE_861 is not set
# CONFIG_FATFS_CODEPAGE_862 is not set
# CONFIG_FATFS_CODEPAGE_863 is not set
# CONFIG_FATFS_CODEPAGE_864 is not set
# CONFIG_FATFS_CODEPAGE_865 is not set
# CONFIG_FATFS_CODEPAGE_866 is not set
# CONFIG_FATFS_CODEPAGE_869 is not set
# CONFIG_FATFS_CODEPAGE_932 is not set
# CONFIG_FATFS_CODEPAGE_936 is not set
# CONFIG_FATFS_CODEPAGE_949 is not set
# CONFIG_FATFS_CODEPAGE_950 is not set
CONFIG_FATFS_CODEPAGE=1
CONFIG_FATFS_MAX_LFN=255
#
# FreeRTOS
#
@ -158,6 +198,10 @@ CONFIG_FREERTOS_BREAK_ON_SCHEDULER_START_JTAG=y
CONFIG_FREERTOS_ISR_STACKSIZE=2000
# CONFIG_FREERTOS_LEGACY_HOOKS is not set
CONFIG_FREERTOS_MAX_TASK_NAME_LEN=16
# CONFIG_SUPPORT_STATIC_ALLOCATION is not set
CONFIG_TIMER_TASK_PRIORITY=1
CONFIG_TIMER_TASK_STACK_DEPTH=2048
CONFIG_TIMER_QUEUE_LENGTH=10
# CONFIG_FREERTOS_DEBUG_INTERNALS is not set
#
@ -186,6 +230,8 @@ CONFIG_LWIP_DHCP_MAX_NTP_SERVERS=1
CONFIG_TCP_MAXRTX=12
CONFIG_TCP_SYNMAXRTX=6
# CONFIG_LWIP_DHCP_DOES_ARP_CHECK is not set
CONFIG_TCPIP_TASK_STACK_SIZE=2560
# CONFIG_PPP_SUPPORT is not set
#
# mbedTLS
@ -210,3 +256,4 @@ CONFIG_OPENSSL_ASSERT_DO_NOTHING=y
# SPI Flash driver
#
# CONFIG_SPI_FLASH_ENABLE_COUNTERS is not set
CONFIG_SPI_FLASH_ROM_DRIVER_PATCH=y