226 lines
5.5 KiB
C++
226 lines
5.5 KiB
C++
|
/******************************************************************************
|
|||
|
* The MIT License
|
|||
|
*
|
|||
|
* Copyright (c) 2015 Frank-Michael Krause
|
|||
|
*
|
|||
|
* Permission is hereby granted, free of charge, to any person
|
|||
|
* obtaining a copy of this software and associated documentation
|
|||
|
* files (the "Software"), to deal in the Software without
|
|||
|
* restriction, including without limitation the rights to use, copy,
|
|||
|
* modify, merge, publish, distribute, sublicense, and/or sell copies
|
|||
|
* of the Software, and to permit persons to whom the Software is
|
|||
|
* furnished to do so, subject to the following conditions:
|
|||
|
*
|
|||
|
* The above copyright notice and this permission notice shall be
|
|||
|
* included in all copies or substantial portions of the Software.
|
|||
|
*
|
|||
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
|||
|
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
|
|||
|
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
|||
|
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
|
|||
|
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
|
|||
|
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
|
|||
|
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|||
|
* SOFTWARE.
|
|||
|
*****************************************************************************/
|
|||
|
|
|||
|
|
|||
|
#include <wirish/wirish.h>
|
|||
|
#include <libmaple/i2c.h>
|
|||
|
#include "lsm303reg.h"
|
|||
|
#include "../ringbuffer/ringbuffer.h"
|
|||
|
#include "lsm303.h"
|
|||
|
|
|||
|
#define ACC_FIFO_WATERMARK 10
|
|||
|
|
|||
|
static int ACC_BIAS_X;
|
|||
|
static int ACC_BIAS_Y;
|
|||
|
static int ACC_BIAS_Z;
|
|||
|
|
|||
|
|
|||
|
int accx, accy, accz;
|
|||
|
|
|||
|
#define RB_SIZE 64
|
|||
|
#define ACC_AVERAGE_X 60
|
|||
|
#define ACC_AVERAGE_Y 60
|
|||
|
#define ACC_AVERAGE_Z 60
|
|||
|
|
|||
|
static int rbvx[RB_SIZE];
|
|||
|
static long unsigned int rbtx[RB_SIZE];
|
|||
|
static int rbvy[RB_SIZE];
|
|||
|
static long unsigned int rbty[RB_SIZE];
|
|||
|
static int rbvz[RB_SIZE];
|
|||
|
static long unsigned int rbtz[RB_SIZE];
|
|||
|
static RingBuffer rbx(RB_SIZE, rbvx, rbtx);
|
|||
|
static RingBuffer rby(RB_SIZE, rbvy, rbty);
|
|||
|
static RingBuffer rbz(RB_SIZE, rbvz, rbtz);
|
|||
|
|
|||
|
//static int averageActiveX = 0;
|
|||
|
//static int averageActiveY = 0;
|
|||
|
//static int averageActiveZ = 0;
|
|||
|
|
|||
|
|
|||
|
static uint8 write_msg_data[3];
|
|||
|
static i2c_msg write_msg;
|
|||
|
|
|||
|
static uint8 read_msg_data[7];
|
|||
|
static i2c_msg read_msg;
|
|||
|
|
|||
|
|
|||
|
void readAccValues()
|
|||
|
{
|
|||
|
int16 *p;
|
|||
|
write_msg_data[0] = LSM303_REG_OUT_X_L_A|LSM303_READ_MULTI_BYTES; // 0x80 -> AutoIncrement address on read
|
|||
|
write_msg.length = 1;
|
|||
|
i2c_master_xfer(I2C1, &write_msg, 1, 0);
|
|||
|
|
|||
|
read_msg.length = 6;
|
|||
|
i2c_master_xfer(I2C1, &read_msg, 1, 2);
|
|||
|
|
|||
|
p = (int16*)read_msg_data;
|
|||
|
accx = *p++ - ACC_BIAS_X;
|
|||
|
accy = *p++ - ACC_BIAS_Y;
|
|||
|
accz = *p++ - ACC_BIAS_Z;
|
|||
|
}
|
|||
|
|
|||
|
uint8 accReadRegister(uint8 reg)
|
|||
|
{
|
|||
|
write_msg_data[0] = reg;
|
|||
|
write_msg.length = 1;
|
|||
|
i2c_master_xfer(I2C1, &write_msg, 1, 0);
|
|||
|
|
|||
|
read_msg.length = 1;
|
|||
|
i2c_master_xfer(I2C1, &read_msg, 1, 2);
|
|||
|
|
|||
|
return read_msg_data[0];
|
|||
|
}
|
|||
|
|
|||
|
void accWriteRegister(uint8 reg, uint8 val)
|
|||
|
{
|
|||
|
write_msg_data[0] = reg;
|
|||
|
write_msg_data[1] = val;
|
|||
|
write_msg.length = 2;
|
|||
|
i2c_master_xfer(I2C1, &write_msg, 1, 0);
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void lsm303I2CInit()
|
|||
|
{
|
|||
|
//i2c_master_enable(I2C1, 0);
|
|||
|
// Aktivierung des I2C-Busses muss im Hauptprogramm erfolgen,
|
|||
|
// da mehrere Komponenten das I2C-Interface benutzen k<>nnen
|
|||
|
// und eine mehrfache Initialisierung nicht funktioniert
|
|||
|
|
|||
|
write_msg.addr = LSM303_ADDR_A;
|
|||
|
write_msg.flags = 0; // write, 7 bit address
|
|||
|
write_msg.length = sizeof(write_msg_data);
|
|||
|
write_msg.xferred = 0;
|
|||
|
write_msg.data = write_msg_data;
|
|||
|
|
|||
|
read_msg.addr = LSM303_ADDR_A;
|
|||
|
read_msg.flags = I2C_MSG_READ;
|
|||
|
read_msg.length = sizeof(read_msg_data);
|
|||
|
read_msg.xferred = 0;
|
|||
|
read_msg.data = read_msg_data;
|
|||
|
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* Initialisiert und aktiviert den Beschleunigungssensor
|
|||
|
* Aktivierung des I2C-Busses muss im Hauptprogramm erfolgen,
|
|||
|
* da mehrere Komponenten das I2C-Interface benutzen k<EFBFBD>nnen
|
|||
|
* und eine mehrfache Initialisierung nicht funktioniert
|
|||
|
*/
|
|||
|
void lsm303Init()
|
|||
|
{
|
|||
|
/* Initialize I2C Transfer-Strukturen f<>r den lsm303*/
|
|||
|
lsm303I2CInit();
|
|||
|
|
|||
|
// Initialize ACC-Chip
|
|||
|
// 50Hz data Rate
|
|||
|
accWriteRegister(LSM303_REG_CTRL_REG1_A, LSM303_A_ODR_50HZ|LSM303_A_ALL_AXIS_EN);
|
|||
|
|
|||
|
// CTRL_REG2 --> no High Pass --> nothing to change
|
|||
|
|
|||
|
// enable Watermark interrupt on INT2 --> not working no int pulse
|
|||
|
// gyroWriteRegister(GYRO_REG_CTRL_REG3, GYRO_I2_EN|GYRO_I2_WTM);
|
|||
|
|
|||
|
// sensitivity 2G is default
|
|||
|
|
|||
|
// enable FIFO ?
|
|||
|
|
|||
|
//Fifomode and watermark
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void accCalibrate() {
|
|||
|
int count = 0;
|
|||
|
int sumx=0,sumy=0,sumz=0;
|
|||
|
|
|||
|
#ifdef DEBUG_ACC
|
|||
|
SerialUSB.print("Biaswerte ermitteln ");
|
|||
|
#endif
|
|||
|
while (count < 100)
|
|||
|
{
|
|||
|
delay(3);
|
|||
|
readAccValues();
|
|||
|
sumx += accx;
|
|||
|
sumy += accy;
|
|||
|
sumz += accz;
|
|||
|
#ifdef DEBUG_ACC
|
|||
|
SerialUSB.print("*");
|
|||
|
#endif
|
|||
|
count += 1;
|
|||
|
}
|
|||
|
ACC_BIAS_X = sumx/count;
|
|||
|
ACC_BIAS_Y = sumy/count;
|
|||
|
ACC_BIAS_Z = sumz/count;
|
|||
|
|
|||
|
#ifdef DEBUG_ACC
|
|||
|
SerialUSB.println("");
|
|||
|
SerialUSB.print("ACC-BIAS ");
|
|||
|
SerialUSB.print(ACC_BIAS_X); SerialUSB.print(" ");
|
|||
|
SerialUSB.print(ACC_BIAS_Y); SerialUSB.print(" ");
|
|||
|
SerialUSB.println(ACC_BIAS_Z);
|
|||
|
#endif
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
|
|||
|
void accUpdate()
|
|||
|
{
|
|||
|
//uint8 gyroFifoStatus = gyroReadRegister(GYRO_REG_FIFO_SRC);
|
|||
|
//uint8 anz = gyroFifoStatus & 0x1F;
|
|||
|
|
|||
|
#ifdef DEBUG_ACC
|
|||
|
// SerialUSB.print("fifo anz = ");
|
|||
|
// SerialUSB.print(anz);
|
|||
|
// SerialUSB.print(" ");
|
|||
|
#endif
|
|||
|
|
|||
|
readAccValues();
|
|||
|
|
|||
|
rbx.addValue(accx);
|
|||
|
rby.addValue(accy);
|
|||
|
rbz.addValue(accz);
|
|||
|
|
|||
|
#ifdef DEBUG_ACC
|
|||
|
SerialUSB.print(accx); SerialUSB.print(" ");
|
|||
|
SerialUSB.print(accy); SerialUSB.print(" ");
|
|||
|
SerialUSB.print(accz); SerialUSB.print(" ");
|
|||
|
SerialUSB.println("");
|
|||
|
#endif
|
|||
|
}
|
|||
|
|
|||
|
void accStart()
|
|||
|
{
|
|||
|
rbx.reset();
|
|||
|
rby.reset();
|
|||
|
rbz.reset();
|
|||
|
}
|
|||
|
|
|||
|
void accStop()
|
|||
|
{
|
|||
|
}
|
|||
|
|