Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 963 → Rev 964

/LoCoHead/UART.h
0,0 → 1,73
/****************************************************************************
UART INITIALIZATION
 
This Program enables you to send data from the ATmega32 Serial port to
the PC so that the data can be read on hyperterminal. This is very
useful for debugging purposes when the development system has no
display of its own.
 
To send data through the UART printf() is used.
 
uart.c and uart.h must be included in the main project, and the function
uart_init() called to enable serial port output.
 
In order to print floating point values, you will need to add the line
"-Wl,-u,vfprintf -lprintf_flt -lm" to AVR Studio4 linker options by going to
Project->Configuration Options->Custom Options
 
Read the "Detailed Description" section of
http://www.nongnu.org/avr-libc/user-manual/group__avr__stdio.html for
more detail.
 
*******************************************************************************/
#ifndef UART_H
#define UART_H
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
 
 
#ifndef F_CPU
/* In neueren Version der WinAVR/Mfile Makefile-Vorlage kann
F_CPU im Makefile definiert werden, eine nochmalige Definition
hier wuerde zu einer Compilerwarnung fuehren. Daher "Schutz" durch
#ifndef/#endif
Dieser "Schutz" kann zu Debugsessions führen, wenn AVRStudio
verwendet wird und dort eine andere, nicht zur Hardware passende
Taktrate eingestellt ist: Dann wird die folgende Definition
nicht verwendet, sondern stattdessen der Defaultwert (8 MHz?)
von AVRStudio - daher Ausgabe einer Warnung falls F_CPU
noch nicht definiert: */
#warning "F_CPU war noch nicht definiert, wird nun nachgeholt mit 4000000"
#define F_CPU 4000000UL // Systemtakt in Hz - Definition als unsigned long beachten
// Ohne ergeben sich unten Fehler in der Berechnung
#endif
#define BAUD 9600UL // Baudrate
// Berechnungen
#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1) // clever runden
#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1))) // Reale Baudrate
#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD) // Fehler in Promille, 1000 = kein Fehler.
#if ((BAUD_ERROR<990) || (BAUD_ERROR>1010))
#error Systematischer Fehler der Baudrate grösser 1% und damit zu hoch!
#endif
 
 
int put_uart_char(char c, FILE *fd);
 
//No need to call this function directly, it's just here for printf().
//Outputs a single character to the UART when the transmit buffer is empty.
//This is the function that we bind to printf so it can output the string
//that printf generates. The FILE parameter is required by printf for behind-
//the-scenes bookkeeping. *Note that we can write a function that will put a
//single char anywhere we want -- parallel interface, SPI, I2C, etc.*
 
void uart_init(void);
 
//Set up the registers that control the UART and bind put_uart_char
//as our output function.
 
#endif
/LoCoHead/Was_ist_LocoHead.txt
0,0 → 1,40
LoCoHead - Das Low-Cost Headtracker Projekt
Mathias Kreider, 2011
 
LocoHead Module sind für private und kommerzielle Nutzung freigegeben.
 
LocoHead Sourcecode und Hardwaredesigns sind lediglich für den Privatgebrauch bestimmt,
sie sind NICHT zur kommerziellen Weiterverwendung freigegeben.
 
Der Sinn dieses Projekts
Ziel von LoCoHead ist die Realisierung einer schlanken Headtrackinglösung in
Soft- und Hardware. Dies wurde auf Basis des LSM303DLH 3-Achs
Magnetormeter / Accelerometers umgesetzt, inklusive Datenaufbereitung und
Ausgabe als PPM oder seriellem Kanal zur Steuerung der Aktuatoren von Kameraaufhängungen.
 
 
 
Version 0.1 verwendet das Polulu-Breakout Board zusammen mit AtMega32L
 
LoCoHEad - Pro
- sehr geringer Preis im Vegleich zu Gyro-basierten Sytemen
- Keine Drift
- Nick, Roll und Gier Achse, dh. komplett natürliche Bewegung des Kopfes wird erfasst
- Alle Sensorfunktionen in einem CHip, keine mechanische Ausrichtung / senkrechte Platinen nötig
- Einfaches Interface
- Einfache Sensorkalibrierung
- OpenSource Software
 
LoCoHead - Contra
- Beeinflussbar durch magnetische Störquellen
- Beeinflussbar durch starke dynamische Beschleunigungen, bes. Axial
- Geringere Auflösung der Gierachse ( +/- 0,5%) als die von Roll und Nick (+/- 0,05%)
 
 
LocoHead entstand auf Basis des Polulu-Minimalbeispiels
(siehe polulu_LICENCE.txt)
 
 
 
 
 
/LoCoHead/What_is_LocoHead.txt
0,0 → 1,34
LoCoHead - The low-cost head tracker project
Mathias Kreider, 2011
 
LocoHead modules are approved for private and commercial use.
 
LocoHead source code and hardware designs are for private use only,
they may NOT be used in commercial software or designs.
 
The purpose of this project
The goal of LoCoHead is the realisation of a lean head tracking solution in
software and hardware. It was based on the 3-axis LSM303DLH
Magnetormeter / Accelerometer, including data processing and
Output as a PPM or serial channel in order to control the actuators of camera mounts/gimbals.
 
 
 
Version 0.1 utilises the Polulu breakout board with ATMEGA32L
 
LoCoHEad - Pros
- Very low price in comparing two of gyro-based systems there
- No Drift
- pitch, roll and yaw axis, ie. all-natural movement of the head is covered
- All sensor functions in one chip, no mechanical alignment / vertical boards needed
- Simple interface
- Easy sensor calibration
- Open source software
 
LoCoHead - Contra
- influenced by magnetic interference
- influenced by strong dynamic acceleration, esp Axial
- Lower resolution of the yaw axis (+ / - 0.5%) than that of roll and pitch (+ / - 0.05%)
 
Part of LoCoHead's code is based on the polulu example
(see polulu_LICENCE.txt)
/LoCoHead/headtracker.c
0,0 → 1,222
#include "vector.h"
#include <math.h>
#include <inttypes.h>
#include <avr/io.h>
#include <stdlib.h>
 
extern vector m_max;
extern vector m_min;
 
 
void i2c_start() {
TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN); // send start condition
while (!(TWCR & (1 << TWINT)));
}
void i2c_write_byte(char byte) {
TWDR = byte;
TWCR = (1 << TWINT) | (1 << TWEN); // start address transmission
while (!(TWCR & (1 << TWINT)));
}
char i2c_read_byte() {
TWCR = (1 << TWINT) | (1 << TWEA) | (1 << TWEN); // start data reception, transmit ACK
while (!(TWCR & (1 << TWINT)));
return TWDR;
}
 
char i2c_read_last_byte() {
TWCR = (1 << TWINT) | (1 << TWEN); // start data reception
while (!(TWCR & (1 << TWINT)));
return TWDR;
}
void i2c_stop() {
TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); // send stop condition
}
 
 
// Returns a set of acceleration and raw magnetic readings from the cmp01a.
void read_data_raw(vector *a, vector *m)
{
// read accelerometer values
i2c_start();
i2c_write_byte(0x30); // write acc
i2c_write_byte(0xa8); // OUT_X_L_A, MSB set to enable auto-increment
i2c_start(); // repeated start
i2c_write_byte(0x31); // read acc
unsigned char axl = i2c_read_byte();
unsigned char axh = i2c_read_byte();
unsigned char ayl = i2c_read_byte();
unsigned char ayh = i2c_read_byte();
unsigned char azl = i2c_read_byte();
unsigned char azh = i2c_read_last_byte();
i2c_stop();
 
// read magnetometer values
i2c_start();
i2c_write_byte(0x3C); // write mag
i2c_write_byte(0x03); // OUTXH_M
i2c_start(); // repeated start
i2c_write_byte(0x3D); // read mag
unsigned char mxh = i2c_read_byte();
unsigned char mxl = i2c_read_byte();
unsigned char myh = i2c_read_byte();
unsigned char myl = i2c_read_byte();
unsigned char mzh = i2c_read_byte();
unsigned char mzl = i2c_read_last_byte();
i2c_stop();
 
a->x = axh << 8 | axl;
a->y = ayh << 8 | ayl;
a->z = azh << 8 | azl;
m->x = mxh << 8 | mxl;
m->y = myh << 8 | myl;
m->z = mzh << 8 | mzl;
}
 
float IIR2(float x, float* z)
{
 
//const for butterworth lowpass fc 0.5Hz
// const float a[3] = {1.0000, -1.8521, 0.8623};
// const float b[3] = {0.0026, 0.0051, 0.0026};
 
//const for butterworth lowpass fc 2Hz
const float a[3] = {1.0000, -1.4190, 0.5533};
const float b[3] = {0.0336, 0.0671, 0.0336};
 
 
float y,r;
 
r = a[1]*z[0]+a[2]*z[1];
y = b[0]*(x-r)+b[1]*z[0]+b[2]*z[1];
z[1]= z[0];
z[0]= x-r;
 
return y;
 
}
 
 
//cancels out movemt below threshold while using step sum to
int thr_filter(int x, int * x_reg, int * y_reg)
{
int y;
int diff;
int sum = 0;
const int thr = 4;
const int lmt = 5;
 
diff = x - *x_reg;
 
if(abs(diff) <= thr)
{
sum += diff;
if(abs(sum) >= lmt)
{
sum = 0;
y = x;
}
else y = *y_reg;
}
else
{
y = x;
sum = 0;
}
 
*x_reg = x;
*y_reg = y;
 
return y;
}
 
 
// Returns corrected and low-pass filtered magnetometer and accelerometer values
void read_data(vector *a, vector *m)
{
//interal state buffers for IIR axis filtering
static float zm_x[2] = {0.0, 0.0};
static float zm_y[2] = {0.0, 0.0};
static float zm_z[2] = {0.0, 0.0};
static float za_x[2] = {0.0, 0.0};
static float za_y[2] = {0.0, 0.0};
static float za_z[2] = {0.0, 0.0};
 
read_data_raw(a, m);
//low pass filter acc
a->x = IIR2(a->x, za_x);
a->y = IIR2(a->y, za_y);
a->z = IIR2(a->z, za_z);
 
//compensate scale and offset, low pass filter mag
m->x = IIR2(((m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0), zm_x);
m->y = IIR2(((m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0), zm_y);
m->z = IIR2(((m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0), zm_z);
}
 
 
 
float get_heading(const vector *a, const vector *m, const vector *p)
{
vector E;
vector N;
 
// cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "west"
// -- right hand rule says
 
vector_cross(m, a, &E);
vector_normalize(&E);
 
// cross "down" with "east" to produce "north" (parallel to the ground)
vector_cross(a, &E, &N);
vector_normalize(&N);
 
// compute heading
float heading = atan2(vector_dot(&E, p), vector_dot(&N, p)) * 180.0 / M_PI;
return heading;
 
}
 
float get_perpendicular(const vector *a, const vector *d, const vector *q)
{
 
float sign = 0.0;
vector norma = *a;
 
if (q->x == 0.0) {norma.x = 0.0; sign = norma.y;}// cancel out movement on undesired axis
else if (q->y == 0.0) {norma.y = 0.0; sign = norma.x;}
vector_normalize(&norma);
 
// compute angle
float angle = acos(vector_dot(&norma,d)) * 180.0/M_PI;
if(sign >= 0.0) angle *= -1;
return angle;
 
}
 
int get_us(float angle, float deg_min, float deg_max, int pwm_min,int pwm_max)
{
//adjust sign change of angular function to new zero offset
if(angle < -180.0) angle += 360.0;
if(angle >= 180.0) angle -= 360.0;
 
//crop
if(angle < deg_min) angle = deg_min;
else if (angle > deg_max) angle = deg_max;
 
//scale to pwm
float ratio = ((float)(pwm_max - pwm_min)) / (deg_max - deg_min);
int diff = ((int)((angle-deg_min) * ratio));
return pwm_min + diff;
}
/LoCoHead/headtracker.h
0,0 → 1,72
//PPM FRAME
#define PPM_CHANNELS 8 //number of channels. Depends on RC, eg. FX-30 wants 8 or 12.
//All but HDG, PIT and ROL channel will be fixed to DEFAULT_US
#define PPM_FRAME 22000; //PPM frame length in microseconds
#define PPM_SYNC_PW 300; //PPM sync pulse length in microseconds. 300-400us usually
#define PPM_PIN (1<<PD4) //OCR1B
#define PPM_INV 0 //0= normal PPM, 1= inverted PPM
#define DEFAULT_US 1500
 
//HEADING
#define HDG_MIN -45.0 //min. Heading angle from starting point after which hdg will be clipped
#define HDG_MAX +45.0 //max. Heading angle
#define HDG_US_MIN 1000 //min. Heading Servo PWM pulse width in microseconds
#define HDG_US_MAX 2000 //max. Heading Servo PWM pulse width in microseconds
#define HDG_CH 6 //Heading CH number
 
//PITCH
#define PIT_MIN -45.0
#define PIT_MAX +45.0
#define PIT_US_MIN 1000
#define PIT_US_MAX 2000
#define PIT_CH 7
 
//ROLL
#define ROL_MIN -45.0
#define ROL_MAX +45.0
#define ROL_US_MIN 1000
#define ROL_US_MAX 2000
#define ROL_CH 8
 
/*
* m_max and m_min are calibration values for the maximum and minimum
* measurements recorded on each magnetic axis, which can vary for each
* LSM303DLH. You should replace the values below with max and min readings from
* your particular device.
*
* To obtain the max and min values, you can use this program's
* calibration mode, which is enabled by setting J1
*/
 
const vector p = {0, -1, 0};
const vector right = {1, 0, 0};
const vector down = {0, 0, -1};
vector m_max = {371, 583, 507};
vector m_min = {-798, -543, -471};
 
void i2c_start();
void i2c_write_byte(char byte);
char i2c_read_byte();
 
char i2c_read_last_byte();
void i2c_stop();
 
void ppm_init(void);
 
void read_data_raw(vector *a, vector *m);
 
float IIR2(float x, float* z);
 
int thr_filter(int x, int * x_reg, int * y_reg);
 
void read_data(vector *a, vector *m);
 
float get_heading(const vector *a, const vector *m, const vector *p);
 
float get_perpendicular(const vector *a, const vector *d, const vector *q);
 
int get_us(float angle, float deg_min, float deg_max, int pwm_min,int pwm_max);
/LoCoHead/main.c
0,0 → 1,169
#include <avr/io.h>
#include <math.h>
#include <stdlib.h>
#include "vector.h"
#include <util/delay.h>
#include <inttypes.h>
#include <avr/interrupt.h>
#include "headtracker.h"
 
#if(DEBUG)
#include "UART.h"
#include <string.h>
#endif
 
 
volatile uint16_t ppm_ch[PPM_CHANNELS];
 
enum calibration_mode {CAL_NONE, CAL_X, CAL_Y, CAL_Z};
 
void ppm_init(void)
{
 
for(int i = 0; i<PPM_CHANNELS; i++) ppm_ch[i] = DEFAULT_US;
//TCNT 1 - PPM out -
DDRD |= PPM_PIN;
TIMSK |= (1<<OCIE1B);
 
TCNT1 = 0;
OCR1A = ppm_ch[0];
OCR1B = PPM_SYNC_PW;
TCCR1A |= (1<<COM1B1) | (PPM_INV<<COM1B0) | (1<<WGM11) | (1<<WGM10);
TCCR1B |= (1<<WGM13) | (1<<WGM12);
TCCR1B |= (0<<CS12) | (1<<CS11) | (0<<CS10 );
 
sei();
 
}
 
 
ISR(TIMER1_COMPB_vect)
{
static uint8_t i = 0;
static uint16_t frame_pw = PPM_FRAME;
//output PPM channels
++i;
if(i >= PPM_CHANNELS+1)
{
i = 0;
frame_pw = PPM_FRAME;
}
if(i < PPM_CHANNELS)
{
 
OCR1A = ppm_ch[i];
frame_pw -= OCR1A;
}
else
{
OCR1A = frame_pw;
}
}
 
 
 
int main()
{
_delay_ms(500); //delay for VCC stabilization
ppm_init();
#if(DEBUG)
uart_init();
#endif
 
//************************************************************************
//** init LSM303DLH **
//************************************************************************
DDRC = 0; // all inputs
PORTC = (1 << PORTC4) | (1 << PORTC5); // enable pull-ups on SDA and SCL
 
 
TWSR = 0; // clear bit-rate prescale bits
TWBR = 17;
 
//enable accelerometer
i2c_start();
i2c_write_byte(0x30); // write acc
i2c_write_byte(0x20); // CTRL_REG1_A
i2c_write_byte(0x27); // normal power mode, 50 Hz data rate, all axes enabled
i2c_stop();
 
//enable magnetometer
i2c_start();
i2c_write_byte(0x3C); // write mag
i2c_write_byte(0x02); // MR_REG_M
i2c_write_byte(0x00); // continuous conversion mode
i2c_stop();
 
//raw acc and mag
vector a, m;
//filtered acc and mag
vector a_avg, m_avg;
 
//IIR filter will overshoot at startup, read in a couple of values to get it stable
for(int i = 0; i<20; i++) read_data(&a_avg, &m_avg);
//get zero reference
float heading_start, pitch_start, roll_start;
heading_start = get_heading(&a_avg, &m_avg, &p);
pitch_start = get_perpendicular(&a_avg, &down, &p);
roll_start = get_perpendicular(&a_avg, &down, &right);
 
enum calibration_mode mode = CAL_NONE;
vector cal_m_max = {0, 0, 0};
vector cal_m_min = {0, 0, 0};
 
while(1)
{
if (mode == CAL_NONE) // run mode
{
int thr_in, thr_out;
read_data(&a_avg, &m_avg);
 
//get angle - startoffset
int heading_us = thr_filter(get_us(get_heading(&a_avg, &m_avg, &p) - heading_start, HDG_MIN, HDG_MAX, HDG_US_MIN, HDG_US_MAX),&thr_in, &thr_out);
int pitch_us = get_us(get_perpendicular(&a_avg, &down, &p) - pitch_start, PIT_MIN, PIT_MAX, PIT_US_MIN, PIT_US_MAX);
int roll_us = get_us(get_perpendicular(&a_avg, &down, &right) - roll_start, ROL_MIN, ROL_MAX, ROL_US_MIN, ROL_US_MAX);;
#if(DEBUG)
printf("H %d P %d R %d \n", heading_us, pitch_us, roll_us);
#endif
ppm_ch[HDG_CH-1] = heading_us; //subtract 1, CHs from 1 - N, array bounds from 0 - N-1
ppm_ch[PIT_CH-1] = pitch_us;
ppm_ch[ROL_CH-1] = roll_us;
}
 
else // calibration mode - record max/min measurements
{
//TODO
//implement calib_test.c
read_data_raw(&a, &m);
if (m.x < cal_m_min.x) cal_m_min.x = m.x;
if (m.x > cal_m_max.x) cal_m_max.x = m.x;
if (m.y < cal_m_min.y) cal_m_min.y = m.y;
if (m.y > cal_m_max.y) cal_m_max.y = m.y;
if (m.z < cal_m_min.z) cal_m_min.z = m.z;
if (m.z > cal_m_max.z) cal_m_max.z = m.z;
 
 
}
 
_delay_ms(40);
}
}
 
/LoCoHead/polulu_LICENSE.txt
0,0 → 1,25
Copyright (c) 2011 Pololu Corporation. For more information, see
 
http://www.pololu.com/
http://forum.pololu.com/
 
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.
/LoCoHead/uart.c
0,0 → 1,63
/****************************************************************************
UART INITIALIZATION
 
This Program enables you to send data from the ATmega32 serial port to
the PC so that the data can be read on hyperterminal. This is very
useful for debugging purposes when the development system has no
display of its own.
 
To send data through the UART printf() is used.
 
uart.c and uart.h must be included in the main project, and the function
uart_init() called to enable serial port output.
 
In order to print floating point values, you will need to add the line
"-Wl,-u,vfprintf -lprintf_flt -lm" to AVR Studio4 linker options by going to
Project->Configuration Options->Custom Options
 
Read the "Detailed Description" section of
http://www.nongnu.org/avr-libc/user-manual/group__avr__stdio.html for
more detail.
 
*******************************************************************************/
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include "uart.h"
 
int put_uart_char(char c, FILE *fd)
{
while (!(UCSRA & _BV(UDRE))); //Wait for transmit buffer to become empty.
UDR = c; //Write the character to the UART.
return 0;
}
 
void uart_init(void)
{
//UART Init
 
UCSRB |= (1<<TXEN); // UART TX einschalten
UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<<UCSZ0); // Asynchron 8N1
UBRRH = UBRR_VAL >> 8;
UBRRL = UBRR_VAL & 0xFF;
 
 
//Call fdevopen. This is what binds printf to put_uart_char. The first
//parameter is the address of our function that will output a single
//character, the second parameter is an optional parameter that is
//used for get functions, ie. receiving a character from the UART.
//This is the function that uses malloc.
fdevopen(&put_uart_char, NULL);
//Enable the interrupts globally.
SREG |= _BV(SREG_I);
}
/LoCoHead/vector.c
0,0 → 1,27
#include "vector.h"
#include <math.h>
 
void vector_cross(const vector *a, const vector *b, vector *out)
{
out->x = a->y * b->z - a->z * b->y;
out->y = a->z * b->x - a->x * b->z;
out->z = a->x * b->y - a->y * b->x;
}
 
float vector_dot(const vector *a, const vector *b)
{
return a->x * b->x + a->y * b->y + a->z * b->z;
}
 
void vector_normalize(vector *a)
{
float mag = sqrt(vector_dot(a, a));
a->x /= mag;
a->y /= mag;
a->z /= mag;
}
 
float vector_magnitude(const vector *a)
{
return sqrt(pow(a->x,2.0) + pow(a->y,2.0) + pow(a->z,2.0));
}
/LoCoHead/vector.h
0,0 → 1,14
#ifndef _VECTOR_H_
#define _VECTOR_H_
 
typedef struct vector
{
float x, y, z;
} vector;
 
extern void vector_cross(const vector *a, const vector *b, vector *out);
extern float vector_dot(const vector *a, const vector *b);
extern void vector_normalize(vector *a);
extern float vector_magnitude(const vector *a);
 
#endif