d95af918fa
lots of code copied over, things filled in to hopefully get the LED matrix lighting up. untested.
169 lines
3.9 KiB
C
169 lines
3.9 KiB
C
/*
|
|
* accel.c
|
|
*
|
|
* Created on: Oct 13, 2024
|
|
* Author: true
|
|
*/
|
|
|
|
#include "ch32x035_conf.h"
|
|
|
|
#include "accel.h"
|
|
#include "lis2hh12_reg.h"
|
|
|
|
#include "comms/spi_master.h"
|
|
|
|
#include "misc/i8atan2.h"
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
|
|
|
|
|
|
// user data
|
|
AccelData accel;
|
|
|
|
uint8_t accel_found = 0;
|
|
|
|
uint8_t movement_idx; // index into "read" register
|
|
int16_t movement_read[4]; // last read movement value
|
|
int16_t movement; // last calculated movement value
|
|
uint16_t movement_worst; // worst seen movement value
|
|
|
|
|
|
// hardware
|
|
static stmdev_ctx_t dev_ctx;
|
|
|
|
|
|
|
|
static int32_t accel_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)
|
|
{
|
|
(void)(handle);
|
|
|
|
// i2c_write_reg8(LIS2_I2C_ADDR_SDO_LOW, reg, bufp, len);
|
|
LIS2_CS_PORT->BCR = LIS2_CS_PIN;
|
|
spim_write_reg8(reg, bufp, len);
|
|
LIS2_CS_PORT->BSHR = LIS2_CS_PIN;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int32_t accel_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
|
|
{
|
|
(void)(handle);
|
|
|
|
// i2c_read_reg8(LIS2DW_I2C_ADDR_SDO_LOW, reg, bufp, len);
|
|
LIS2_CS_PORT->BCR = LIS2_CS_PIN;
|
|
spim_read_reg8(reg, bufp, len);
|
|
LIS2_CS_PORT->BSHR = LIS2_CS_PIN;
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
void accel_init()
|
|
{
|
|
uint8_t devid;
|
|
uint8_t reset;
|
|
uint16_t timeout = 1000;
|
|
|
|
dev_ctx.write_reg = accel_write;
|
|
dev_ctx.read_reg = accel_read;
|
|
|
|
// make sure we've got the right device
|
|
lis2hh12_dev_id_get(&dev_ctx, &devid);
|
|
if (devid == LIS2HH12_ID) {
|
|
accel_found = 1;
|
|
}
|
|
|
|
// reset accelerometer
|
|
if (accel_found) {
|
|
lis2hh12_dev_reset_set(&dev_ctx, PROPERTY_ENABLE);
|
|
do {
|
|
lis2hh12_dev_reset_get(&dev_ctx, &reset);
|
|
if (!reset) {
|
|
timeout = 0;
|
|
accel_found = 1;
|
|
}
|
|
} while (timeout--);
|
|
}
|
|
|
|
if (accel_found) {
|
|
// disable block update
|
|
// data in output registers is updated immediately; FIFO is disabled
|
|
lis2hh12_block_data_update_set(&dev_ctx, PROPERTY_DISABLE);
|
|
|
|
// configure scale, power mode
|
|
lis2hh12_xl_full_scale_set(&dev_ctx, LIS2HH12_2g);
|
|
// TODO: check this
|
|
// lis2hh12_power_mode_set(&dev_ctx, LIS2DW12_CONT_LOW_PWR_LOW_NOISE_4);
|
|
|
|
// configure filter chain
|
|
// low pass filter enabled for 6D (not currently used)
|
|
lis2hh12_xl_filter_out_path_set(&dev_ctx, LIS2HH12_FILT_LP);
|
|
// digital LPF2 filter of output data
|
|
lis2hh12_xl_filter_low_bandwidth_set(&dev_ctx, LIS2HH12_LP_ODR_DIV_9);
|
|
|
|
// configure output data rate
|
|
lis2hh12_xl_data_rate_set(&dev_ctx, LIS2HH12_XL_ODR_200Hz);
|
|
}
|
|
}
|
|
|
|
void accel_poll()
|
|
{
|
|
uint8_t reg = 1;
|
|
int16_t xyz[3];
|
|
|
|
if (!accel_found) return;
|
|
|
|
while (reg) {
|
|
// read output only if new value is available
|
|
lis2hh12_xl_flag_data_ready_get(&dev_ctx, ®);
|
|
|
|
if (reg) {
|
|
// read acceleration data
|
|
memset(xyz, 0x00, 3 * sizeof(int16_t));
|
|
lis2hh12_acceleration_raw_get(&dev_ctx, xyz);
|
|
|
|
accel.x = xyz[0] >>= 8;
|
|
accel.y = xyz[1] >>= 8;
|
|
accel.z = xyz[2] >>= 8;
|
|
|
|
// compute our shitty "movement" thing
|
|
// awful way to detect being still
|
|
reg = sizeof(movement_read) / sizeof(movement_read[0]);
|
|
|
|
movement_idx++;
|
|
if (movement_idx == reg) {
|
|
movement_idx = 0;
|
|
movement = abs(movement_read[3] - movement_read[0]);
|
|
if (movement > movement_worst) {
|
|
movement_worst = movement;
|
|
}
|
|
}
|
|
|
|
movement_read[movement_idx] = abs(accel.x) + abs(accel.y);
|
|
}
|
|
}
|
|
}
|
|
|
|
int8_t accel_get_rotation(struct AccelData *a)
|
|
{
|
|
int8_t nx, ny, ret;
|
|
|
|
nx = -a->x;
|
|
ny = a->y;
|
|
|
|
ret = i8atan2(nx, ny) >> 1;
|
|
if (ret < 0) {
|
|
ret += 128;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
int16_t accel_get_movement()
|
|
{
|
|
return movement;
|
|
}
|