/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 |