Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 68 → Rev 67

/trunk/main.c
106,10 → 106,8
int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0; // nick and roll angle from acc
 
int16_t Heading = -1; // the current compass heading in deg
int16_t ZeroHeading = 180;
unsigned char InternalCalstate = 0, ActualCalstate = 0;
signed char PotiToFC[8] = {10,20,30,40,50,60,70,80};
 
 
void CalcFields(void)
{
UncalMagX = (RawMagnet1a - RawMagnet1b);
121,7 → 119,7
else MagY = 0;
if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range);
else MagZ = 0;
if(AttitudeSource == ATTITUDE_SOURCE_ACC)
if(AccPresent)
{
AccX = (RawAccX - Calibration.AccX.Offset);
AccY = (RawAccY - Calibration.AccY.Offset);
240,22 → 238,28
 
void Calibrate(void)
{
uint8_t cal;
static uint8_t calold = 0;
static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0;
static uint8_t blinkcount = 0;
static uint8_t invert_blinking = 0;
 
if(ActualCalstate > 5) ActualCalstate = 0;
// check both sources of communication for calibration request
if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
else cal = ExternData.CalState;
 
 
if(cal > 5) cal = 0;
// blink code for current calibration state
if(ActualCalstate)
if(cal)
{
if(CheckDelay(Led_Timer) || (ActualCalstate != calold))
if(CheckDelay(Led_Timer) || (cal != calold))
{
if(blinkcount & 0x01) if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
else if(invert_blinking) LED_GRN_OFF; else LED_GRN_ON;
 
// end of blinkcount sequence
if((blinkcount + 1 ) >= (2 * ActualCalstate))
if((blinkcount + 1 ) >= (2 * cal))
{
blinkcount = 0;
Led_Timer = SetDelay(1500);
272,7 → 276,7
if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
}
// calibration state machine
switch(ActualCalstate)
switch(cal)
{
case 1: // 1st step of calibration
// initialize ranges
312,7 → 316,7
 
case 5:
// Save values
if(ActualCalstate != calold) // avoid continously writing of eeprom!
if(cal != calold) // avoid continously writing of eeprom!
{
Calibration.MagX.Range = Xmax - Xmin;
Calibration.MagX.Offset = (Xmin + Xmax) / 2;
342,7 → 346,7
default:
break;
}
calold = ActualCalstate;
calold = cal;
}
 
 
365,14 → 369,12
DebugOut.Analog[6] = ExternData.Attitude[NICK];
DebugOut.Analog[7] = ExternData.Attitude[ROLL];
break;
 
 
case ATTITUDE_SOURCE_I2C:
DebugOut.Analog[6] = I2C_WriteAttitude.Nick;
DebugOut.Analog[7] = I2C_WriteAttitude.Roll;
break;
default:
DebugOut.Analog[6] = -1;
DebugOut.Analog[7] = -1;
break;
}
DebugOut.Analog[8] = Calibration.MagX.Offset;
DebugOut.Analog[9] = Calibration.MagX.Range;
380,10 → 382,11
DebugOut.Analog[11] = Calibration.MagY.Range;
DebugOut.Analog[12] = Calibration.MagZ.Offset;
DebugOut.Analog[13] = Calibration.MagZ.Range;
DebugOut.Analog[14] = ActualCalstate;
if(I2C_WriteCal.CalByte) DebugOut.Analog[14] = I2C_WriteCal.CalByte;
else DebugOut.Analog[14] = ExternData.CalState;
DebugOut.Analog[15] = Heading;
DebugOut.Analog[16] = 0;
DebugOut.Analog[17] = 0;
DebugOut.Analog[16] = ExternData.UserParam[0];
DebugOut.Analog[17] = ExternData.UserParam[1];
DebugOut.Analog[18] = AccX;
DebugOut.Analog[19] = AccY;
DebugOut.Analog[20] = AccZ;
390,9 → 393,9
DebugOut.Analog[21] = RawAccX;
DebugOut.Analog[22] = RawAccY;
DebugOut.Analog[23] = RawAccZ;
DebugOut.Analog[24] = PotiToFC[0];
DebugOut.Analog[25] = PotiToFC[1];
DebugOut.Analog[26] = PotiToFC[2];
DebugOut.Analog[24] = Calibration.AccX.Offset;
DebugOut.Analog[25] = Calibration.AccY.Offset;
DebugOut.Analog[26] = Calibration.AccZ.Offset;
DebugOut.Analog[29] = AttitudeSource;
}
 
412,28 → 415,6
}
}
 
#ifdef HEADTRACKER
void CalcFcPotis(void)
{
signed int tmp0, tmp1, tmp2;
if(AccPresent)
{
tmp0 = AccAttitudeNick/4;
tmp1 = AccAttitudeRoll/4;
 
tmp2 = (360 + Heading - ZeroHeading) % 360;
tmp2 /= 2;
tmp2 -= 90;
if(tmp0 > 127) tmp0 = 127; else if(tmp0 < -127) tmp0 = -127;
if(tmp1 > 127) tmp1 = 127; else if(tmp1 < -127) tmp1 = -127;
if(tmp2 > 127) tmp2 = 127; else if(tmp2 < -127) tmp2 = -127;
PotiToFC[0] = tmp0;
PotiToFC[1] = tmp1;
PotiToFC[2] = tmp2;
}
}
#endif
 
int main (void)
{
static uint8_t state = 0;
450,17 → 431,12
 
sei(); // enable globale interrupts
USART0_Print("\n\rthe use of this software is only permitted\n\ron original MikroKopter-Hardware");
USART0_Print("\n\rwww.MikroKopter.de (c) HiSystems GmbH\n\n");
#ifdef HEADTRACKER
USART0_Print("Head-Tracker\n");
#endif
USART0_Print("\n\rwww.MikroKopter.de (c) HiSystems GmbH");
 
if(AccPresent)
{
USART0_Print("ACC present\n");
USART0_Print("\n\nACC present\n");
}
#ifdef HEADTRACKER
else USART0_Print("\nERROR: NO ACC\n");
#endif
 
LED_GRN_ON;
 
469,11 → 445,11
// read calibration info from eeprom
eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
 
InternalCalstate = 0;
ExternData.CalState = 0;
I2C_WriteCal.CalByte = 0;
ActualCalstate = 0;
 
statetimer = SetDelay(0);
 
// main loop
while (1)
{
519,29 → 495,19
AccMeasurement();
statetimer = SetDelay(1);
state = 0;
 
CalcFields();
// check both sources of communication for calibration request
if(InternalCalstate) ActualCalstate = InternalCalstate;
else
if(I2C_WriteCal.CalByte) ActualCalstate = I2C_WriteCal.CalByte;
else ActualCalstate = ExternData.CalState;
if(ActualCalstate) Calibrate();
if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
else CalcHeading();
if(NC_Connected) NC_Connected--;
if(FC_Connected) FC_Connected--;
if(PC_Connected) PC_Connected--;
// fall back to attitude estimation from onboard acc sensor if NC or FC does'nt send attitude data
#ifdef HEADTRACKER
CalcFcPotis();
if(AccPresent) AttitudeSource = ATTITUDE_SOURCE_ACC;
#else
if(!FC_Connected && ! NC_Connected)
{
if(AccPresent) AttitudeSource = ATTITUDE_SOURCE_ACC;
else AttitudeSource = ATTITUDE_SOURCE_NONE;
AttitudeSource = ATTITUDE_SOURCE_ACC;
Orientation = ORIENTATION_FC;
}
#endif
}
break;
 
553,9 → 519,7
 
// check data from USART
USART0_ProcessRxData();
#ifdef HEADTRACKER
USART0_TransmitTxData();
#else
 
if(PC_Connected)
{
USART0_EnableTXD();
565,7 → 529,6
{
USART0_DisableTXD();
}
#endif
} // while(1)
}
 
/trunk/main.h
1,7 → 1,6
#ifndef _MAIN_H_
#define _MAIN_H_
 
#define HEADTRACKER
 
#include <avr/eeprom.h>
#include <inttypes.h>
11,8 → 10,7
{
ATTITUDE_SOURCE_I2C = 0,
ATTITUDE_SOURCE_UART = 1,
ATTITUDE_SOURCE_ACC = 2,
ATTITUDE_SOURCE_NONE = 3
ATTITUDE_SOURCE_ACC = 2
} AttitudeSource_t;
 
 
27,10 → 25,8
 
extern AttitudeSource_t AttitudeSource;
extern Orientation_t Orientation;
extern unsigned char InternalCalstate, ActualCalstate;
 
extern int16_t MagX, MagY, MagZ;
extern signed char PotiToFC[8];
 
void SetDebugValues(void);
 
/trunk/menu.c
88,36 → 88,10
 
case 1: // Magnet readings
LCD_printfxy(0,0,"Magnet Sensors: ");
LCD_printfxy(0,1,"X:%+4d Attitude", MagX);
LCD_printfxy(0,2,"Y:%+4d by", MagY);
LCD_printfxy(0,3,"Z:%+4d ", MagZ);
switch(AttitudeSource)
{
case ATTITUDE_SOURCE_I2C: LCD_printfxy(12,2,"I2C"); break;
case ATTITUDE_SOURCE_UART: LCD_printfxy(12,2,"UART"); break;
case ATTITUDE_SOURCE_ACC: LCD_printfxy(12,2,"ACC"); break;
default: LCD_printfxy(12,2,"???"); break;
LCD_printfxy(0,1,"X:%+4d ", MagX);
LCD_printfxy(0,2,"Y:%+4d ", MagY);
LCD_printfxy(0,3,"Z:%+4d ", MagZ);
break;
}
break;
case 2: // Calcstate
LCD_printfxy(0,0,"Calibrating: ");
LCD_printfxy(0,1,"Step: %1d ",ActualCalstate);
switch(ActualCalstate)
{
default:
case 0: LCD_printfxy(0,2," -- "); break;
case 3:
case 1: LCD_printfxy(0,2," pause "); break;
case 2: LCD_printfxy(0,2,"measure horizontal"); break;
case 4: LCD_printfxy(0,2,"measure vertical"); break;
case 5: LCD_printfxy(0,2,"store data"); break;
}
if(!ActualCalstate) LCD_printfxy(13,3,"(start)")
else LCD_printfxy(9,3,"(ESC)(step)");
if(RemoteKeys & KEY4) InternalCalstate++;
if(ActualCalstate >= 5 || (RemoteKeys & KEY3)) InternalCalstate = 0;
break;
 
default:
MaxMenuItem = MenuItem - 1;
/trunk/uart.c
98,6 → 98,7
#define RQST_EXTERN_CTRL 0x10
#define RQST_DISPLAY_DATA 0x20
 
 
uint8_t RequestFlags = 0x00;
uint8_t RequestDebugLabel = 0;
uint8_t ConfirmFrame = 0;
107,6 → 108,7
uint16_t PC_Connected = 0;
uint16_t FC_Connected = 0;
 
 
DebugOut_t DebugOut;
ExternData_t ExternData;
ExternControl_t ExternControl;
113,11 → 115,9
UART_VersionInfo_t UART_VersionInfo;
 
uint16_t DebugData_Timer;
uint16_t DebugData_Interval = 5000;
uint16_t DebugData_Interval = 500;
uint16_t Display_Timer;
uint16_t Display_Interval = 0;
uint16_t PotiIntervall = 50;
uint16_t PotiTimer;
 
const prog_uint8_t ANALOG_LABEL[32][16] =
{
138,8 → 138,8
"Magnet Z Range ",
"Calstate ",
"Heading ", //15
" ",
" ",
"User0 ",
"User1 ",
"Acc X ",
"Acc Y ",
"Acc Z ", //20
146,9 → 146,9
"RawAcc X ",
"RawAcc Y ",
"RawAcc Z ",
"Serial Poti1 ",
"Serial Poti2 ", //25
"Serial Poti3 ",
"Acc X Offset ",
"Acc Y Offset ", //25
"Acc Z Offset ",
"Heading X ",
"Heading Y ",
"Attitude Source ",
219,7 → 219,6
 
// initialize the debug timer
DebugData_Timer = SetDelay(DebugData_Interval);
PotiTimer = SetDelay(PotiIntervall);
 
// unlock rxd_buffer
rxd_buffer_locked = FALSE;
611,7 → 610,7
 
if(!txd_complete) return;
 
if((RequestFlags & RQST_DEBUG_LABEL))
if((RequestFlags & RQST_DEBUG_LABEL) && txd_complete)
{
uint8_t label[16];
memcpy_P(label, ANALOG_LABEL[RequestDebugLabel], 16); // read lable from flash to sram buffer
619,12 → 618,12
RequestDebugLabel = 0xFF;
RequestFlags &= ~RQST_DEBUG_LABEL;
}
else if(ConfirmFrame)
else if(ConfirmFrame && txd_complete)
{
SendOutData('B', MK3MAG_ADDRESS, 1, (uint8_t *) &ConfirmFrame, sizeof(ConfirmFrame));
ConfirmFrame = 0;
}
else if((((DebugData_Interval > 0) && CheckDelay(DebugData_Timer)) || (RequestFlags & RQST_DEBUG_DATA)))
else if(( ((DebugData_Interval > 0) && CheckDelay(DebugData_Timer)) || (RequestFlags & RQST_DEBUG_DATA)) && txd_complete)
{
SetDebugValues();
SendOutData('D', MK3MAG_ADDRESS, 1, (uint8_t *) &DebugOut, sizeof(DebugOut));
631,7 → 630,7
DebugData_Timer = SetDelay(DebugData_Interval);
RequestFlags &= ~RQST_DEBUG_DATA;
}
else if(( ((Display_Interval > 0) && CheckDelay(Display_Timer)) || (RequestFlags & RQST_DISPLAY_DATA)))
else if(( ((Display_Interval > 0) && CheckDelay(Display_Timer)) || (RequestFlags & RQST_DISPLAY_DATA)) && txd_complete)
{
if(DisplayLine > 3)// new format
{
647,30 → 646,26
Display_Timer = SetDelay(Display_Interval);
RequestFlags &= ~RQST_DISPLAY_DATA;
}
else if((RequestFlags & RQST_EXTERN_CTRL))
 
else if((RequestFlags & RQST_EXTERN_CTRL) && txd_complete)
{
SendOutData('G', MK3MAG_ADDRESS, 1, (uint8_t *) &ExternControl,sizeof(ExternControl));
RequestFlags &= ~RQST_EXTERN_CTRL;
}
else if((RequestFlags & RQST_COMPASS_HEADING))
 
else if((RequestFlags & RQST_COMPASS_HEADING) && txd_complete)
{
SendOutData('K', FC_ADDRESS, 1, (uint8_t *) &Heading, sizeof(Heading)); // send compass heading to FC
RequestFlags &= ~RQST_COMPASS_HEADING;
}
else if((RequestFlags & RQST_VERSION_INFO))
else if((RequestFlags & RQST_VERSION_INFO) && txd_complete)
{
SendOutData('V', MK3MAG_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo));
RequestFlags &= ~RQST_VERSION_INFO;
}
#ifdef HEADTRACKER
else if((PotiIntervall > 0) && CheckDelay(PotiTimer))
{
SendOutData('y', FC_ADDRESS, 1, (uint8_t *) PotiToFC, sizeof(PotiToFC)); // send compass heading to FC
PotiTimer = SetDelay(PotiIntervall);
}
#endif
}
 
 
void USART0_Print(int8_t *msg)
{
uint8_t i = 0;