Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 20 → Rev 21

/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c
57,6 → 57,7
#include <avr/interrupt.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
 
#include "main.h"
#include "timer0.h"
70,6 → 71,8
int16_t RawMagnet2a, RawMagnet2b;
int16_t RawMagnet3a, RawMagnet3b;
 
uint16_t Led_Timer = 0;
 
typedef struct
{
int16_t Range;
87,12 → 90,12
Calibration_t Calibration; // calibration data in RAM
 
 
int16_t UncalMagnetX, UncalMagnetY, UncalMagnetZ; // sensor signal difference without Scaling
int16_t MagnetX, MagnetY, MagnetZ; // rescaled magnetic field readings
int16_t UncalMagnetX, UncalMagnetY, UncalMagnetZ; // sensor signal difference without Scaling
int16_t MagnetX, MagnetY, MagnetZ; // rescaled magnetic field readings
 
uint8_t PC_Connected = 0;
 
int16_t Heading;
int16_t Heading = -1;
 
 
void CalcFields(void)
112,7 → 115,14
double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
int16_t heading = -1;
 
// blink code for normal operation
if(CheckDelay(Led_Timer))
{
LED_GRN_TOGGLE;
Led_Timer = SetDelay(500);
}
 
 
Cx = MagnetX;
Cy = MagnetY;
Cz = MagnetZ;
146,23 → 156,52
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;
/*
// check both sources of communication for calibration request
if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte;
else cal = ExternData.CalState;
else cal = ExternData.CalState;*/
 
cal = ExternData.CalState;
 
// blink code for current calibration state
if(cal)
{
if(CheckDelay(Led_Timer) || (cal != calold))
{
if(blinkcount & 0x01) LED_GRN_OFF;
else LED_GRN_ON;
 
// end of blinkcount sequence
if( (blinkcount + 1 ) >= (2 * cal) )
{
blinkcount = 0;
Led_Timer = SetDelay(1000);
}
else
{
blinkcount++;
Led_Timer = SetDelay(170);
}
}
}
else
{
LED_GRN_OFF;
}
 
 
// calibration state machine
switch(cal)
{
case 0: // no calibration
LED_GRN_ON;
break;
 
case 1: // 1st step of calibration
// initialize ranges
// used to change the orientation of the MK3MAG in the horizontal plane
LED_GRN_OFF;
Xmin = 10000;
Xmax = -10000;
Ymin = 10000;
173,7 → 212,6
 
case 2: // 2nd step of calibration
// find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane
LED_GRN_ON;
if(UncalMagnetX < Xmin) Xmin = UncalMagnetX;
if(UncalMagnetX > Xmax) Xmax = UncalMagnetX;
if(UncalMagnetY < Ymin) Ymin = UncalMagnetY;
182,36 → 220,43
 
case 3: // 3rd step of calibration
// used to change the orietation of the MK3MAG vertical to the horizontal plane
LED_GRN_OFF;
break;
 
case 4:
// find Min and Max of the Z-Sensor
LED_GRN_ON;
if(UncalMagnetZ < Zmin) Zmin = UncalMagnetZ;
if(UncalMagnetZ > Zmax) Zmax = UncalMagnetZ;
break;
 
case 5:
LED_GRN_OFF; // Save values
Calibration.X.Range = Xmax - Xmin;
Calibration.X.Offset = (Xmin + Xmax) / 2;
Calibration.Y.Range = Ymax - Ymin;
Calibration.Y.Offset = (Ymin + Ymax) / 2;
Calibration.Z.Range = Zmax - Zmin;
Calibration.Z.Offset = (Zmin + Zmax) / 2;
if((Calibration.X.Range > 150) && (Calibration.Y.Range > 150) && (Calibration.Z.Range > 150))
// Save values
if(cal != calold) // avoid continously wrinting of eeprom!
{
eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration_t));
Delay_ms(2000);
Calibration.X.Range = Xmax - Xmin;
Calibration.X.Offset = (Xmin + Xmax) / 2;
Calibration.Y.Range = Ymax - Ymin;
Calibration.Y.Offset = (Ymin + Ymax) / 2;
Calibration.Z.Range = Zmax - Zmin;
Calibration.Z.Offset = (Zmin + Zmax) / 2;
if((Calibration.X.Range > 150) && (Calibration.Y.Range > 150) && (Calibration.Z.Range > 150))
{
// indicate write process by setting the led
LED_GRN_ON;
eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration_t));
Delay_ms(2000);
// reset led state
LED_GRN_OFF;
// reset blinkcode
blinkcount = 0;
Led_Timer = SetDelay(1000);
}
}
LED_GRN_ON;
break;
 
default:
LED_GRN_ON;
break;
}
calold = cal;
}
 
 
235,6 → 280,7
DebugOut.Analog[15] = Heading;
DebugOut.Analog[16] = ExternData.UserParam[0];
DebugOut.Analog[17] = ExternData.UserParam[1];
DebugOut.Analog[31] = PC_Connected;
}
 
 
247,9 → 293,13
ADC_Init();
I2C_Init();
 
 
sei(); //Globale Interrupts Einschalten
 
 
 
Debug_Timer = SetDelay(100); // Sendeintervall
Led_Timer = SetDelay(100);
 
// read calibration info from eeprom
eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration_t));
268,6 → 318,7
RawMagnet3a = ADC_GetValue(ADC7);
Delay_ms(1);
 
 
FLIP_HIGH;
Delay_ms(2);
RawMagnet1b = ADC_GetValue(ADC0);
277,7 → 328,8
 
CalcFields();
 
if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
//if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate();
if(ExternData.CalState) Calibrate();
else CalcHeading();
 
// check data from USART
/branches/MK3Mag V0.14 Code Redesign Killagreg/uart.c
66,8 → 66,8
#define FALSE 0
#define TRUE 1
 
#define TXD_BUFFER_LEN 100
#define RXD_BUFFER_LEN 100
#define TXD_BUFFER_LEN 150
#define RXD_BUFFER_LEN 150
 
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
volatile uint8_t rxd_buffer_locked = FALSE;
75,6 → 75,8
volatile uint8_t txd_complete = TRUE;
volatile uint8_t ReceivedBytes = 0;
 
 
 
uint8_t RequestVerInfo = FALSE;
uint8_t RequestDebugData = FALSE;
uint8_t RequestDebugLabel = FALSE;
109,7 → 111,7
"ZOffset ",
"ZRange ",
"Calstate ",
"Heading Request ", //15
"Heading ", //15
"User0 ",
"User1 ",
"Analog18 ",
162,9 → 164,9
 
// USART0 Control and Status Register A, B, C
 
// enable double speed operation in
// enable double speed operation
UCSR0A |= (1 << U2X0);
// enable receiver and transmitter in
// enable receiver and transmitter
UCSR0B |= (1 << TXEN0) | (1 << RXEN0);
// set asynchronous mode
UCSR0C &= ~(1 << UMSEL01);
201,7 → 203,7
VersionInfo.Minor = VERSION_MINOR;
VersionInfo.PCCompatible = VERSION_COMPATIBLE;
 
// Version beim Start ausgeben (nicht schön, aber geht... )
// Version beim Start ausgeben (nicht schön, aber geht... )
uart_putchar ('\n');uart_putchar ('C');uart_putchar ('P');uart_putchar (':');
uart_putchar ('V');uart_putchar (0x30 + VERSION_MAJOR);uart_putchar ('.');uart_putchar (0x30 + VERSION_MINOR/10); uart_putchar (0x30 + VERSION_MINOR%10);
uart_putchar ('\n');
210,6 → 212,7
// ---------------------------------------------------------------------------------
void USART0_EnableTXD(void)
{
//if(!(UCSR0B & (1 << TXEN0))) return;
DDRD |= (1<<DDD1); // set TXD pin as output
UCSR0B |= (1 << TXEN0); // enable TX in USART
}
217,6 → 220,9
// ---------------------------------------------------------------------------------
void USART0_DisableTXD(void)
{
//if((UCSR0B & (1 << TXEN0))) return;
while(!txd_complete){ };
 
UCSR0B &= ~(1 << TXEN0); // disable TXD in USART
DDRD &= ~(1<<DDD1); // set TXD pin as input
}
236,7 → 242,7
if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
{
ptr_txd_buffer = 0; // reset txd pointer
txd_complete = 1; // stop transmission
txd_complete = TRUE; // stop transmission
}
UDR0 = tmp_tx; // send current byte will trigger this ISR again
}
250,9 → 256,9
ISR(USART_RX_vect)
{
static uint16_t crc;
static uint8_t ptr_rxd_buffer = 0;
uint8_t crc1, crc2;
uint8_t c;
static uint8_t ptr_rxd_buffer = 0;
 
c = UDR0; // catch the received byte
 
381,7 → 387,7
 
 
// --------------------------------------------------------------------------
int16_t uart_putchar (int8_t c)
int uart_putchar (int8_t c)
{
// if tx is not enabled return immediatly
if(!(UCSR0B & (1 << TXEN0))) return (0);
406,7 → 412,6
{
case 'w':// Attitude
Decode64((uint8_t *) &ExternData, sizeof(ExternData), 3, ReceivedBytes);
DebugOut.Analog[15]++;
RequestCompassHeading = TRUE;
break;
 
413,8 → 418,20
case 'b': // extern control
case 'c': // extern control with debug request
Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3,ReceivedBytes);
#define KEY1 0x01
#define KEY2 0x02
#define KEY3 0x04
#define KEY4 0x08
#define KEY5 0x10
// use right arrow at display for switching the calstate
if(ExternControl.RemoteButtons & KEY2)
{
ExternData.CalState++;
if(ExternData.CalState == 6) ExternData.CalState = 0;
}
ExternData.Attitude[0] = ExternControl.Par1;
ExternData.Attitude[1] = ExternControl.Par2;
PC_Connected = 255;
break;
 
case 'h':// x-1 display columns
/branches/MK3Mag V0.14 Code Redesign Killagreg/uart.h
18,7 → 18,7
void USART0_DisableTXD(void);
void USART0_TransmitTxData(void);
void USART0_ProcessRxData(void);
int16_t uart_putchar(int8_t c);
int uart_putchar (int8_t c);