0,0 → 1,333 |
/* |
|
Copyright 2007, Niklas Nold |
|
This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify |
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
either version 3 of the License, or (at your option) any later version. |
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License |
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
*/ |
#include <stdlib.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
|
#include "mm3.h" |
#include "main.h" |
#include "mymath.h" |
#include "fc.h" |
#include "timer0.h" |
#include "rc.h" |
#include "eeprom.h" |
|
#define MAX_AXIS_VALUE 500 |
|
|
typedef struct |
{ |
uint8_t STATE; |
uint16_t DRDY; |
uint8_t AXIS; |
int16_t x_axis; |
int16_t y_axis; |
int16_t z_axis; |
} MM3_working_t; |
|
|
// MM3 State Machine |
#define MM3_STATE_RESET 0 |
#define MM3_STATE_START_TRANSFER 1 |
#define MM3_STATE_WAIT_DRDY 2 |
#define MM3_STATE_DRDY 3 |
#define MM3_STATE_BYTE2 4 |
|
#define MM3_X_AXIS 0x01 |
#define MM3_Y_AXIS 0x02 |
#define MM3_Z_AXIS 0x03 |
|
|
#define MM3_PERIOD_32 0x00 |
#define MM3_PERIOD_64 0x10 |
#define MM3_PERIOD_128 0x20 |
#define MM3_PERIOD_256 0x30 |
#define MM3_PERIOD_512 0x40 |
#define MM3_PERIOD_1024 0x50 |
#define MM3_PERIOD_2048 0x60 |
#define MM3_PERIOD_4096 0x70 |
|
MM3_calib_t MM3_calib; |
volatile MM3_working_t MM3; |
|
|
|
/*********************************************/ |
/* Initialize Interface to MM3 Compass */ |
/*********************************************/ |
void MM3_init(void) |
{ |
uint8_t sreg = SREG; |
|
cli(); |
|
// Configure Pins for SPI |
// set SCK (PB7), MOSI (PB5) as output |
DDRB |= (1<<DDB7)|(1<<DDB5); |
// set MISO (PB6) as input |
DDRB &= ~(1<<DDB6); |
|
// Output Pins PC4->MM3_SS ,PC5->MM3_RESET |
DDRC |= (1<<DDC4)|(1<<DDC5); |
// set pins permanent to low |
PORTC &= ~((1<<PORTC4)|(1<<PORTC5)); |
|
// Initialize SPI-Interface |
// Enable interrupt (SPIE=1) |
// Enable SPI bus (SPE=1) |
// MSB transmitted first (DORD = 0) |
// Master SPI Mode (MSTR=1) |
// Clock polarity low whn idle (CPOL=0) |
// clock phase sample at leading edge (CPHA=0) |
// clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz |
SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0); |
SPSR &= ~(1<<SPI2X); |
|
// Init Statemachine |
MM3.AXIS = MM3_X_AXIS; |
MM3.STATE = MM3_STATE_RESET; |
|
// Read calibration from EEprom |
MM3_calib.X_off = (int8_t)GetParamByte(PID_MM3_X_OFF); |
MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF); |
MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF); |
MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE); |
MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE); |
MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE); |
|
SREG = sreg; |
} |
|
|
/*********************************************/ |
/* Get Data from MM3 */ |
/*********************************************/ |
void MM3_timer0() // called every 102.4 ms by timer 0 ISR |
{ |
switch (MM3.STATE) |
{ |
case MM3_STATE_RESET: |
PORTC |= (1<<PORTC5); // PC5 to High, MM3 Reset |
MM3.STATE = MM3_STATE_START_TRANSFER; |
return; |
|
case MM3_STATE_START_TRANSFER: |
PORTC &= ~(1<<PORTC5); // PC4 auf Low (was 102.4 µs at high level) |
|
// write to SPDR triggers automatically the transfer MOSI MISO |
// MM3 Period, + AXIS code |
if (MM3.AXIS == MM3_X_AXIS) SPDR = MM3_PERIOD_256 + MM3_X_AXIS; |
else if (MM3.AXIS == MM3_Y_AXIS) SPDR = MM3_PERIOD_256 + MM3_Y_AXIS; |
else SPDR = MM3_PERIOD_256 + MM3_Z_AXIS; // MM3_Z_AXIS |
|
// DRDY line is not connected, therefore |
// wait before reading data back |
MM3.DRDY = SetDelay(8); // wait 8ms for data ready |
MM3.STATE = MM3_STATE_WAIT_DRDY; |
return; |
|
case MM3_STATE_WAIT_DRDY: |
if (CheckDelay(MM3.DRDY)) |
{ |
// write something into SPDR to trigger data reading |
SPDR = 0x00; |
MM3.STATE = MM3_STATE_DRDY; |
} |
return; |
} |
} |
|
|
/*********************************************/ |
/* Interrupt SPI transfer complete */ |
/*********************************************/ |
ISR(SPI_STC_vect) |
{ |
static int8_t tmp; |
int16_t value; |
|
switch (MM3.STATE) |
{ |
// 1st byte received |
case MM3_STATE_DRDY: |
tmp = SPDR; // store 1st byte |
SPDR = 0x00; // trigger transfer of 2nd byte |
MM3.STATE = MM3_STATE_BYTE2; |
return; |
|
case MM3_STATE_BYTE2: // 2nd byte received |
value = (int16_t)tmp; // combine the 1st and 2nd byte to a word |
value <<= 8; // shift 1st byte to MSB-Position |
value |= (int16_t)SPDR; // add 2nd byte |
|
if(abs(value) < MAX_AXIS_VALUE) // ignore spikes |
{ |
switch (MM3.AXIS) |
{ |
case MM3_X_AXIS: |
MM3.x_axis = value; |
MM3.AXIS = MM3_Y_AXIS; |
break; |
case MM3_Y_AXIS: |
MM3.y_axis = value; |
MM3.AXIS = MM3_Z_AXIS; |
break; |
case MM3_Z_AXIS: |
MM3.z_axis = value; |
MM3.AXIS = MM3_X_AXIS; |
break; |
default: |
MM3.AXIS = MM3_X_AXIS; |
break; |
} |
} |
MM3.STATE = MM3_STATE_RESET; |
} |
} |
|
|
|
/*********************************************/ |
/* Calibrate Compass */ |
/*********************************************/ |
void MM3_calibrate(void) |
{ |
int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0; |
uint8_t measurement = 50, beeper = 0; |
uint16_t timer; |
|
GRN_ON; |
ROT_OFF; |
|
// get maximum and minimum reading of all axis |
while (measurement) |
{ |
if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
else if (MM3.x_axis < x_min) x_min = MM3.x_axis; |
|
if (MM3.y_axis > y_max) y_max = MM3.y_axis; |
else if (MM3.y_axis < y_min) y_min = MM3.y_axis; |
|
if (MM3.z_axis > z_max) z_max = MM3.z_axis; |
else if (MM3.z_axis < z_min) z_min = MM3.z_axis; |
|
if (!beeper) |
{ |
ROT_FLASH; |
GRN_FLASH; |
BeepTime = 50; |
beeper = 50; |
} |
beeper--; |
|
// loop with period of 10 ms / 100 Hz |
timer = SetDelay(10); |
while(!CheckDelay(timer)); |
|
// If Gas is less than 100, stop calibration with a delay of 0.5 seconds |
if (PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < 100) measurement--; |
} |
|
// Rage of all axis |
MM3_calib.X_range = (x_max - x_min); |
MM3_calib.Y_range = (y_max - y_min); |
MM3_calib.Z_range = (z_max - z_min); |
|
// Offset of all axis |
MM3_calib.X_off = (x_max + x_min) / 2; |
MM3_calib.Y_off = (y_max + y_min) / 2; |
MM3_calib.Z_off = (z_max + z_min) / 2; |
|
// save to EEProm |
SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
} |
|
|
/*********************************************/ |
/* Calculate north direction (heading) */ |
/*********************************************/ |
int16_t MM3_heading(void) |
{ |
int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw; |
int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
int16_t angle; |
uint16_t div_factor; |
int16_t heading; |
|
// calibration factor for transforming Gyro Integrals to angular degrees |
div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
|
// Offset correction and normalization (values of H are +/- 512) |
Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range; |
Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range; |
Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range; |
|
// Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation |
// assuming the MM3 board is mounted parallel to the frame. |
// User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading |
// in a top view counter clockwise direction. |
// North is in opposite direction of the small arrow on the MM3 board. |
// Therefore 180 deg must be added to that angle. |
angle = ((int16_t)ParamSet.UserParam4 + 180); |
// wrap angle to interval of 0°- 359° |
angle += 360; |
angle %= 360; |
sin_yaw = (int32_t)(c_sin_8192(angle)); |
cos_yaw = (int32_t)(c_cos_8192(angle)); |
|
Hx_corr = Hx; |
Hy_corr = Hy; |
|
// rotate |
Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192; |
Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192; |
|
|
// tilt compensation |
|
// calibration factor for transforming Gyro Integrals to angular degrees |
div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
|
// calculate sinus cosinus of nick and tilt angle |
angle = (IntegralNick/div_factor); |
sin_nick = (int32_t)(c_sin_8192(angle)); |
cos_nick = (int32_t)(c_cos_8192(angle)); |
|
angle = (IntegralRoll/div_factor); |
sin_roll = (int32_t)(c_sin_8192(angle)); |
cos_roll = (int32_t)(c_cos_8192(angle)); |
|
Hx_corr = Hx * cos_nick; |
Hx_corr -= Hz * sin_nick; |
Hx_corr /= 8192; |
|
Hy_corr = Hy * cos_roll; |
Hy_corr += Hz * sin_roll; |
Hy_corr /= 8192; |
|
// calculate Heading |
heading = c_atan2(Hy_corr, Hx_corr); |
|
// transform range from +-180° to 0°- 359° |
heading += 360; |
heading %= 360; |
|
return heading; |
} |