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

Subversion Repositories FlightCtrl

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