Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 6 → Rev 7

/trunk/main.c
1,8 → 1,9
/*#######################################################################################
Flight Control
MK3Mag 3D-Magnet sensor
#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Copyright (c) 05.2008 Holger Buss
// + Thanks to Ilja Fähnrich
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
37,8 → 38,9
// + for non-commercial use (directly or indirectly)
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// + with our written permission
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// + clearly linked as origin // + * porting to systems other than hardware from www.mikrokopter.de is not allowed
// + * If sources or documentations are redistributet on other webpages, our webpage (http://www.MikroKopter.de) must be
// + clearly linked as origin
// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
51,27 → 53,27
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
signed int OffsetN, OffsetR, OffsetZ;
/*
signed int CalTabelleHorizontalN[4] = {67,-450,77,-411}; // Vier Messwerte 90° Horizontal gedreht
signed int CalTabelleHorizontalR[4] = {-310,175,183,-333};
signed int CalTabelleHorizontalZ[4] = {-100,-390,-400,100}; // Messwinkel des Z-Sensors auch horizontal gemessen
*/
signed int CalTabelleHorizontalN[4] = { 75,-180,-440,-166}; // Vier Messwerte 90° Horizontal gedreht
signed int CalTabelleHorizontalR[4] = { -5,-230, 66, 246};
signed int CalTabelleHorizontalZ[4] = {-290,-280, 44, 33}; // Messwinkel des Z-Sensors auch horizontal gemessen
signed int OffsetN, OffsetR, OffsetZ;
 
signed int RawMagnet1a,RawMagnet1b; // reine AD-Messung
signed int RawMagnet1a,RawMagnet1b; // raw AD-Data
signed int RawMagnet2a,RawMagnet2b;
signed int RawMagnet3a,RawMagnet3b;
signed int Xmin = 0, Xmax = 0; Ymin = 0, Ymax = 0; Zmin = 0, Zmax = 0;
signed int UncalMagnetN,UncalMagnetR,UncalMagnetZ; // Messwert-Delta ohne Offset- und Verstärker korrektur
signed int MagnetN,MagnetR,MagnetZ; // Kalibrierte Messerte
signed int MagnetN,MagnetR,MagnetZ;
unsigned int PwmHeading = 0;
unsigned char PC_Connected = 0;
int Heading;
unsigned int PC_Connected = 0;
unsigned int Heading;
#include "main.h"
 
uint16_t eeXmin EEMEM = 0;
uint16_t eeXmax EEMEM = 0;
uint16_t eeYmin EEMEM = 0;
uint16_t eeYmax EEMEM = 0;
uint16_t eeZmin EEMEM = 0;
uint16_t eeZmax EEMEM = 0;
 
 
//############################################################################
//
void Wait(unsigned char dauer)
82,35 → 84,39
}
 
void CalcFields(void)
{
UncalMagnetN = (2 * UncalMagnetN + (RawMagnet1a - RawMagnet1b)) / 3;
UncalMagnetR = (2 * UncalMagnetR + (RawMagnet3a - RawMagnet3b)) / 3;
UncalMagnetZ = (2 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 3;
MagnetN = UncalMagnetN - OffsetN;
MagnetR = UncalMagnetR - OffsetR;
MagnetZ = UncalMagnetZ - OffsetZ;
{
UncalMagnetN = (1 * UncalMagnetN + (RawMagnet1a - RawMagnet1b)) / 2;
UncalMagnetR = (1 * UncalMagnetR + (RawMagnet3a - RawMagnet3b)) / 2;
UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2;
 
OffsetN = (Xmin + Xmax) / 2;
OffsetR = (Ymin + Ymax) / 2;
OffsetZ = (Zmin + Zmax) / 2;
 
MagnetN = (1024L * (long)(UncalMagnetN - OffsetN)) / (Xmax - Xmin);
MagnetR = (1024L * (long)(UncalMagnetR - OffsetR)) / (Ymax - Ymin);
MagnetZ = (1024L * (long)(UncalMagnetZ - OffsetZ)) / (Zmax - Zmin);
}
 
//------------------------------------------------------
void CalcHeading(void)
{
double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
float nick, roll, XEx, XEy, YEy, YEz, YE, XE, XrawCal, YrawCal, ZrawCal, XEz;
int heading, azimuthgrad;
int heading;
nick_rad = ((double)WinkelOut.Winkel[0]) * M_PI / (double)(180);
roll_rad = ((double)WinkelOut.Winkel[1]) * M_PI / (double)(180);
//roll_rad = 0; nick_rad = 0;
nick_rad = ((double)ExternData.Winkel[0]) * M_PI / (double)(1800);
roll_rad = ((double)ExternData.Winkel[1]) * M_PI / (double)(1800);
 
/*
Cx = (double) (MicroMag.Axis[Y_AXIS] + CY_OFFSET) * 0.707f - (double) (MicroMag.Axis[X_AXIS]+ CX_OFFSET) * 0.707f;
Cy = ((double) (MicroMag.Axis[Y_AXIS]+ CY_OFFSET) * 0.707f + (double) (MicroMag.Axis[X_AXIS]+ CX_OFFSET) * 0.707f);
Cz = -MicroMag.Axis[Z_AXIS] + CZ_OFFSET;
*/
Cx = MagnetN;
Cy = MagnetR;
Cz = -MagnetZ;
Cz = MagnetZ;
 
if(ExternData.Orientation == 1)
{
Cx = MagnetR;
Cy = -MagnetN;
Cz = MagnetZ;
}
 
Hx = Cx * (double)cos(nick_rad) +
Cy * (double)sin(nick_rad) * (double)sin(roll_rad) -
Cz * (double)sin(nick_rad) * (double)cos(roll_rad);
121,22 → 127,90
if(Hx == 0 && Hy < 0) heading = 90;
else if(Hx == 0 && Hy > 0) heading = 270;
else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180 / M_PI);
else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180 / M_PI);
else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180 / M_PI);
else if(Hx < 0) heading = 180 - (atan(Hy / Hx) * 180.0) / M_PI;
else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI;
else if(Hx > 0 && Hy > 0) heading = 360 - (atan(Hy / Hx) * 180.0) / M_PI;
 
// DebugOut.Analog[14] = heading;
// if (FromFlightCtrl.IntegralNick > 0) heading = heading + FromFlightCtrl.IntegralNick/60;
if(heading < 361) DebugOut.Analog[14] = heading;
Heading = heading;
PwmHeading = heading + 10;
// MicroMag.Heading = heading;
// DebugOut.Analog[14] = heading;
if(abs(heading) < 361) Heading = heading;
PwmHeading = Heading + 10;
}
 
 
void Calibrate(void)
{
unsigned char cal;
if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
else cal = ExternData.CalState;
switch(cal)
{
case 0:
LED_ON;
break;
case 1:
Xmin = 10000;
Xmax = -10000;
Ymin = 10000;
Ymax = -10000;
Zmin = 10000;
Zmax = -10000;
LED_OFF;
break;
case 2:
LED_ON; // find Min and Max of the X- and Y-Sensors
if(UncalMagnetN < Xmin) Xmin = UncalMagnetN;
if(UncalMagnetN > Xmax) Xmax = UncalMagnetN;
if(UncalMagnetR < Ymin) Ymin = UncalMagnetR;
if(UncalMagnetR > Ymax) Ymax = UncalMagnetR;
break;
case 3:
LED_OFF;
break;
case 4:
LED_ON; // find Min and Max of the Z-Sensor
if(UncalMagnetZ < Zmin) Zmin = UncalMagnetZ;
if(UncalMagnetZ > Zmax) Zmax = UncalMagnetZ;
break;
case 5:
LED_OFF; // Save values
if((Xmax - Xmin) > 150 && (Ymax - Ymin) > 150 && (Zmax - Zmin) > 150)
{
eeprom_write_word(&eeXmin, Xmin);
eeprom_write_word(&eeXmax, Xmax);
eeprom_write_word(&eeYmin, Ymin);
eeprom_write_word(&eeYmax, Ymax);
eeprom_write_word(&eeZmin, Zmin);
eeprom_write_word(&eeZmax, Zmax);
Delay_ms(2000);
}
LED_ON;
break;
}
}
 
 
void SetDebugValues(void)
{
DebugOut.Analog[0] = MagnetN;
DebugOut.Analog[1] = MagnetR;
DebugOut.Analog[2] = MagnetZ;
DebugOut.Analog[3] = UncalMagnetN;
DebugOut.Analog[4] = UncalMagnetR;
DebugOut.Analog[5] = UncalMagnetZ;
DebugOut.Analog[6] = ExternData.Winkel[0];
DebugOut.Analog[7] = ExternData.Winkel[1];
DebugOut.Analog[8] = Xmin;
DebugOut.Analog[9] = Xmax;
DebugOut.Analog[10] = Ymin;
DebugOut.Analog[11] = Ymax;
DebugOut.Analog[12] = Zmin;
DebugOut.Analog[13] = Zmax;
DebugOut.Analog[14] = ExternData.CalState;
DebugOut.Analog[15] = Heading;
DebugOut.Analog[16] = ExternData.UserParameter[0];
DebugOut.Analog[17] = ExternData.UserParameter[1];
}
 
 
//############################################################################
//Hauptprogramm
int main (void)
158,67 → 232,54
sei();//Globale Interrupts Einschalten
Debug_Timer = SetDelay(100); // Sendeintervall
Xmin = eeprom_read_word(&eeXmin);
Xmax = eeprom_read_word(&eeXmax);
Ymin = eeprom_read_word(&eeYmin);
Ymax = eeprom_read_word(&eeYmax);
Zmin = eeprom_read_word(&eeZmin);
Zmax = eeprom_read_word(&eeZmax);
 
OffsetN = (CalTabelleHorizontalN[0] + CalTabelleHorizontalN[1] + CalTabelleHorizontalN[2] + CalTabelleHorizontalN[3]) / 4;
OffsetR = (CalTabelleHorizontalR[0] + CalTabelleHorizontalR[1] + CalTabelleHorizontalR[2] + CalTabelleHorizontalR[3]) / 4;
OffsetZ = (CalTabelleHorizontalZ[0] + CalTabelleHorizontalZ[1] + CalTabelleHorizontalZ[2] + CalTabelleHorizontalZ[3]) / 4;
 
VersionInfo.Hauptversion = VERSION_HAUPTVERSION;
VersionInfo.Nebenversion = VERSION_NEBENVERSION;
VersionInfo.PCKompatibel = 7;
ExternData.Orientation = 0;
ExternData.CalState = 0;
I2C_WriteCal.CalByte = 0;
while (1)
{
LED_ON;
FLIP_LOW;
Delay_ms(2);
RawMagnet1a = MessAD(0);
RawMagnet2a = MessAD(1);
RawMagnet2a = -MessAD(1);
RawMagnet3a = MessAD(7);
Delay_ms(1);
LED_OFF;
 
FLIP_HIGH;
Delay_ms(2);
RawMagnet1b = MessAD(0);
RawMagnet2b = MessAD(1);
RawMagnet2b = -MessAD(1);
RawMagnet3b = MessAD(7);
Delay_ms(1);
 
CalcFields();
DebugOut.Analog[0] = MagnetN;
DebugOut.Analog[1] = MagnetR;
DebugOut.Analog[2] = MagnetZ;
DebugOut.Analog[3] = UncalMagnetN;
DebugOut.Analog[4] = UncalMagnetR;
DebugOut.Analog[5] = UncalMagnetZ;
/*
DebugOut.Analog[3] = RawMagnet1a;
DebugOut.Analog[4] = RawMagnet1b;
DebugOut.Analog[5] = RawMagnet3a;
DebugOut.Analog[6] = RawMagnet3b;*/
DebugOut.Analog[6] = WinkelOut.Winkel[0];
DebugOut.Analog[7] = WinkelOut.Winkel[1];
if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
else CalcHeading();
BearbeiteRxDaten();
 
DebugOut.Analog[8] = WinkelOut.UserParameter[0];
DebugOut.Analog[9] = WinkelOut.UserParameter[1];
DebugOut.Analog[10] = WinkelOut.UserParameter[2];
DebugOut.Analog[11] = WinkelOut.UserParameter[3];
 
 
CalcHeading();
BearbeiteRxDaten();
PC_Connected = 100;
if(PC_Connected)
{
DDRD |= 0x02; // TXD-Portpin
UCR |= (1 << TXEN);
DatenUebertragung();
PC_Connected--;
DatenUebertragung();
DDRD |= 0x02; // TXD-Leitung
}
else
{
DDRD &= ~0x02; // TXD-Leitung
UCR &= ~(1 << TXEN);
DDRD &= ~0x02; // TXD-Portpin
}
} // while(1) - Hauptschleife
} // while(1)
}
 
/trunk/main.h
12,6 → 12,7
#include <avr/eeprom.h>
#include <avr/boot.h>
#include <avr/wdt.h>
#include <avr/eeprom.h>
 
#include "twislave.h"
#include "old_macros.h"
56,9 → 57,17
#define FLIP_LOW PORTD &= ~0x60
 
extern unsigned int PwmHeading;
extern unsigned char PC_Connected;
extern int Heading;
extern unsigned int PC_Connected;
extern unsigned int Heading;
extern signed int MagnetN,MagnetR,MagnetZ;
 
extern uint16_t eeXmin EEMEM;
extern uint16_t eeXmax EEMEM;
extern uint16_t eeYmin EEMEM;
extern uint16_t eeYmax EEMEM;
extern uint16_t eeZmin EEMEM;
extern uint16_t eeZmax EEMEM;
 
#endif //_MAIN_H
 
 
/trunk/makefile
3,7 → 3,7
MCU = atmega168
#-------------------------------------------------------------------
HAUPT_VERSION = 0
NEBEN_VERSION = 5
NEBEN_VERSION = 14
#-------------------------------------------------------------------
 
# Output format. (can be srec, ihex, binary)
/trunk/timer0.c
1,6 → 1,13
/*****************************************************************************
*****************************************************************************/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + MK3Mag 3D-Compass
// + ATMEGA168 mit 8MHz
// + (c) 05.2008 Holger Buss
// + Nur für den privaten Gebrauch
// + Keine Garantie auf Fehlerfreiheit
// + Kommerzielle Nutzung nur mit meiner Zustimmung
// + Der Code ist für die Hardware MK3Mag entwickelt worden
// + www.mikrokopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
volatile unsigned int CountMilliseconds = 0;
volatile unsigned char Timer0Overflow;
24,7 → 31,7
{
static unsigned char cnt;
static unsigned int cmps_cnt;
TCNT0 -= 97; // reload
TCNT0 -= 101; // reload
Timer0Overflow++;
 
if(++cmps_cnt == 380)
/trunk/twislave.c
14,6 → 14,7
struct str_I2C_Mag I2C_Mag;
struct str_I2C_EEPROM I2C_ReadEEPROM, I2C_WriteEEPROM;
struct str_I2C_Version I2C_Version;
struct str_I2C_WriteCal I2C_WriteCal;
 
 
 
64,6 → 65,13
I2C_RxBuffer = (unsigned char *)&I2C_WriteEEPROM;
I2C_RxBufferSize = sizeof(I2C_WriteEEPROM);
break;
 
case I2C_CMD_WRITE_CAL:
I2C_TxBufferSize = 0;
I2C_RxBuffer = (unsigned char *)&I2C_WriteCal;
I2C_RxBufferSize = sizeof(I2C_WriteCal);
break;
 
case I2C_CMD_READ_EEPROM:
I2C_TxBuffer = (unsigned char *)&I2C_ReadEEPROM.Inhalt;
I2C_TxBufferSize = 2;
87,8 → 95,8
I2C_RxBufferSize = sizeof(I2C_WriteNickRoll);
I2C_Heading.Heading = Heading;
WinkelOut.Winkel[NICK] = I2C_WriteNickRoll.Nick;
WinkelOut.Winkel[ROLL] = I2C_WriteNickRoll.Roll;
ExternData.Winkel[NICK] = I2C_WriteNickRoll.Nick;
ExternData.Winkel[ROLL] = I2C_WriteNickRoll.Roll;
break;
}
}
/trunk/twislave.h
34,6 → 34,14
int Roll;
} ;
 
#define I2C_CMD_WRITE_CAL 0x04
struct str_I2C_WriteCal
{
unsigned char CalByte;
unsigned char Dummy1;
unsigned char Dummy2;
} ;
 
struct str_I2C_Heading
{
unsigned int Heading;
47,6 → 55,7
extern struct str_I2C_Mag I2C_Mag;
extern struct str_I2C_EEPROM I2C_ReadEEPROM, I2C_WriteEEPROM;
extern struct str_I2C_Version I2C_Version;
extern struct str_I2C_WriteCal I2C_WriteCal;
 
 
extern void InitIC2_Slave (void);
/trunk/uart.c
1,12 → 1,12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Regler für Brushless-Motoren
// + ATMEGA8 mit 8MHz
// + (c) 01.2007 Holger Buss
// + MK3Mag 3D-Compass
// + ATMEGA168 mit 8MHz
// + (c) 05.2008 Holger Buss
// + Nur für den privaten Gebrauch
// + Keine Garantie auf Fehlerfreiheit
// + Kommerzielle Nutzung nur mit meiner Zustimmung
// + Der Code ist für die Hardware BL_Ctrl V1.0 entwickelt worden
// + www.mikrocontroller.com
// + Der Code ist für die Hardware MK3Mag entwickelt worden
// + www.mikrokopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include "main.h"
28,7 → 28,7
unsigned char PcZugriff;
 
struct str_DebugOut DebugOut;
struct str_WinkelOut WinkelOut;
struct str_ExternData ExternData;
struct str_ExternControl ExternControl;
struct str_VersionInfo VersionInfo;
 
38,42 → 38,43
 
const unsigned char ANALOG_TEXT[32][16] =
{
//1234567890123456
//1234567890123456
"Magnet N ", //0
"Magnet R ",
"Magnet Z ",
"Roh N ",
"Roh R ",
"Roh Z ", //5
"Magnet Z ",
"Raw N ",
"Raw R ",
"Raw Z ", //5
"Lage N ",
"Lage R ",
"Xmin ",
"Xmax ",
"Ymin ", //10
"Ymax ",
"Zmin ",
"ZMax ",
"Calstate ",
"Kompass ", //15
"User0 ",
"User1 ",
"User2 ", //10
"User3 ",
" ",
" ",
"Kompass ",
" ", //15
"Acc_Z ",
"MittelAccNick ",
"MittelAccRoll ",
"IntegralErrNick ",
"IntegralErrRoll ", //20
"MittelIntNick ",
"MittelIntRoll ",
"NeutralNick ",
"RollOffset ",
"IntRoll*Faktor ", //25
"Analog18 ",
"Analog19 ",
"Analog20 ", //20
"Analog21 ",
"Analog22 ",
"Analog23 ",
"Analog24 ",
"Analog25 ", //25
"Analog26 ",
"DirektAusglRoll ",
"MesswertRoll ",
"AusgleichRoll ",
"I-LageRoll ", //30
"StickRoll "
"Analog27 ",
"Analog28 ",
"Analog29 ",
"Analog30 ", //30
"Analog31 "
};
 
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ Sende-Part der Datenübertragung
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
248,6 → 249,7
int uart_putchar (char c)
//############################################################################
{
if(!(UCR & (1 << TXEN))) return (0);
if (c == '\n')
uart_putchar('\r');
//Warten solange bis Zeichen gesendet wurde
307,16 → 309,15
switch(RxdBuffer[2])
{
case 'w':// Lagewinkel
Decode64((unsigned char *) &WinkelOut,sizeof(WinkelOut),3,AnzahlEmpfangsBytes);
Decode64((unsigned char *) &ExternData,sizeof(ExternData),3,AnzahlEmpfangsBytes);
DebugOut.Analog[15]++;
KompassAntwort = 1;
LED_ON;
break;
case 'c':
case 'b':
Decode64((unsigned char *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes);
WinkelOut.Winkel[0] = ExternControl.Par1;
WinkelOut.Winkel[1] = ExternControl.Par2;
ExternData.Winkel[0] = ExternControl.Par1;
ExternData.Winkel[1] = ExternControl.Par2;
break;
case 'v': // Version-Anforderung und Ausbaustufe
GetVersionAnforderung = 1;
400,6 → 401,7
{
if((CheckDelay(Debug_Timer) && UebertragungAbgeschlossen)) // im Singlestep-Betrieb in jedem Schtitt senden
{
SetDebugValues();
SendOutData('D',MeineSlaveAdresse,(unsigned char *) &DebugOut,sizeof(DebugOut));
Debug_Timer = SetDelay(250); // Sendeintervall
}
/trunk/uart.h
16,15 → 16,17
struct str_DebugOut
{
unsigned char Digital[2];
unsigned int Analog[16]; // Debugwerte
unsigned int Analog[32]; // Debugwerte
};
 
struct str_WinkelOut
struct str_ExternData
{
signed int Winkel[2];
unsigned char UserParameter[4];
signed int Winkel[2]; // in 0,1 degree
unsigned char UserParameter[2];
unsigned char CalState;
unsigned char Orientation;
};
extern struct str_WinkelOut WinkelOut;
extern struct str_ExternData ExternData;
#define NICK 0
#define ROLL 1