Rev 726 |
Blame |
Last modification |
View Log
| RSS feed
/*
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_Update
() // called every 102.4 ms by timer 0 ISR
{
switch (MM3.
STATE)
{
case MM3_STATE_RESET
:
PORTC
&= ~
(1<<PORTC4
); // select slave
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;
}
}
PORTC
|= (1<<PORTC4
); // deselect slave
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 thrust is less than 100, stop calibration with a delay of 0.5 seconds
if (PPM_in
[ParamSet.
ChannelAssignment[CH_THRUST
]] < 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_pitch
, cos_pitch
, 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 pitch and tilt angle
angle
= (IntegralPitch
/div_factor
);
sin_pitch
= (int32_t)(c_sin_8192
(angle
));
cos_pitch
= (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_pitch
;
Hx_corr
-= Hz
* sin_pitch
;
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
);
// atan returns angular range from -180 deg to 180 deg in counter clockwise notation
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
if (heading
< 0) heading
= -heading
;
else heading
= 360 - heading
;
return heading
;
}