Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 740 → Rev 741

/branches/V0.68d Code Redesign killagreg/mm3.c
1,8 → 1,8
/*
 
Copyright 2007, Niklas Nold
Copyright 2008, by Killagreg
 
This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify
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;
10,7 → 10,8
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
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>
61,6 → 62,7
 
MM3_calib_t MM3_calib;
volatile MM3_working_t MM3;
static volatile uint8_t MM3_Timeout = 0;
 
 
 
107,6 → 109,8
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;
}
 
114,12 → 118,12
/*********************************************/
/* Get Data from MM3 */
/*********************************************/
void MM3_Update() // called every 102.4 ms by timer 0 ISR
void MM3_Update() // called every 102.4 µs by timer 0 ISR
{
switch (MM3.STATE)
{
case MM3_STATE_RESET:
PORTC &= ~(1<<PORTC4); // select slave
PORTC &= ~(1<<PORTC4); // select slave
PORTC |= (1<<PORTC5); // PC5 to High, MM3 Reset
MM3.STATE = MM3_STATE_START_TRANSFER;
return;
129,9 → 133,22
 
// 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
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
196,6 → 213,25
}
PORTC |= (1<<PORTC4); // deselect slave
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;
}
}
 
214,7 → 250,7
ROT_OFF;
 
// get maximum and minimum reading of all axis
while (measurement)
while (measurement && !MM3_Timeout)
{
if (MM3.x_axis > x_max) x_max = MM3.x_axis;
else if (MM3.x_axis < x_min) x_min = MM3.x_axis;
233,7 → 269,6
beeper = 50;
}
beeper--;
 
// loop with period of 10 ms / 100 Hz
timer = SetDelay(10);
while(!CheckDelay(timer));
241,24 → 276,26
// If thrust is less than 100, stop calibration with a delay of 0.5 seconds
if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--;
}
if(!MM3_Timeout)
{
// 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);
 
// 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;
 
// 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);
// 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);
}
}
 
 
273,64 → 310,70
uint16_t div_factor;
int16_t heading;
 
// calibration factor for transforming Gyro Integrals to angular degrees
div_factor = (uint16_t)ParamSet.UserParam3 * 8;
if (MM3_Timeout)
{
// 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;
// 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));
// 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;
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;
// rotate
Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192;
Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192;
 
 
// tilt compensation
// tilt compensation
 
// calibration factor for transforming Gyro Integrals to angular degrees
div_factor = (uint16_t)ParamSet.UserParam3 * 8;
// 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));
// 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));
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;
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;
Hy_corr = Hy * cos_roll;
Hy_corr += Hz * sin_roll;
Hy_corr /= 8192;
 
// calculate Heading
heading = c_atan2(Hy_corr, Hx_corr);
// 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;
 
// 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
{
heading = -1;
}
return heading;
}