Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 67 → Rev 68

/trunk/main.c
106,8 → 106,10
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);
119,7 → 121,7
else MagY = 0;
if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range);
else MagZ = 0;
if(AccPresent)
if(AttitudeSource == ATTITUDE_SOURCE_ACC)
{
AccX = (RawAccX - Calibration.AccX.Offset);
AccY = (RawAccY - Calibration.AccY.Offset);
238,28 → 240,22
 
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;
 
// 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;
if(ActualCalstate > 5) ActualCalstate = 0;
// blink code for current calibration state
if(cal)
if(ActualCalstate)
{
if(CheckDelay(Led_Timer) || (cal != calold))
if(CheckDelay(Led_Timer) || (ActualCalstate != 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 * cal))
if((blinkcount + 1 ) >= (2 * ActualCalstate))
{
blinkcount = 0;
Led_Timer = SetDelay(1500);
276,7 → 272,7
if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF;
}
// calibration state machine
switch(cal)
switch(ActualCalstate)
{
case 1: // 1st step of calibration
// initialize ranges
316,7 → 312,7
 
case 5:
// Save values
if(cal != calold) // avoid continously writing of eeprom!
if(ActualCalstate != calold) // avoid continously writing of eeprom!
{
Calibration.MagX.Range = Xmax - Xmin;
Calibration.MagX.Offset = (Xmin + Xmax) / 2;
346,7 → 342,7
default:
break;
}
calold = cal;
calold = ActualCalstate;
}
 
 
369,12 → 365,14
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;
382,11 → 380,10
DebugOut.Analog[11] = Calibration.MagY.Range;
DebugOut.Analog[12] = Calibration.MagZ.Offset;
DebugOut.Analog[13] = Calibration.MagZ.Range;
if(I2C_WriteCal.CalByte) DebugOut.Analog[14] = I2C_WriteCal.CalByte;
else DebugOut.Analog[14] = ExternData.CalState;
DebugOut.Analog[14] = ActualCalstate;
DebugOut.Analog[15] = Heading;
DebugOut.Analog[16] = ExternData.UserParam[0];
DebugOut.Analog[17] = ExternData.UserParam[1];
DebugOut.Analog[16] = 0;
DebugOut.Analog[17] = 0;
DebugOut.Analog[18] = AccX;
DebugOut.Analog[19] = AccY;
DebugOut.Analog[20] = AccZ;
393,9 → 390,9
DebugOut.Analog[21] = RawAccX;
DebugOut.Analog[22] = RawAccY;
DebugOut.Analog[23] = RawAccZ;
DebugOut.Analog[24] = Calibration.AccX.Offset;
DebugOut.Analog[25] = Calibration.AccY.Offset;
DebugOut.Analog[26] = Calibration.AccZ.Offset;
DebugOut.Analog[24] = PotiToFC[0];
DebugOut.Analog[25] = PotiToFC[1];
DebugOut.Analog[26] = PotiToFC[2];
DebugOut.Analog[29] = AttitudeSource;
}
 
415,6 → 412,28
}
}
 
#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;
431,12 → 450,17
 
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");
 
USART0_Print("\n\rwww.MikroKopter.de (c) HiSystems GmbH\n\n");
#ifdef HEADTRACKER
USART0_Print("Head-Tracker\n");
#endif
if(AccPresent)
{
USART0_Print("\n\nACC present\n");
USART0_Print("ACC present\n");
}
#ifdef HEADTRACKER
else USART0_Print("\nERROR: NO ACC\n");
#endif
 
LED_GRN_ON;
 
445,11 → 469,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)
{
495,19 → 519,29
AccMeasurement();
statetimer = SetDelay(1);
state = 0;
 
CalcFields();
if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
// 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();
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)
{
AttitudeSource = ATTITUDE_SOURCE_ACC;
if(AccPresent) AttitudeSource = ATTITUDE_SOURCE_ACC;
else AttitudeSource = ATTITUDE_SOURCE_NONE;
Orientation = ORIENTATION_FC;
}
#endif
}
break;
 
519,7 → 553,9
 
// check data from USART
USART0_ProcessRxData();
 
#ifdef HEADTRACKER
USART0_TransmitTxData();
#else
if(PC_Connected)
{
USART0_EnableTXD();
529,6 → 565,7
{
USART0_DisableTXD();
}
#endif
} // while(1)
}
 
/trunk/main.h
1,6 → 1,7
#ifndef _MAIN_H_
#define _MAIN_H_
 
#define HEADTRACKER
 
#include <avr/eeprom.h>
#include <inttypes.h>
10,7 → 11,8
{
ATTITUDE_SOURCE_I2C = 0,
ATTITUDE_SOURCE_UART = 1,
ATTITUDE_SOURCE_ACC = 2
ATTITUDE_SOURCE_ACC = 2,
ATTITUDE_SOURCE_NONE = 3
} AttitudeSource_t;
 
 
25,8 → 27,10
 
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,10 → 88,36
 
case 1: // Magnet readings
LCD_printfxy(0,0,"Magnet Sensors: ");
LCD_printfxy(0,1,"X:%+4d ", MagX);
LCD_printfxy(0,2,"Y:%+4d ", MagY);
LCD_printfxy(0,3,"Z:%+4d ", MagZ);
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;
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,7 → 98,6
#define RQST_EXTERN_CTRL 0x10
#define RQST_DISPLAY_DATA 0x20
 
 
uint8_t RequestFlags = 0x00;
uint8_t RequestDebugLabel = 0;
uint8_t ConfirmFrame = 0;
108,7 → 107,6
uint16_t PC_Connected = 0;
uint16_t FC_Connected = 0;
 
 
DebugOut_t DebugOut;
ExternData_t ExternData;
ExternControl_t ExternControl;
115,9 → 113,11
UART_VersionInfo_t UART_VersionInfo;
 
uint16_t DebugData_Timer;
uint16_t DebugData_Interval = 500;
uint16_t DebugData_Interval = 5000;
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 ",
"Acc X Offset ",
"Acc Y Offset ", //25
"Acc Z Offset ",
"Serial Poti1 ",
"Serial Poti2 ", //25
"Serial Poti3 ",
"Heading X ",
"Heading Y ",
"Attitude Source ",
219,6 → 219,7
 
// initialize the debug timer
DebugData_Timer = SetDelay(DebugData_Interval);
PotiTimer = SetDelay(PotiIntervall);
 
// unlock rxd_buffer
rxd_buffer_locked = FALSE;
610,7 → 611,7
 
if(!txd_complete) return;
 
if((RequestFlags & RQST_DEBUG_LABEL) && txd_complete)
if((RequestFlags & RQST_DEBUG_LABEL))
{
uint8_t label[16];
memcpy_P(label, ANALOG_LABEL[RequestDebugLabel], 16); // read lable from flash to sram buffer
618,12 → 619,12
RequestDebugLabel = 0xFF;
RequestFlags &= ~RQST_DEBUG_LABEL;
}
else if(ConfirmFrame && txd_complete)
else if(ConfirmFrame)
{
SendOutData('B', MK3MAG_ADDRESS, 1, (uint8_t *) &ConfirmFrame, sizeof(ConfirmFrame));
ConfirmFrame = 0;
}
else if(( ((DebugData_Interval > 0) && CheckDelay(DebugData_Timer)) || (RequestFlags & RQST_DEBUG_DATA)) && txd_complete)
else if((((DebugData_Interval > 0) && CheckDelay(DebugData_Timer)) || (RequestFlags & RQST_DEBUG_DATA)))
{
SetDebugValues();
SendOutData('D', MK3MAG_ADDRESS, 1, (uint8_t *) &DebugOut, sizeof(DebugOut));
630,7 → 631,7
DebugData_Timer = SetDelay(DebugData_Interval);
RequestFlags &= ~RQST_DEBUG_DATA;
}
else if(( ((Display_Interval > 0) && CheckDelay(Display_Timer)) || (RequestFlags & RQST_DISPLAY_DATA)) && txd_complete)
else if(( ((Display_Interval > 0) && CheckDelay(Display_Timer)) || (RequestFlags & RQST_DISPLAY_DATA)))
{
if(DisplayLine > 3)// new format
{
646,26 → 647,30
Display_Timer = SetDelay(Display_Interval);
RequestFlags &= ~RQST_DISPLAY_DATA;
}
 
else if((RequestFlags & RQST_EXTERN_CTRL) && txd_complete)
else if((RequestFlags & RQST_EXTERN_CTRL))
{
SendOutData('G', MK3MAG_ADDRESS, 1, (uint8_t *) &ExternControl,sizeof(ExternControl));
RequestFlags &= ~RQST_EXTERN_CTRL;
}
 
else if((RequestFlags & RQST_COMPASS_HEADING) && txd_complete)
else if((RequestFlags & RQST_COMPASS_HEADING))
{
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) && txd_complete)
else if((RequestFlags & RQST_VERSION_INFO))
{
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;