338 lines
8.8 KiB
C++
338 lines
8.8 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.
|
|||
|
*****************************************************************************/
|
|||
|
|
|||
|
/**
|
|||
|
* @brief functions for accessing l3g20 gyro on stm32f3discovery board
|
|||
|
*
|
|||
|
* SPI1 is set up to be a master
|
|||
|
* at 9 MHz (SPI_1_125MHZ),
|
|||
|
* 8 Bit
|
|||
|
* MSB first in/out (MSBFIRST)
|
|||
|
* MODE 0 // following the datasheet it should be MODE 3 but 0 is working too
|
|||
|
*
|
|||
|
* Pin PE3 is used as slave select, as defined for SPI1 in board.h
|
|||
|
*
|
|||
|
* There is no interrupt support because I was not able to get out any int2-output of the chip
|
|||
|
*
|
|||
|
*/
|
|||
|
|
|||
|
#include <wirish/wirish.h>
|
|||
|
#include "l3gd20.h"
|
|||
|
#include "../ringbuffer/ringbuffer.h"
|
|||
|
#include "gyro.h"
|
|||
|
|
|||
|
|
|||
|
#define NSS PE3
|
|||
|
#define GYRO_FIFO_WATERMARK 10
|
|||
|
|
|||
|
HardwareSPI spi1(1);
|
|||
|
static uint8 gyroValuesSingle[6];
|
|||
|
|
|||
|
static int GYRO_BIAS_X = -75;
|
|||
|
static int GYRO_BIAS_Y = 94;
|
|||
|
static int GYRO_BIAS_Z = 0;
|
|||
|
|
|||
|
float winkelx, winkely, winkelz;
|
|||
|
int gyroxRaw, gyroyRaw, gyrozRaw; // bei jedem Update wird ein Datensatz der gelesenen
|
|||
|
// Werte in den Raw-Variablen gespeichert um den Bias pr<70>fen zu
|
|||
|
// k<>nnen
|
|||
|
|
|||
|
#define RB_SIZE 256
|
|||
|
#define GYRO_AVERAGE_X 120
|
|||
|
#define GYRO_AVERAGE_Y 120
|
|||
|
#define GYRO_AVERAGE_Z 120
|
|||
|
|
|||
|
static int rbvx[RB_SIZE];
|
|||
|
static int rbvy[RB_SIZE];
|
|||
|
static int rbvz[RB_SIZE];
|
|||
|
static RingBufferAverage rbx(RB_SIZE, rbvx, NULL, GYRO_AVERAGE_X);
|
|||
|
static RingBufferAverage rby(RB_SIZE, rbvy, NULL, GYRO_AVERAGE_Y);
|
|||
|
static RingBufferAverage rbz(RB_SIZE, rbvz, NULL, GYRO_AVERAGE_Z);
|
|||
|
|
|||
|
static int averageActiveX = 0;
|
|||
|
static int averageActiveY = 0;
|
|||
|
static int averageActiveZ = 0;
|
|||
|
|
|||
|
void readGyroValues()
|
|||
|
{
|
|||
|
int j;
|
|||
|
digitalWrite(NSS, LOW);
|
|||
|
spi1.transfer(0xA8|0x40);
|
|||
|
|
|||
|
for (j=0; j<6; j++)
|
|||
|
gyroValuesSingle[j] = spi1.transfer(0);
|
|||
|
|
|||
|
digitalWrite(NSS, HIGH);
|
|||
|
}
|
|||
|
|
|||
|
uint8 gyroReadRegister(uint8 reg)
|
|||
|
{
|
|||
|
digitalWrite(NSS, LOW);
|
|||
|
spi1.transfer(reg|0x80);
|
|||
|
|
|||
|
uint8 val = spi1.transfer(0);
|
|||
|
|
|||
|
digitalWrite(NSS, HIGH);
|
|||
|
return val;
|
|||
|
}
|
|||
|
|
|||
|
void gyroWriteRegister(uint8 reg, uint8 val)
|
|||
|
{
|
|||
|
digitalWrite(NSS, LOW);
|
|||
|
spi1.transfer(reg);
|
|||
|
spi1.transfer(val);
|
|||
|
digitalWrite(NSS, HIGH);
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* Initialisiert das spi1 interface des stm32f3discovery
|
|||
|
*/
|
|||
|
void gyroSpiInit()
|
|||
|
{
|
|||
|
spi1.begin(SPI_9MHZ, MSBFIRST, 0);
|
|||
|
/* NSS is usually active LOW, so initialize it HIGH */
|
|||
|
pinMode(NSS, OUTPUT);
|
|||
|
digitalWrite(NSS, HIGH);
|
|||
|
}
|
|||
|
|
|||
|
#define GYRO_OP_PARAMS (GYRO_ODR_190|GYRO_LP_BW1|GYRO_ENABLE)
|
|||
|
|
|||
|
/**
|
|||
|
* konfiguriert den Gyro.
|
|||
|
* der Gyro ist nach dem Aufruf der Funktion aktiv. Wird er nicht mehr benötigt, kann er
|
|||
|
* mir gyroStop() wieder deaktiviert werden. Für eine Reaktivierung kann die Funktion
|
|||
|
* gyroStart() verwendet werden.
|
|||
|
*
|
|||
|
* Im aktiven Zustand muss im Abstand von höchstens 120ms die Funktion gyroUpdate()
|
|||
|
* aufgerufen werden.
|
|||
|
*
|
|||
|
* Die globalen Variablen winkelx, winkely und winkelz enthalten den jeweils aktuellen winkel.
|
|||
|
* Die Funktion gyroResetWinkel setzt die globalen Variablen wieder auf 0.
|
|||
|
*/
|
|||
|
void gyroInit()
|
|||
|
{
|
|||
|
/* Initialize SPI */
|
|||
|
// gyroSpiInit(); --> called aus dem Anwendungsmodul,
|
|||
|
// da an SPI1 noch weitere Ger<65>te betrieben werden
|
|||
|
|
|||
|
// Initialize GyroChip
|
|||
|
// 190Hz data Rate, 50Hz Bandwidth
|
|||
|
gyroWriteRegister(GYRO_REG_CTRL_REG1, GYRO_OP_PARAMS);
|
|||
|
|
|||
|
// 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);
|
|||
|
|
|||
|
// set resolution to 500 dps
|
|||
|
gyroWriteRegister(GYRO_REG_CTRL_REG4, GYRO_FULL_SCALE_500);
|
|||
|
|
|||
|
// enable FIFO, Output after second LP-Filter, no HP
|
|||
|
gyroWriteRegister(GYRO_REG_CTRL_REG5, GYRO_FIFO_ENABLE|GYRO_OUT_SELECT2);
|
|||
|
//gyroWriteRegister(GYRO_REG_CTRL_REG5, GYRO_OUT_SELECT2);
|
|||
|
|
|||
|
//Fifomode and watermark
|
|||
|
gyroWriteRegister(GYRO_REG_FIFO_CTRL, GYRO_FIFO_MODE_STREAM|GYRO_FIFO_WATERMARK);
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* Es werden 1000 Werte eingelesen (ca. 5,5s) und aus den Werten die Drift-Kompensations-
|
|||
|
* werte gemittelt. Der Gyro darf während der Zeit nicht bewegt werden.
|
|||
|
*/
|
|||
|
void gyroCalibrate() {
|
|||
|
int count = 0;
|
|||
|
int sumx=0,sumy=0,sumz=0;
|
|||
|
|
|||
|
#ifdef DEBUG_GYRO
|
|||
|
SerialUSB.print("Biaswerte ermitteln ");
|
|||
|
#endif
|
|||
|
while (count < 1000)
|
|||
|
{
|
|||
|
delay(100);
|
|||
|
uint8 anz = gyroReadRegister(GYRO_REG_FIFO_SRC) & 0x1F;
|
|||
|
for (int i=0; i<anz; i++)
|
|||
|
{
|
|||
|
readGyroValues();
|
|||
|
int16 *p = (int16*)gyroValuesSingle;
|
|||
|
sumx += *p++;
|
|||
|
sumy += *p++;
|
|||
|
sumz += *p;
|
|||
|
}
|
|||
|
#ifdef DEBUG_GYRO
|
|||
|
SerialUSB.print("*");
|
|||
|
#endif
|
|||
|
count += anz;
|
|||
|
}
|
|||
|
GYRO_BIAS_X = sumx/count;
|
|||
|
GYRO_BIAS_Y = sumy/count;
|
|||
|
GYRO_BIAS_Z = sumz/count;
|
|||
|
|
|||
|
#ifdef DEBUG_GYRO
|
|||
|
SerialUSB.println("");
|
|||
|
SerialUSB.print("BIAS ");
|
|||
|
SerialUSB.print(GYRO_BIAS_X); SerialUSB.print(" ");
|
|||
|
SerialUSB.print(GYRO_BIAS_Y); SerialUSB.print(" ");
|
|||
|
SerialUSB.println(GYRO_BIAS_Z);
|
|||
|
#endif
|
|||
|
}
|
|||
|
|
|||
|
void gyroSetBiasX(int bias) { GYRO_BIAS_X = bias; }
|
|||
|
void gyroSetBiasY(int bias) { GYRO_BIAS_Y = bias; }
|
|||
|
void gyroSetBiasZ(int bias) { GYRO_BIAS_Z = bias; }
|
|||
|
|
|||
|
|
|||
|
|
|||
|
/**
|
|||
|
* Die Winkel-Variablen werden auf 0 zurückgesetzt
|
|||
|
*/
|
|||
|
void gyroResetWinkel()
|
|||
|
{
|
|||
|
winkelx = 0;
|
|||
|
winkely = 0;
|
|||
|
winkelz = 0;
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
/**
|
|||
|
* Neben der "unendlichen" Integration <EFBFBD>ber die Winkelgeschwindigkeitsmesswerte
|
|||
|
* kann eine zeitlich begrenzte Integration <EFBFBD>ber die letzten n Messwerte eingestellt
|
|||
|
* werden. Damit erh<EFBFBD>lt man den Winkel, um den die Achse in z.B. der letzten Sekunde
|
|||
|
* gedreht wurde. Der additive Fehler der "unendlichen" Integration entf<EFBFBD>llt hierbei.
|
|||
|
*
|
|||
|
* Au<EFBFBD>erdem kann eine positive und eine negative Schwelle eingestellt werden, bei
|
|||
|
* deren <EFBFBD>berschreiten je eine Variable incrementiert wird.
|
|||
|
*/
|
|||
|
void gyroSetActualAngleHistory(int axis, int len )
|
|||
|
{
|
|||
|
if (axis == GYRO_AXIS_X)
|
|||
|
{
|
|||
|
rbx.setAverageLen(len);
|
|||
|
if (len != 0)
|
|||
|
averageActiveX = 1;
|
|||
|
else
|
|||
|
averageActiveX = 0;
|
|||
|
}
|
|||
|
else if (axis == GYRO_AXIS_Y)
|
|||
|
{
|
|||
|
rby.setAverageLen(len);
|
|||
|
if (len != 0)
|
|||
|
averageActiveY = 1;
|
|||
|
else
|
|||
|
averageActiveY = 0;
|
|||
|
}
|
|||
|
else if (axis == GYRO_AXIS_Z)
|
|||
|
{
|
|||
|
rbz.setAverageLen(len);
|
|||
|
if (len != 0)
|
|||
|
averageActiveZ = 1;
|
|||
|
else
|
|||
|
averageActiveZ = 0;
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
/**
|
|||
|
* Diese Funktion muss innehalb von 120ms mindestens einmal aufgerufen werden.
|
|||
|
* Es werden die vorhandenen Messwerte aus dem Fifo-Speicher des Chips ausgelesen und
|
|||
|
* die aktuellen Winkel berechnet
|
|||
|
*/
|
|||
|
void gyroUpdate()
|
|||
|
{
|
|||
|
uint8 gyroFifoStatus = gyroReadRegister(GYRO_REG_FIFO_SRC);
|
|||
|
uint8 anz = gyroFifoStatus & 0x1F;
|
|||
|
|
|||
|
#ifdef DEBUG_GYRO
|
|||
|
SerialUSB.print("fifo anz = ");
|
|||
|
SerialUSB.print(anz);
|
|||
|
SerialUSB.print(" ");
|
|||
|
#endif
|
|||
|
|
|||
|
int16* p;
|
|||
|
int sumx=0,sumy=0,sumz=0;
|
|||
|
|
|||
|
for (int i=0; i<anz; i++)
|
|||
|
{
|
|||
|
readGyroValues();
|
|||
|
p = (int16*)gyroValuesSingle;
|
|||
|
gyroxRaw = *p++ - GYRO_BIAS_X;
|
|||
|
gyroyRaw = *p++ - GYRO_BIAS_Y;
|
|||
|
gyrozRaw = *p++ - GYRO_BIAS_Z;
|
|||
|
sumx += gyroxRaw;
|
|||
|
sumy += gyroyRaw;
|
|||
|
sumz += gyrozRaw;
|
|||
|
|
|||
|
if (averageActiveX)
|
|||
|
rbx.addValue(gyroxRaw);
|
|||
|
}
|
|||
|
|
|||
|
winkelx += (sumx*500.0F)/(160.0F*32767.0F);
|
|||
|
winkely += (sumy*500.0F)/(160.0F*32767.0F);
|
|||
|
winkelz += (sumz*500.0F)/(160.0F*32767.0F);
|
|||
|
|
|||
|
#ifdef DEBUG_GYRO
|
|||
|
SerialUSB.print(waktX); SerialUSB.print(" ");
|
|||
|
SerialUSB.print(waktY); SerialUSB.print(" ");
|
|||
|
SerialUSB.print(waktZ); SerialUSB.print(" ");
|
|||
|
SerialUSB.println("");
|
|||
|
#endif
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* Versetzt den GyroChip in den aktiven Zustand, leert den Fifo und setzt die Winkel zurueck
|
|||
|
*/
|
|||
|
void gyroStart()
|
|||
|
{
|
|||
|
gyroWriteRegister(GYRO_REG_CTRL_REG1, GYRO_OP_PARAMS);
|
|||
|
// Empty Fifo
|
|||
|
gyroUpdate();
|
|||
|
gyroResetWinkel();
|
|||
|
}
|
|||
|
|
|||
|
/**
|
|||
|
* Versetzt den gyro in den Power Down mode
|
|||
|
*/
|
|||
|
void gyroStop()
|
|||
|
{
|
|||
|
gyroWriteRegister(GYRO_REG_CTRL_REG1, 0);
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
float gyroGetShortValueX()
|
|||
|
{
|
|||
|
return (rbx.getAverageSum()*500.0F)/(160.0F*32767.0F);
|
|||
|
}
|
|||
|
|
|||
|
float gyroGetShortValueY()
|
|||
|
{
|
|||
|
return (rby.getAverageSum()*500.0F)/(160.0F*32767.0F);
|
|||
|
}
|
|||
|
|
|||
|
float gyroGetShortValueZ()
|
|||
|
{
|
|||
|
return (rby.getAverageSum()*500.0F)/(160.0F*32767.0F);
|
|||
|
}
|
|||
|
|