Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 291 → Rev 292

/trunk/compass.c
58,9 → 58,12
#include "compass.h"
#include "mk3mag.h"
#include "ncmag.h"
#include "spi_slave.h"
#include "mymath.h"
#include "uart1.h"
#include "fifo.h"
#include "led.h"
#include "main.h"
 
 
u8 CompassCalStateQueue[10];
108,30 → 111,48
}
 
 
void Compass_UpdateHeading(void)
void Compass_CalcHeading(void)
{
switch(Compass_Device)
if((UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX) || Compass_CalState)
{
case COMPASS_MK3MAG:
MK3MAG_UpdateCompass();
break;
case COMPASS_NCMAG:
NCMAG_UpdateCompass();
break;
default:
break;
Compass_Heading = -1;
}
else // fc attitude is avialable and no compass calibration active
{
// calculate attitude correction
// a float implementation takes too long
s16 tmp, Hx, Hy;
s32 sinnick, cosnick, sinroll, cosroll;
tmp = FromFlightCtrl.AngleNick/10; // in deg
sinnick = (s32)c_sin_8192(tmp);
cosnick = (s32)c_cos_8192(tmp);
tmp = FromFlightCtrl.AngleRoll/10; // in deg
sinroll = (s32)c_sin_8192(tmp);
cosroll = (s32)c_cos_8192(tmp);
// tbd. compensation signs and oriantation has to be fixed
Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L);
Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L);
// calculate heading
tmp = (s16)(c_tan2_546(Hy, Hx)/546L);
if (tmp > 0) tmp = 360 - tmp;
else tmp = -tmp;
Compass_Heading = tmp;
}
}
 
void Compass_UpdateMagVector(void)
void Compass_Update(void)
{
// check for new cal state
Compass_UpdateCalState();
// initiate new compass communication
switch(Compass_Device)
{
case COMPASS_MK3MAG:
MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT);
MK3MAG_Update();
break;
case COMPASS_NCMAG:
NCMAG_UpdateCompass();
NCMAG_Update();
default:
break;
}
/trunk/compass.h
18,8 → 18,8
extern u8 Compass_Device;
 
void Compass_Init(void);
void Compass_UpdateHeading(void);
void Compass_UpdateMagVector(void);
void Compass_Update(void);
void Compass_CalcHeading(void);
void Compass_SetCalState(u8 CalState);
void Compass_UpdateCalState(void);
 
/trunk/main.c
133,7 → 133,7
{
UART_VersionInfo.HardwareError[0] = 0;
 
if(/*(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) ||*/ CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.Status[1] |= 0x08;
if(CheckDelay(I2C1_Timeout) || (Compass_Heading < 0)) DebugOut.Status[1] |= 0x08;
else DebugOut.Status[1] &= ~0x08; // MK3Mag green status
 
if((FC.Error[1] & FC_ERROR1_I2C) || (FC.Error[1] & FC_ERROR1_BL_MISSING)) DebugOut.Status[1] |= 0x02;
230,14 → 230,6
sprintf(ErrorMSG,"Mixer Error");
ErrorCode = 19;
}
/* else if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
{
sprintf(ErrorMSG,"MK3Mag not compatible ");
LED_RED_ON;
ErrorCode = 2;
StopNavigation = 1;
UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
} */
else if(CheckDelay(UBX_Timeout))
{
LED_RED_ON;
293,7 → 285,7
 
VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq
VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit
Compass_UpdateHeading(); // update compass communication
Compass_Update(); // update compass communication
Analog_Update(); // get new ADC values
VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq
 
/trunk/menu.c
331,61 → 331,50
LCD_printfxy(0,3,"UP7:%3i UP8:%3i",Parameter.User7,Parameter.User8);
break;
case 15: // magnetic field
if(Compass_Device == COMPASS_NCMAG)
if(Compass_CalState)
{
if(Compass_CalState)
LCD_printfxy(0,0,"Calibration:");
LCD_printfxy(0,1,"Step %d/", Compass_CalState);
LCD_printfxy(0,2,"X %4i Y %4i Z %4i",MagVector.X,MagVector.Y,MagVector.Z);
LCD_printfxy(9,3,"(ESC)(NEXT)");
switch(Compass_CalState)
{
LCD_printfxy(0,0,"Calibration:");
LCD_printfxy(0,1,"Step %d/", Compass_CalState);
LCD_printfxy(0,2,"X %4i Y %4i Z %4i",MagVector.X,MagVector.Y,MagVector.Z);
LCD_printfxy(9,3,"(ESC)(NEXT)");
switch(Compass_CalState)
{
case 1:
case 3:
LCD_printfxy(7,1,"pause");
break;
case 2:
LCD_printfxy(7,1,"horizontal");
break;
case 4:
LCD_printfxy(7,1,"vertical");
break;
case 5:
LCD_printfxy(7,1,"data saved");
LCD_printfxy(8,3," (END) ");
break;
default:
break;
}
case 1:
case 3:
LCD_printfxy(7,1,"pause");
break;
 
case 2:
LCD_printfxy(7,1,"horizontal");
break;
 
case 4:
LCD_printfxy(7,1,"vertical");
break;
 
case 5:
LCD_printfxy(7,1,"data saved");
LCD_printfxy(8,3," (END) ");
break;
default:
break;
}
else
{
LCD_printfxy(0,0,"Magnetic Field");
LCD_printfxy(0,1,"X:%5i (%4i) ",MagVector.X, MagRawVector.X);
LCD_printfxy(0,2,"Y:%5i (%4i) ",MagVector.Y, MagRawVector.Y);
LCD_printfxy(0,3,"Z:%5i (%4i) ",MagVector.Z, MagRawVector.Z);
LCD_printfxy(15,3,"(CAL)");
}
if(Keys & KEY4) // next step
{
if(Compass_CalState <5) Compass_SetCalState(Compass_CalState+1);
else Compass_SetCalState(0);
}
if(Keys & KEY3)Compass_SetCalState(0); // cancel
}
else // other compass
else
{
Compass_UpdateMagVector();
LCD_printfxy(0,0,"Magnetic Field");
LCD_printfxy(0,1,"X: %4i",MagVector.X);
LCD_printfxy(0,2,"Y: %4i",MagVector.Y);
LCD_printfxy(0,3,"Z: %4i",MagVector.Z);
}
LCD_printfxy(0,1,"X:%5i",MagVector.X);
LCD_printfxy(0,2,"Y:%5i",MagVector.Y);
LCD_printfxy(0,3,"Z:%5i",MagVector.Z);
LCD_printfxy(15,3,"(CAL)");
}
if(Keys & KEY4) // next step
{
if(Compass_CalState <5) Compass_SetCalState(Compass_CalState+1);
else Compass_SetCalState(0);
}
if(Keys & KEY3)Compass_SetCalState(0); // cancel
break;
default:
//MaxMenuItem = MenuItem - 1;
/trunk/mk3mag.c
54,6 → 54,7
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <stdio.h>
#include <string.h>
#include "91x_lib.h"
#include "mk3mag.h"
60,7 → 61,6
#include "i2c.h"
#include "timer1.h"
#include "led.h"
#include "spi_slave.h"
#include "main.h"
#include "uart1.h"
#include "compass.h"
75,6 → 75,21
s16 Roll;
} __attribute__((packed)) MK3MAG_WriteAttitude_t;
 
typedef struct
{
u8 Major;
u8 Minor;
u8 Patch;
u8 Compatible;
} __attribute__((packed)) MK3MAG_Version_t;
 
typedef struct
{
u8 CalByte;
u8 Dummy1;
u8 Dummy2;
} __attribute__((packed)) MK3MAG_Cal_t;
 
// Transfer buffers
volatile MK3MAG_WriteAttitude_t MK3MAG_WriteAttitude;
volatile MK3MAG_Version_t MK3MAG_Version;
141,20 → 156,19
{ // if crc is ok and number of byte are matching
if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MagVector)+1)) )
{
memcpy((u8 *)&MagVector, pRxBuffer, sizeof(MagVector));
s16vec_t *pMagVector = (s16vec_t*)pRxBuffer;
MagVector.X = pMagVector->Y;
MagVector.Y = pMagVector->X;
MagVector.Z = -pMagVector->Z;
Compass_CalcHeading();
}
}
 
// rx data handler for heading request
void MK3MAG_UpdateHeading(u8* pRxBuffer, u8 RxBufferSize)
{ // if crc is ok and number of byte are matching
if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(Compass_Heading)+1)) )
{
memcpy((u8 *)&Compass_Heading, pRxBuffer, sizeof(Compass_Heading));
}
}
//----------------------------------------------------------------
#define MK3MAG_CMD_VERSION 0x01
#define MK3MAG_CMD_READ_MAGVECT 0x02
#define MK3MAG_CMD_WRITE_CAL 0x04
 
//----------------------------------------------------------------
void MK3MAG_SendCommand(u8 command)
{
// try to catch the I2C buffer
184,17 → 198,6
RxBytes = sizeof(MagVector)+1;
pRxHandlerFunc = &MK3MAG_UpdateMagVector;
break;
case MK3MAG_CMD_READ_HEADING:
RxBytes = sizeof(Compass_Heading)+1;
pRxHandlerFunc = &MK3MAG_UpdateHeading;
// update attitude from spi rx buffer
VIC_ITCmd(SSP0_ITLine, DISABLE); // avoid spi buffer update during copy
MK3MAG_WriteAttitude.Roll = FromFlightCtrl.AngleRoll;
MK3MAG_WriteAttitude.Nick = FromFlightCtrl.AngleNick;
VIC_ITCmd(SSP0_ITLine, ENABLE);
memcpy((u8*)I2C_Buffer+1, (u8*)&MK3MAG_WriteAttitude, sizeof(MK3MAG_WriteAttitude));
TxBytes += sizeof(MK3MAG_WriteAttitude);
break;
default: // unknown command id
RxBytes = 0;
pRxHandlerFunc = NULL;
258,43 → 261,50
}
 
//----------------------------------------------------------------
void MK3MAG_UpdateCompass(void)
void MK3MAG_Update(void)
{
static u32 TimerCompassUpdate = 0;
static s32 x_max,y_max,z_max,x_min,y_min,z_min;
static u32 TimerUpdate = 0;
static s16 x_max,y_max,z_max,x_min,y_min,z_min;
 
if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return;
if(CheckDelay(TimerCompassUpdate))
if(CheckDelay(TimerUpdate))
{
// check for incomming compass calibration request
Compass_UpdateCalState();
MK3MAG_WriteCal.CalByte = Compass_CalState;
// send new calstate
if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
{
{ // send new calibration state
MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL);
x_max = -30000; y_max = -30000; z_max = -30000;
x_min = 30000; y_min = 30000; z_min = 30000;
}
if(MK3MAG_WriteCal.CalByte == 2)
{
MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT);
if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 30; };
if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 30; };
if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 30; };
if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 30; };
else
{ // calibration state matches
MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); // initiate magvector transfer
switch(Compass_CalState)
{
case 1:
x_max = -30000; y_max = -30000; z_max = -30000;
x_min = 30000; y_min = 30000; z_min = 30000;
break;
case 2:
if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 60; }
else if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 20; }
if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 60; }
else if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 20; }
break;
case 4:
if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 80; }
else if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 80; }
break;
default:
break;
}
}
if(MK3MAG_WriteCal.CalByte == 4)
{
MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT);
if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 30; };
if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 30; };
}
else // request current heading
{
MK3MAG_SendCommand(MK3MAG_CMD_READ_HEADING);
}
TimerCompassUpdate = SetDelay(20); // every 20 ms are 50 Hz
TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz
}
}
/trunk/mk3mag.h
1,30 → 1,7
#ifndef __MK3MAG_H
#define __MK3MAG_H
 
typedef struct
{
u8 Major;
u8 Minor;
u8 Patch;
u8 Compatible;
} __attribute__((packed)) MK3MAG_Version_t;
 
typedef struct
{
u8 CalByte;
u8 Dummy1;
u8 Dummy2;
} __attribute__((packed)) MK3MAG_Cal_t;
 
extern volatile MK3MAG_Version_t MK3MAG_Version;
 
#define MK3MAG_CMD_VERSION 0x01
#define MK3MAG_CMD_READ_MAGVECT 0x02
#define MK3MAG_CMD_READ_HEADING 0x03
#define MK3MAG_CMD_WRITE_CAL 0x04
 
u8 MK3MAG_Init(void);
void MK3MAG_SendCommand(u8 command);
void MK3MAG_UpdateCompass(void);
void MK3MAG_Update(void);
 
#endif // __MK3MAG_H
/trunk/ncmag.c
55,17 → 55,17
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "91x_lib.h"
#include "main.h"
#include "ncmag.h"
#include "i2c.h"
#include "timer1.h"
#include "led.h"
#include "spi_slave.h"
#include "uart1.h"
#include "eeprom.h"
#include "mymath.h"
#include "main.h"
 
u8 NCMAG_Present = 0;
u8 NCMAG_IsCalibrated = 0;
325,7 → 325,7
// Save values
if(Compass_CalState != OldCalState) // avoid continously writing of eeprom!
{
#define MIN_CALIBRATION 256
#define MIN_CALIBRATION 256
Calibration.MagX.Range = Xmax - Xmin;
Calibration.MagX.Offset = (Xmin + Xmax) / 2;
Calibration.MagY.Range = Ymax - Ymin;
395,33 → 395,7
MagVector.X = (s16)((1024L*(s32)(MagRawVector.X - Calibration.MagX.Offset))/Calibration.MagX.Range);
MagVector.Y = (s16)((1024L*(s32)(MagRawVector.Y - Calibration.MagY.Offset))/Calibration.MagY.Range);
MagVector.Z = (s16)((1024L*(s32)(MagRawVector.Z - Calibration.MagZ.Offset))/Calibration.MagZ.Range);
if(UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX)
{
Compass_Heading = -1;
}
else // fc attitude is avialable
{
// calculate attitude correction
// a float implementation takes too long for an ISR call!
s16 tmp, Hx, Hy;
s32 sinnick, cosnick, sinroll, cosroll;
tmp = FromFlightCtrl.AngleNick/10; // in deg
sinnick = (s32)c_sin_8192(tmp);
cosnick = (s32)c_cos_8192(tmp);
tmp = FromFlightCtrl.AngleRoll/10; // in deg
sinroll = (s32)c_sin_8192(tmp);
cosroll = (s32)c_cos_8192(tmp);
// tbd. compensation signs and oriantation has to be fixed
Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L);
Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L);
// calculate heading
tmp = (s16)(c_tan2_546(Hy, Hx)/546L);
if (tmp > 0) tmp = 360 - tmp;
else tmp = -tmp;
Compass_Heading = tmp;
}
Compass_CalcHeading();
}
}
// rx data handler for acceleration raw data
588,9 → 562,9
}
 
// --------------------------------------------------------
void NCMAG_UpdateCompass(void)
void NCMAG_Update(void)
{
static u32 TimerCompassUpdate = 0;
static u32 TimerUpdate = 0;
 
if( (I2C_State == I2C_STATE_OFF) || !NCMAG_Present )
{
598,13 → 572,13
return;
}
 
if(CheckDelay(TimerCompassUpdate))
if(CheckDelay(TimerUpdate))
{
// check for new calibration state
Compass_UpdateCalState();
if(Compass_CalState) NCMAG_Calibrate();
NCMAG_GetMagVector(); //Get new data;
TimerCompassUpdate = SetDelay(20); // every 20 ms are 50 Hz
TimerUpdate = SetDelay(20); // every 20 ms are 50 Hz
}
}
 
/trunk/ncmag.h
4,10 → 4,9
#include "compass.h"
 
extern s16vec_t ACC_Vector;
extern volatile s16vec_t MagRawVector;
 
u8 NCMAG_Init(void);
void NCMAG_UpdateCompass(void);
void NCMAG_Update(void);
 
#endif // __NCMAG_H