WebSVN - FlightCtrl - Diff - Rev 781 and 782 - /branches/V0.68d Code Redesign killagreg/mm3.c


/*
 
Copyright 2008, by Killagreg
 
This program (files mm3.c and mm3.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: The original implementation was done by Niklas Nold.
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"
#include "uart.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;
volatile uint8_t MM3_Timeout = 0;
 
 
 
/*********************************************/
/*  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);
 
#ifdef USE_WALTER_EXT // walthers board
        // Output Pins (J9)PC6->MM3_SS ,(J8)PB2->MM3_RESET
        DDRB |= (1<<DDB2);
        DDRC |= (1<<DDC6);
        // set pins permanent to low
        PORTB &= ~((1<<PORTB2));
        PORTC &= ~((1<<PORTC6));
#else // killagregs board
        // Output Pins PC4->MM3_SS ,PC5->MM3_RESET
        DDRC |= (1<<DDC4)|(1<<DDC5);
        // set pins permanent to low
        PORTC &= ~((1<<PORTC4)|(1<<PORTC5));
#endif
 
        // 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 when 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);
 
        MM3_Timeout = 0;
 
        SREG = sreg;
}
 
 
/*********************************************/
/*  Get Data from MM3                        */
/*********************************************/
void MM3_Update(void) // called every 102.4 µs by timer 0 ISR
{
        switch (MM3.STATE)
        {
        case MM3_STATE_RESET:
                #ifdef USE_WALTER_EXT // walthers board
                PORTC &= ~(1<<PORTC6);  // select slave
                PORTB |= (1<<PORTB2);   // PB2 to High, MM3 Reset
                #else
                PORTC &= ~(1<<PORTC4);  // select slave
                PORTC |= (1<<PORTC5);   // PC5 to High, MM3 Reset
                #endif
                MM3.STATE = MM3_STATE_START_TRANSFER;
                return;
 
        case MM3_STATE_START_TRANSFER:
                #ifdef USE_WALTER_EXT // walthers board
                PORTB &= ~(1<<PORTB2);  // PB2 auf Low (was 102.4 µs at high level)
                #else
                PORTC &= ~(1<<PORTC5);  // PC4 auf Low (was 102.4 µs at high level)
                #endif
                // write to SPDR triggers automatically the transfer MOSI MISO
                // MM3 Period, + AXIS code
                switch(MM3.AXIS)
                {
                case MM3_X_AXIS:
                        SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
                        break;
                case MM3_Y_AXIS:
                        SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
                        break;
                case MM3_Z_AXIS:
                        SPDR = MM3_PERIOD_256 + MM3_Z_AXIS;
                        break;
                default:
                        MM3.AXIS = MM3_X_AXIS;
                        MM3.STATE = MM3_STATE_RESET;
                        return;
                }
 
                // 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;
                        }
                }
                #ifdef USE_WALTER_EXT // walthers board
                PORTC |= (1<<PORTC6); // deselect slave
                #else
                PORTC |= (1<<PORTC4); // deselect slave
                #endif
                MM3.STATE = MM3_STATE_RESET;
                // Update timeout is called every 102.4 µs.
                // It takes 2 cycles to write a measurement data request for one axis and
                // at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back.
                // I.e. 81 cycles * 102.4 µs = 8.3ms per axis.
                // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() -
                // decremtent the MM3_Timeout every 100 ms.
                // incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout.
                if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis))
                {       // if all axis measurements give diffrent readings the data should be valid
                        if(MM3_Timeout < 20) MM3_Timeout++;
                }
                else // something is very strange here
                {
                        if(MM3_Timeout ) MM3_Timeout--;
                }
                return;
 
        default:
                return;
        }
}
 
 
 
/*********************************************/
/*  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;
 
        DebugOut.Analog[11] = MM3_Timeout;
 
        if (MM3_Timeout)
        {
                // 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;
        }
        else // MM3_Timeout = 0 i.e now new data from external board
        {
                if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem
                heading = -1;
        }
return heading;
}
 

Rev 781	Rev 782
1	/*	1	/*
2		2
3	Copyright 2008, by Killagreg	3	Copyright 2008, by Killagreg
4		4
5	This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify	5	This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify
6	it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;	6	it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation;
7	either version 3 of the License, or (at your option) any later version.	7	either version 3 of the License, or (at your option) any later version.
8	This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;	8	This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	9	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10	GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License	10	GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License
11	along with this program. If not, see <http://www.gnu.org/licenses/>.	11	along with this program. If not, see <http://www.gnu.org/licenses/>.
12		12
13	Please note: The original implementation was done by Niklas Nold.	13	Please note: The original implementation was done by Niklas Nold.
14	All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de	14	All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de
15	*/	15	*/
16	#include <stdlib.h>	16	#include <stdlib.h>
17	#include <avr/io.h>	17	#include <avr/io.h>
18	#include <avr/interrupt.h>	18	#include <avr/interrupt.h>
19		19
20	#include "mm3.h"	20	#include "mm3.h"
21	#include "main.h"	21	#include "main.h"
22	#include "mymath.h"	22	#include "mymath.h"
23	#include "fc.h"	23	#include "fc.h"
24	#include "timer0.h"	24	#include "timer0.h"
25	#include "rc.h"	25	#include "rc.h"
26	#include "eeprom.h"	26	#include "eeprom.h"
-		27	#include "uart.h"
27		28
28	#define MAX_AXIS_VALUE 500	29	#define MAX_AXIS_VALUE 500
29		30
30		31
31	typedef struct	32	typedef struct
32	{	33	{
33	uint8_t STATE;	34	uint8_t STATE;
34	uint16_t DRDY;	35	uint16_t DRDY;
35	uint8_t AXIS;	36	uint8_t AXIS;
36	int16_t x_axis;	37	int16_t x_axis;
37	int16_t y_axis;	38	int16_t y_axis;
38	int16_t z_axis;	39	int16_t z_axis;
39	} MM3_working_t;	40	} MM3_working_t;
40		41
41		42
42	// MM3 State Machine	43	// MM3 State Machine
43	#define MM3_STATE_RESET 0	44	#define MM3_STATE_RESET 0
44	#define MM3_STATE_START_TRANSFER 1	45	#define MM3_STATE_START_TRANSFER 1
45	#define MM3_STATE_WAIT_DRDY 2	46	#define MM3_STATE_WAIT_DRDY 2
46	#define MM3_STATE_DRDY 3	47	#define MM3_STATE_DRDY 3
47	#define MM3_STATE_BYTE2 4	48	#define MM3_STATE_BYTE2 4
48		49
49	#define MM3_X_AXIS 0x01	50	#define MM3_X_AXIS 0x01
50	#define MM3_Y_AXIS 0x02	51	#define MM3_Y_AXIS 0x02
51	#define MM3_Z_AXIS 0x03	52	#define MM3_Z_AXIS 0x03
52		53
53		54
54	#define MM3_PERIOD_32 0x00	55	#define MM3_PERIOD_32 0x00
55	#define MM3_PERIOD_64 0x10	56	#define MM3_PERIOD_64 0x10
56	#define MM3_PERIOD_128 0x20	57	#define MM3_PERIOD_128 0x20
57	#define MM3_PERIOD_256 0x30	58	#define MM3_PERIOD_256 0x30
58	#define MM3_PERIOD_512 0x40	59	#define MM3_PERIOD_512 0x40
59	#define MM3_PERIOD_1024 0x50	60	#define MM3_PERIOD_1024 0x50
60	#define MM3_PERIOD_2048 0x60	61	#define MM3_PERIOD_2048 0x60
61	#define MM3_PERIOD_4096 0x70	62	#define MM3_PERIOD_4096 0x70
62		63
63	MM3_calib_t MM3_calib;	64	MM3_calib_t MM3_calib;
64	volatile MM3_working_t MM3;	65	volatile MM3_working_t MM3;
65	volatile uint8_t MM3_Timeout = 0;	66	volatile uint8_t MM3_Timeout = 0;
66		67
67		68
68		69
69	/*********************************************/	70	/*********************************************/
70	/* Initialize Interface to MM3 Compass */	71	/* Initialize Interface to MM3 Compass */
71	/*********************************************/	72	/*********************************************/
72	void MM3_Init(void)	73	void MM3_Init(void)
73	{	74	{
74	uint8_t sreg = SREG;	75	uint8_t sreg = SREG;
75		76
76	cli();	77	cli();
77		78
78	// Configure Pins for SPI	79	// Configure Pins for SPI
79	// set SCK (PB7), MOSI (PB5) as output	80	// set SCK (PB7), MOSI (PB5) as output
80	DDRB \|= (1<<DDB7)\|(1<<DDB5);	81	DDRB \|= (1<<DDB7)\|(1<<DDB5);
81	// set MISO (PB6) as input	82	// set MISO (PB6) as input
82	DDRB &= ~(1<<DDB6);	83	DDRB &= ~(1<<DDB6);
83		84
84	#ifdef USE_WALTER_EXT // walthers board	85	#ifdef USE_WALTER_EXT // walthers board
85	// Output Pins (J9)PC6->MM3_SS ,(J8)PB2->MM3_RESET	86	// Output Pins (J9)PC6->MM3_SS ,(J8)PB2->MM3_RESET
86	DDRB \|= (1<<DDB2);	87	DDRB \|= (1<<DDB2);
87	DDRC \|= (1<<DDC6);	88	DDRC \|= (1<<DDC6);
88	// set pins permanent to low	89	// set pins permanent to low
89	PORTB &= ~((1<<PORTB2));	90	PORTB &= ~((1<<PORTB2));
90	PORTC &= ~((1<<PORTC6));	91	PORTC &= ~((1<<PORTC6));
91	#else // killagregs board	92	#else // killagregs board
92	// Output Pins PC4->MM3_SS ,PC5->MM3_RESET	93	// Output Pins PC4->MM3_SS ,PC5->MM3_RESET
93	DDRC \|= (1<<DDC4)\|(1<<DDC5);	94	DDRC \|= (1<<DDC4)\|(1<<DDC5);
94	// set pins permanent to low	95	// set pins permanent to low
95	PORTC &= ~((1<<PORTC4)\|(1<<PORTC5));	96	PORTC &= ~((1<<PORTC4)\|(1<<PORTC5));
96	#endif	97	#endif
97		98
98	// Initialize SPI-Interface	99	// Initialize SPI-Interface
99	// Enable interrupt (SPIE=1)	100	// Enable interrupt (SPIE=1)
100	// Enable SPI bus (SPE=1)	101	// Enable SPI bus (SPE=1)
101	// MSB transmitted first (DORD = 0)	102	// MSB transmitted first (DORD = 0)
102	// Master SPI Mode (MSTR=1)	103	// Master SPI Mode (MSTR=1)
103	// Clock polarity low when idle (CPOL=0)	104	// Clock polarity low when idle (CPOL=0)
104	// Clock phase sample at leading edge (CPHA=0)	105	// Clock phase sample at leading edge (CPHA=0)
105	// Clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz	106	// Clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz
106	SPCR = (1<<SPIE)\|(1<<SPE)\|(0<<DORD)\|(1<<MSTR)\|(0<<CPOL)\|(0<<CPHA)\|(1<<SPR1)\|(1<<SPR0);	107	SPCR = (1<<SPIE)\|(1<<SPE)\|(0<<DORD)\|(1<<MSTR)\|(0<<CPOL)\|(0<<CPHA)\|(1<<SPR1)\|(1<<SPR0);
107	SPSR &= ~(1<<SPI2X);	108	SPSR &= ~(1<<SPI2X);
108		109
109	// Init Statemachine	110	// Init Statemachine
110	MM3.AXIS = MM3_X_AXIS;	111	MM3.AXIS = MM3_X_AXIS;
111	MM3.STATE = MM3_STATE_RESET;	112	MM3.STATE = MM3_STATE_RESET;
112		113
113	// Read calibration from EEprom	114	// Read calibration from EEprom
114	MM3_calib.X_off = (int8_t)GetParamByte(PID_MM3_X_OFF);	115	MM3_calib.X_off = (int8_t)GetParamByte(PID_MM3_X_OFF);
115	MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF);	116	MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF);
116	MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF);	117	MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF);
117	MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);	118	MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE);
118	MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);	119	MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE);
119	MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);	120	MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE);
120		121
121	MM3_Timeout = 0;	122	MM3_Timeout = 0;
122		123
123	SREG = sreg;	124	SREG = sreg;
124	}	125	}
125		126
126		127
127	/*********************************************/	128	/*********************************************/
128	/* Get Data from MM3 */	129	/* Get Data from MM3 */
129	/*********************************************/	130	/*********************************************/
130	void MM3_Update(void) // called every 102.4 µs by timer 0 ISR	131	void MM3_Update(void) // called every 102.4 µs by timer 0 ISR
131	{	132	{
132	switch (MM3.STATE)	133	switch (MM3.STATE)
133	{	134	{
134	case MM3_STATE_RESET:	135	case MM3_STATE_RESET:
135	#ifdef USE_WALTER_EXT // walthers board	136	#ifdef USE_WALTER_EXT // walthers board
136	PORTC &= ~(1<<PORTC6); // select slave	137	PORTC &= ~(1<<PORTC6); // select slave
137	PORTB \|= (1<<PORTB2); // PB2 to High, MM3 Reset	138	PORTB \|= (1<<PORTB2); // PB2 to High, MM3 Reset
138	#else	139	#else
139	PORTC &= ~(1<<PORTC4); // select slave	140	PORTC &= ~(1<<PORTC4); // select slave
140	PORTC \|= (1<<PORTC5); // PC5 to High, MM3 Reset	141	PORTC \|= (1<<PORTC5); // PC5 to High, MM3 Reset
141	#endif	142	#endif
142	MM3.STATE = MM3_STATE_START_TRANSFER;	143	MM3.STATE = MM3_STATE_START_TRANSFER;
143	return;	144	return;
144		145
145	case MM3_STATE_START_TRANSFER:	146	case MM3_STATE_START_TRANSFER:
146	#ifdef USE_WALTER_EXT // walthers board	147	#ifdef USE_WALTER_EXT // walthers board
147	PORTB &= ~(1<<PORTB2); // PB2 auf Low (was 102.4 µs at high level)	148	PORTB &= ~(1<<PORTB2); // PB2 auf Low (was 102.4 µs at high level)
148	#else	149	#else
149	PORTC &= ~(1<<PORTC5); // PC4 auf Low (was 102.4 µs at high level)	150	PORTC &= ~(1<<PORTC5); // PC4 auf Low (was 102.4 µs at high level)
150	#endif	151	#endif
151	// write to SPDR triggers automatically the transfer MOSI MISO	152	// write to SPDR triggers automatically the transfer MOSI MISO
152	// MM3 Period, + AXIS code	153	// MM3 Period, + AXIS code
153	switch(MM3.AXIS)	154	switch(MM3.AXIS)
154	{	155	{
155	case MM3_X_AXIS:	156	case MM3_X_AXIS:
156	SPDR = MM3_PERIOD_256 + MM3_X_AXIS;	157	SPDR = MM3_PERIOD_256 + MM3_X_AXIS;
157	break;	158	break;
158	case MM3_Y_AXIS:	159	case MM3_Y_AXIS:
159	SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;	160	SPDR = MM3_PERIOD_256 + MM3_Y_AXIS;
160	break;	161	break;
161	case MM3_Z_AXIS:	162	case MM3_Z_AXIS:
162	SPDR = MM3_PERIOD_256 + MM3_Z_AXIS;	163	SPDR = MM3_PERIOD_256 + MM3_Z_AXIS;
163	break;	164	break;
164	default:	165	default:
165	MM3.AXIS = MM3_X_AXIS;	166	MM3.AXIS = MM3_X_AXIS;
166	MM3.STATE = MM3_STATE_RESET;	167	MM3.STATE = MM3_STATE_RESET;
167	return;	168	return;
168	}	169	}
169		170
170	// DRDY line is not connected, therefore	171	// DRDY line is not connected, therefore
171	// wait before reading data back	172	// wait before reading data back
172	MM3.DRDY = SetDelay(8); // wait 8ms for data ready	173	MM3.DRDY = SetDelay(8); // wait 8ms for data ready
173	MM3.STATE = MM3_STATE_WAIT_DRDY;	174	MM3.STATE = MM3_STATE_WAIT_DRDY;
174	return;	175	return;
175		176
176	case MM3_STATE_WAIT_DRDY:	177	case MM3_STATE_WAIT_DRDY:
177	if (CheckDelay(MM3.DRDY))	178	if (CheckDelay(MM3.DRDY))
178	{	179	{
179	// write something into SPDR to trigger data reading	180	// write something into SPDR to trigger data reading
180	SPDR = 0x00;	181	SPDR = 0x00;
181	MM3.STATE = MM3_STATE_DRDY;	182	MM3.STATE = MM3_STATE_DRDY;
182	}	183	}
183	return;	184	return;
184	}	185	}
185	}	186	}
186		187
187		188
188	/*********************************************/	189	/*********************************************/
189	/* Interrupt SPI transfer complete */	190	/* Interrupt SPI transfer complete */
190	/*********************************************/	191	/*********************************************/
191	ISR(SPI_STC_vect)	192	ISR(SPI_STC_vect)
192	{	193	{
193	static int8_t tmp;	194	static int8_t tmp;
194	int16_t value;	195	int16_t value;
195		196
196	switch (MM3.STATE)	197	switch (MM3.STATE)
197	{	198	{
198	// 1st byte received	199	// 1st byte received
199	case MM3_STATE_DRDY:	200	case MM3_STATE_DRDY:
200	tmp = SPDR; // store 1st byte	201	tmp = SPDR; // store 1st byte
201	SPDR = 0x00; // trigger transfer of 2nd byte	202	SPDR = 0x00; // trigger transfer of 2nd byte
202	MM3.STATE = MM3_STATE_BYTE2;	203	MM3.STATE = MM3_STATE_BYTE2;
203	return;	204	return;
204		205
205	case MM3_STATE_BYTE2: // 2nd byte received	206	case MM3_STATE_BYTE2: // 2nd byte received
206	value = (int16_t)tmp; // combine the 1st and 2nd byte to a word	207	value = (int16_t)tmp; // combine the 1st and 2nd byte to a word
207	value <<= 8; // shift 1st byte to MSB-Position	208	value <<= 8; // shift 1st byte to MSB-Position
208	value \|= (int16_t)SPDR; // add 2nd byte	209	value \|= (int16_t)SPDR; // add 2nd byte
209		210
210	if(abs(value) < MAX_AXIS_VALUE) // ignore spikes	211	if(abs(value) < MAX_AXIS_VALUE) // ignore spikes
211	{	212	{
212	switch (MM3.AXIS)	213	switch (MM3.AXIS)
213	{	214	{
214	case MM3_X_AXIS:	215	case MM3_X_AXIS:
215	MM3.x_axis = value;	216	MM3.x_axis = value;
216	MM3.AXIS = MM3_Y_AXIS;	217	MM3.AXIS = MM3_Y_AXIS;
217	break;	218	break;
218	case MM3_Y_AXIS:	219	case MM3_Y_AXIS:
219	MM3.y_axis = value;	220	MM3.y_axis = value;
220	MM3.AXIS = MM3_Z_AXIS;	221	MM3.AXIS = MM3_Z_AXIS;
221	break;	222	break;
222	case MM3_Z_AXIS:	223	case MM3_Z_AXIS:
223	MM3.z_axis = value;	224	MM3.z_axis = value;
224	MM3.AXIS = MM3_X_AXIS;	225	MM3.AXIS = MM3_X_AXIS;
225	break;	226	break;
226	default:	227	default:
227	MM3.AXIS = MM3_X_AXIS;	228	MM3.AXIS = MM3_X_AXIS;
228	break;	229	break;
229	}	230	}
230	}	231	}
231	#ifdef USE_WALTER_EXT // walthers board	232	#ifdef USE_WALTER_EXT // walthers board
232	PORTC \|= (1<<PORTC6); // deselect slave	233	PORTC \|= (1<<PORTC6); // deselect slave
233	#else	234	#else
234	PORTC \|= (1<<PORTC4); // deselect slave	235	PORTC \|= (1<<PORTC4); // deselect slave
235	#endif	236	#endif
236	MM3.STATE = MM3_STATE_RESET;	237	MM3.STATE = MM3_STATE_RESET;
237	// Update timeout is called every 102.4 µs.	238	// Update timeout is called every 102.4 µs.
238	// It takes 2 cycles to write a measurement data request for one axis and	239	// It takes 2 cycles to write a measurement data request for one axis and
239	// at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back.	240	// at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back.
240	// I.e. 81 cycles * 102.4 µs = 8.3ms per axis.	241	// I.e. 81 cycles * 102.4 µs = 8.3ms per axis.
241	// The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() -	242	// The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() -
242	// decremtent the MM3_Timeout every 100 ms.	243	// decremtent the MM3_Timeout every 100 ms.
243	// incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout.	244	// incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout.
244	if ((MM3.x_axis != MM3.y_axis) \|\| (MM3.x_axis != MM3.z_axis) \|\| (MM3.y_axis != MM3.z_axis))	245	if ((MM3.x_axis != MM3.y_axis) \|\| (MM3.x_axis != MM3.z_axis) \|\| (MM3.y_axis != MM3.z_axis))
245	{ // if all axis measurements give diffrent readings the data should be valid	246	{ // if all axis measurements give diffrent readings the data should be valid
246	if(MM3_Timeout < 20) MM3_Timeout++;	247	if(MM3_Timeout < 20) MM3_Timeout++;
247	}	248	}
248	else // something is very strange here	249	else // something is very strange here
249	{	250	{
250	if(MM3_Timeout ) MM3_Timeout--;	251	if(MM3_Timeout ) MM3_Timeout--;
251	}	252	}
252	return;	253	return;
253		254
254	default:	255	default:
255	return;	256	return;
256	}	257	}
257	}	258	}
258		259
259		260
260		261
261	/*********************************************/	262	/*********************************************/
262	/* Calibrate Compass */	263	/* Calibrate Compass */
263	/*********************************************/	264	/*********************************************/
264	void MM3_Calibrate(void)	265	void MM3_Calibrate(void)
265	{	266	{
266	int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;	267	int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0;
267	uint8_t measurement = 50, beeper = 0;	268	uint8_t measurement = 50, beeper = 0;
268	uint16_t timer;	269	uint16_t timer;
269		270
270	GRN_ON;	271	GRN_ON;
271	ROT_OFF;	272	ROT_OFF;
272		273
273	// get maximum and minimum reading of all axis	274	// get maximum and minimum reading of all axis
274	while (measurement)	275	while (measurement)
275	{	276	{
276	if (MM3.x_axis > x_max) x_max = MM3.x_axis;	277	if (MM3.x_axis > x_max) x_max = MM3.x_axis;
277	else if (MM3.x_axis < x_min) x_min = MM3.x_axis;	278	else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
278		279
279	if (MM3.y_axis > y_max) y_max = MM3.y_axis;	280	if (MM3.y_axis > y_max) y_max = MM3.y_axis;
280	else if (MM3.y_axis < y_min) y_min = MM3.y_axis;	281	else if (MM3.y_axis < y_min) y_min = MM3.y_axis;
281		282
282	if (MM3.z_axis > z_max) z_max = MM3.z_axis;	283	if (MM3.z_axis > z_max) z_max = MM3.z_axis;
283	else if (MM3.z_axis < z_min) z_min = MM3.z_axis;	284	else if (MM3.z_axis < z_min) z_min = MM3.z_axis;
284		285
285	if (!beeper)	286	if (!beeper)
286	{	287	{
287	ROT_FLASH;	288	ROT_FLASH;
288	GRN_FLASH;	289	GRN_FLASH;
289	BeepTime = 50;	290	BeepTime = 50;
290	beeper = 50;	291	beeper = 50;
291	}	292	}
292	beeper--;	293	beeper--;
293	// loop with period of 10 ms / 100 Hz	294	// loop with period of 10 ms / 100 Hz
294	timer = SetDelay(10);	295	timer = SetDelay(10);
295	while(!CheckDelay(timer));	296	while(!CheckDelay(timer));
296		297
297	// If thrust is less than 100, stop calibration with a delay of 0.5 seconds	298	// If thrust is less than 100, stop calibration with a delay of 0.5 seconds
298	if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--;	299	if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--;
299	}	300	}
300	// Rage of all axis	301	// Rage of all axis
301	MM3_calib.X_range = (x_max - x_min);	302	MM3_calib.X_range = (x_max - x_min);
302	MM3_calib.Y_range = (y_max - y_min);	303	MM3_calib.Y_range = (y_max - y_min);
303	MM3_calib.Z_range = (z_max - z_min);	304	MM3_calib.Z_range = (z_max - z_min);
304		305
305	// Offset of all axis	306	// Offset of all axis
306	MM3_calib.X_off = (x_max + x_min) / 2;	307	MM3_calib.X_off = (x_max + x_min) / 2;
307	MM3_calib.Y_off = (y_max + y_min) / 2;	308	MM3_calib.Y_off = (y_max + y_min) / 2;
308	MM3_calib.Z_off = (z_max + z_min) / 2;	309	MM3_calib.Z_off = (z_max + z_min) / 2;
309		310
310	// save to EEProm	311	// save to EEProm
311	SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off);	312	SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off);
312	SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off);	313	SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off);
313	SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off);	314	SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off);
314	SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);	315	SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range);
315	SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);	316	SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range);
316	SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);	317	SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range);
317		318
318	}	319	}
319		320
320		321
321	/*********************************************/	322	/*********************************************/
322	/* Calculate north direction (heading) */	323	/* Calculate north direction (heading) */
323	/*********************************************/	324	/*********************************************/
324	int16_t MM3_Heading(void)	325	int16_t MM3_Heading(void)
325	{	326	{
326	int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw;	327	int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw;
327	int32_t Hx, Hy, Hz, Hx_corr, Hy_corr;	328	int32_t Hx, Hy, Hz, Hx_corr, Hy_corr;
328	int16_t angle;	329	int16_t angle;
329	uint16_t div_factor;	330	uint16_t div_factor;
330	int16_t heading;	331	int16_t heading;
-		332
-		333	DebugOut.Analog[11] = MM3_Timeout;
331		334
332	if (MM3_Timeout)	335	if (MM3_Timeout)
333	{	336	{
334	// Offset correction and normalization (values of H are +/- 512)	337	// Offset correction and normalization (values of H are +/- 512)
335	Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;	338	Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range;
336	Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;	339	Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range;
337	Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;	340	Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range;
338		341
339	// Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation	342	// Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation
340	// assuming the MM3 board is mounted parallel to the frame.	343	// assuming the MM3 board is mounted parallel to the frame.
341	// User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading	344	// User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading
342	// in a top view counter clockwise direction.	345	// in a top view counter clockwise direction.
343	// North is in opposite direction of the small arrow on the MM3 board.	346	// North is in opposite direction of the small arrow on the MM3 board.
344	// Therefore 180 deg must be added to that angle.	347	// Therefore 180 deg must be added to that angle.
345	angle = ((int16_t)ParamSet.UserParam4 + 180);	348	angle = ((int16_t)ParamSet.UserParam4 + 180);
346	// wrap angle to interval of 0°- 359°	349	// wrap angle to interval of 0°- 359°
347	angle += 360;	350	angle += 360;
348	angle %= 360;	351	angle %= 360;
349	sin_yaw = (int32_t)(c_sin_8192(angle));	352	sin_yaw = (int32_t)(c_sin_8192(angle));
350	cos_yaw = (int32_t)(c_cos_8192(angle));	353	cos_yaw = (int32_t)(c_cos_8192(angle));
351		354
352	Hx_corr = Hx;	355	Hx_corr = Hx;
353	Hy_corr = Hy;	356	Hy_corr = Hy;
354		357
355	// rotate	358	// rotate
356	Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192;	359	Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192;
357	Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192;	360	Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192;
358		361
359		362
360	// tilt compensation	363	// tilt compensation
361		364
362	// calibration factor for transforming Gyro Integrals to angular degrees	365	// calibration factor for transforming Gyro Integrals to angular degrees
363	div_factor = (uint16_t)ParamSet.UserParam3 * 8;	366	div_factor = (uint16_t)ParamSet.UserParam3 * 8;
364		367
365	// calculate sinus cosinus of pitch and tilt angle	368	// calculate sinus cosinus of pitch and tilt angle
366	angle = (IntegralPitch/div_factor);	369	angle = (IntegralPitch/div_factor);
367	sin_pitch = (int32_t)(c_sin_8192(angle));	370	sin_pitch = (int32_t)(c_sin_8192(angle));
368	cos_pitch = (int32_t)(c_cos_8192(angle));	371	cos_pitch = (int32_t)(c_cos_8192(angle));
369		372
370	angle = (IntegralRoll/div_factor);	373	angle = (IntegralRoll/div_factor);
371	sin_roll = (int32_t)(c_sin_8192(angle));	374	sin_roll = (int32_t)(c_sin_8192(angle));
372	cos_roll = (int32_t)(c_cos_8192(angle));	375	cos_roll = (int32_t)(c_cos_8192(angle));
373		376
374	Hx_corr = Hx * cos_pitch;	377	Hx_corr = Hx * cos_pitch;
375	Hx_corr -= Hz * sin_pitch;	378	Hx_corr -= Hz * sin_pitch;
376	Hx_corr /= 8192;	379	Hx_corr /= 8192;
377		380
378	Hy_corr = Hy * cos_roll;	381	Hy_corr = Hy * cos_roll;
379	Hy_corr += Hz * sin_roll;	382	Hy_corr += Hz * sin_roll;
380	Hy_corr /= 8192;	383	Hy_corr /= 8192;
381		384
382	// calculate Heading	385	// calculate Heading
383	heading = c_atan2(Hy_corr, Hx_corr);	386	heading = c_atan2(Hy_corr, Hx_corr);
384		387
385	// atan returns angular range from -180 deg to 180 deg in counter clockwise notation	388	// atan returns angular range from -180 deg to 180 deg in counter clockwise notation
386	// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.	389	// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
387	if (heading < 0) heading = -heading;	390	if (heading < 0) heading = -heading;
388	else heading = 360 - heading;	391	else heading = 360 - heading;
389	}	392	}
390	else // MM3_Timeout = 0 i.e now new data from external board	393	else // MM3_Timeout = 0 i.e now new data from external board
391	{	394	{
392	if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem	395	if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem
393	heading = -1;	396	heading = -1;
394	}	397	}
395	return heading;	398	return heading;
396	}	399	}
397		400

Subversion Repositories FlightCtrl

(root)/branches/V0.68d Code Redesign killagreg/mm3.c @ 894 - Rev 781 → 782