Subversion Repositories MK3Mag

Compare Revisions

Ignore whitespace Rev 28 → Rev 29

/branches/MK3Mag V0.14 Code Redesign Killagreg/analog.c
54,14 → 54,19
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//#include <stdlib.h>
#include <util/delay.h>
#include "analog.h"
 
uint8_t AccPresent = 0;
 
void ADC_Init(void)
{
// The analog inputs have a VREF of 5V and a resolution of 10 bit (0...1024 counts)
// i.e. 4.88mV/Count
 
// set PortC 0,1,2,3 as input
DDRC &= ~((1<<DDC3)|(1<<DDC2)|(1<<DDC1)|(1<<DDC0));
// set PortC 0,1,2,3 and 6,7 as tri state
// set PortC 0,1,2,3 as tri state
PORTC &= ~((1<<PORTC3)|(1<<PORTC2)|(1<<PORTC1)|(1<<PORTC0));
 
// port PD5 and PD6 control the current direction of the test coils of the sensors
78,6 → 83,22
ADCSRA |= ((1<<ADEN)|(1<<ADIF)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0));
ADMUX = 0x00; // select ADC0
ADCSRA |= (1<<ADSC); // start conversion
 
 
// Check if acceleration sensor is present
// The output of the LIS3L02AL (MK3MAG V1.0) and the LIS344ALH (MK3MAG V1.1) is Vdd/5 +/- 10% per 1g.
// The Vdd is 3.0V at this board therefore the sensitivity is 0.6V/g +/-10% that corresponds to 123 counts.
// The offsets at 0g is VDD/2 (1.5V) that is 307 counts.
// that yields to an ADC range of 184 to 430 counts.
 
// pullup PC2(AccX) and PC3 (AccY)
PORTC |= ((1<<PORTC3)|(1<<PORTC2));
_delay_ms(10.0);
// if ADC2 and ADC 3 is larger than 1000 counts (4.88V) no load is at the pins
if((ADC_GetValue(ADC2) > 1000) && (ADC_GetValue(ADC3) > 1000)) AccPresent = 0;
else AccPresent = 1;
// set port back to tristate
PORTC &= ~((1<<PORTC3)|(1<<PORTC2));
}
 
 
84,11 → 105,11
uint16_t ADC_GetValue(ADChannel_t channel)
{
uint16_t value = 0;
ADMUX = channel; // set muxer bits
ADCSRA |= (1<<ADIF); // clear ready-flag
ADCSRA |= (1<<ADSC); // start conversion
ADMUX = channel; // set muxer bits
ADCSRA |= (1<<ADIF); // clear ready-flag
ADCSRA |= (1<<ADSC); // start conversion
while (((ADCSRA & (1<<ADIF)) == 0)); // wait for end of conversion
value = ADCW; // read adc result
value = ADCW; // read adc result
return(value);
}
 
/branches/MK3Mag V0.14 Code Redesign Killagreg/analog.h
21,6 → 21,17
AGND = 15
} ADChannel_t;
 
 
#define MAG_X ADC0
#define MAG_Y ADC1
#define MAG_Z ADC7
#define ACC_X ADC2
#define ACC_Y ADC3
#define ACC_Z ADC6
 
 
extern uint8_t AccPresent;
 
void ADC_Init(void);
uint16_t ADC_GetValue(ADChannel_t channel);
 
/branches/MK3Mag V0.14 Code Redesign Killagreg/main.c
68,6 → 68,7
 
 
AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC;
Orientation_t Orientation = ORIENTATION_FC;
 
uint16_t Led_Timer = 0;
 
85,7 → 86,7
struct Scaling_t AccX;
struct Scaling_t AccY;
struct Scaling_t AccZ;
} ;
};
 
struct Calibration_t eeCalibration EEMEM; // calibration data in EEProm
struct Calibration_t Calibration; // calibration data in RAM
95,16 → 96,16
int16_t RawMagnet2a, RawMagnet2b;
int16_t RawMagnet3a, RawMagnet3b;
int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling
int16_t MagX, MagY, MagZ; // rescaled magnetic field readings
// acc sensor variables
int16_t RawAccX, RawAccY, RawAccZ; // raw acceleration readings
int16_t AccX, AccY, AccZ; // rescaled acceleration readings
uint8_t AccPresent = 0;
uint8_t PC_Connected = 0;
int16_t MagX = 0, MagY = 0, MagZ = 0; // rescaled magnetic field readings
 
int16_t Heading = -1;
// acceleration sensor variables
int16_t RawAccX = 0, RawAccY = 0, RawAccZ = 0; // raw acceleration readings
int16_t AccX = 0, AccY = 0, AccZ = 0; // rescaled acceleration readings
int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0; // nick and roll angle from acc
 
int16_t Heading = -1; // the current compass heading in deg
 
 
void CalcFields(void)
{
UncalMagX = (RawMagnet1a - RawMagnet1b);
117,12 → 118,46
else MagY = 0;
if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range);
else MagZ = 0;
 
if(AccPresent)
{
AccX = (RawAccX - Calibration.AccX.Offset);
AccY = (RawAccY - Calibration.AccY.Offset);
AccZ = (Calibration.AccZ.Offset - RawAccZ);
 
#if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL
// acc mode assumes orientation like FC
if(AccX > 136) AccAttitudeNick = -800;
else
if(AccX < -136) AccAttitudeNick = 800;
else AccAttitudeNick = (int16_t)(-1800.0 * asin((double) AccX / 138.0) / M_PI);
 
 
if(AccY > 136) AccAttitudeRoll = 800;
else
if(AccY < -136) AccAttitudeRoll = -800;
else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
 
#else // the hardware 1.1 has the LIS344ALH with a different axis definition (X -> -Y, Y -> X, Z -> Z)
// acc mode assumes orientation like FC
if(AccY > 136) AccAttitudeNick = 800;
else
if(AccY < -136) AccAttitudeNick = -800;
else AccAttitudeNick = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI);
 
 
if(AccX > 136) AccAttitudeRoll = 800;
else
if(AccX < -136) AccAttitudeRoll = -800;
else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccX / 138.0) / M_PI);
#endif
}
}
 
 
void CalcHeading(void)
{
double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0;
int16_t heading = -1;
 
// blink code for normal operation
132,15 → 167,20
Led_Timer = SetDelay(500);
}
 
Cx = MagX;
Cy = MagY;
Cz = MagZ;
switch(Orientation)
{
case ORIENTATION_NC:
Cx = MagX;
Cy = MagY;
Cz = MagZ;
break;
 
if(ExternData.Orientation == 1)
{
Cx = MagX;
Cy = -MagY;
Cz = MagZ;
case ORIENTATION_FC:
// rotation of 90 deg compared to NC setup
Cx = MagY;
Cy = -MagX;
Cz = MagZ;
break;
}
 
// calculate nick and roll angle in rad
157,22 → 197,14
break;
 
case ATTITUDE_SOURCE_ACC:
if(AccX > 125) nick_rad = M_PI / 2;
else
if(AccX < -125) nick_rad = -M_PI / 2;
else
{
nick_rad = asin((double) AccX / 125.0);
}
nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0);
roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0);
break;
 
if(AccY > 125) roll_rad = M_PI / 2;
else
if(AccY < -125) roll_rad = -M_PI / 2;
else
{
roll_rad = asin((double) AccY / 125.0);
}
break;
default:
nick_rad = 0;
roll_rad = 0;
break;
}
 
// calculate attitude correction
179,6 → 211,10
Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
 
DebugOut.Analog[27] = (int16_t)Hx;
DebugOut.Analog[28] = (int16_t)Hy;
 
 
// calculate Heading
heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
// atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
269,7 → 305,7
// Save values
if(cal != calold) // avoid continously writing of eeprom!
{
Calibration.MagY.Range = Xmax - Xmin;
Calibration.MagX.Range = Xmax - Xmin;
Calibration.MagX.Offset = (Xmin + Xmax) / 2;
Calibration.MagY.Range = Ymax - Ymin;
Calibration.MagY.Offset = (Ymin + Ymax) / 2;
280,12 → 316,9
// indicate write process by setting the led
LED_GRN_ON;
eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration));
Delay_ms(2000);
// reset led state
LED_GRN_OFF;
Led_Timer = SetDelay(2000);
// reset blinkcode
blinkcount = 0;
Led_Timer = SetDelay(1000);
}
}
break;
308,7 → 341,8
switch(AttitudeSource)
{
case ATTITUDE_SOURCE_ACC:
 
DebugOut.Analog[6] = AccAttitudeNick;
DebugOut.Analog[7] = AccAttitudeRoll;
break;
 
case ATTITUDE_SOURCE_UART:
341,6 → 375,7
DebugOut.Analog[24] = Calibration.AccX.Offset;
DebugOut.Analog[25] = Calibration.AccY.Offset;
DebugOut.Analog[26] = Calibration.AccZ.Offset;
DebugOut.Analog[29] = AttitudeSource;
}
 
void AccMeasurement(void)
347,9 → 382,9
{
if(AccPresent)
{
RawAccX = (RawAccX + (int16_t)ADC_GetValue(ADC2))/2;
RawAccY = (RawAccY + (int16_t)ADC_GetValue(ADC3))/2;
RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ADC6))/2;
RawAccX = (RawAccX + (int16_t)ADC_GetValue(ACC_X))/2;
RawAccY = (RawAccY + (int16_t)ADC_GetValue(ACC_Y))/2;
RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ACC_Z))/2;
}
else
{
357,13 → 392,8
RawAccY = 0;
RawAccZ = 0;
}
AccX = ((RawAccX - Calibration.AccX.Offset) + AccX * 7) / 8;
AccY = ((RawAccY - Calibration.AccY.Offset) + AccY * 7) / 8;
AccZ = ((Calibration.AccZ.Offset - RawAccZ) + AccZ * 7) / 8;
}
 
 
 
int main (void)
{
// reset input pullup
378,6 → 408,11
 
sei(); // enable globale interrupts
 
if(AccPresent)
{
USART0_Print("ACC present\n");
}
 
LED_GRN_ON;
 
Debug_Timer = SetDelay(200);
386,7 → 421,6
// read calibration info from eeprom
eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration));
 
ExternData.Orientation = 0;
ExternData.CalState = 0;
I2C_WriteCal.CalByte = 0;
 
396,17 → 430,17
{
FLIP_LOW;
Delay_ms(2);
RawMagnet1a = ADC_GetValue(ADC0);
RawMagnet2a = -ADC_GetValue(ADC1);
RawMagnet3a = ADC_GetValue(ADC7);
RawMagnet1a = ADC_GetValue(MAG_X);
RawMagnet2a = -ADC_GetValue(MAG_Y);
RawMagnet3a = ADC_GetValue(MAG_Z);
AccMeasurement();
Delay_ms(1);
 
FLIP_HIGH;
Delay_ms(2);
RawMagnet1b = ADC_GetValue(ADC0);
RawMagnet2b = -ADC_GetValue(ADC1);
RawMagnet3b = ADC_GetValue(ADC7);
RawMagnet1b = ADC_GetValue(MAG_X);
RawMagnet2b = -ADC_GetValue(MAG_Y);
RawMagnet3b = ADC_GetValue(MAG_Z);
AccMeasurement();
Delay_ms(1);
 
417,8 → 451,16
 
// check data from USART
USART0_ProcessRxData();
USART0_TransmitTxData();
 
if(NC_Connected) NC_Connected--;
if(FC_Connected) FC_Connected--;
// fall back to attitude estimation from acc sensor if NC or FC does'nt send attittude data
if(!NC_Connected && ! NC_Connected)
{
AttitudeSource = ATTITUDE_SOURCE_ACC;
Orientation = ORIENTATION_FC;
}
 
if(PC_Connected)
{
USART0_EnableTXD();
/branches/MK3Mag V0.14 Code Redesign Killagreg/main.h
6,9 → 6,6
#include <inttypes.h>
 
 
 
#define SYSCLK 8000000L //Quarz Frequenz in Hz
 
typedef enum
{
ATTITUDE_SOURCE_I2C,
17,9 → 14,17
} AttitudeSource_t;
 
 
extern uint8_t PC_Connected;
typedef enum
{
ORIENTATION_NC = 0,
ORIENTATION_FC = 1
} Orientation_t;
 
 
extern int16_t Heading;
 
extern AttitudeSource_t AttitudeSource;
extern Orientation_t Orientation;
 
extern int16_t MagX, MagY, MagZ;
 
/branches/MK3Mag V0.14 Code Redesign Killagreg/makefile
1,9 → 1,12
#--------------------------------------------------------------------
# MCU name
MCU = atmega168
F_CPU = 8000000
#-------------------------------------------------------------------
BOARD = 10
#BOARD = 11
VERSION_MAJOR = 0
VERSION_MINOR = 14
VERSION_MINOR = 16
 
VERSION_COMPATIBLE = 8 # PC-Kompatibilität
#-------------------------------------------------------------------
39,6 → 42,8
EXTRAINCDIRS =
 
 
CDEFS = -DF_CPU=$(F_CPU)UL
 
# Optional compiler flags.
# -g: generate debugging information (for GDB, or for COFF conversion)
# -O*: optimization level
60,9 → 65,9
#CFLAGS += -std=gnu89
#CFLAGS += -std=c99
CFLAGS += -std=gnu99
CFLAGS += $(CDEFS)
CFLAGS += -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_COMPATIBLE=$(VERSION_COMPATIBLE) -DBOARD=$(BOARD)
 
CFLAGS += -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_COMPATIBLE=$(VERSION_COMPATIBLE)
 
ifeq ($(AVR_CTRL_PLATINE), 1)
CFLAGS += -DAVR_CTRL_PLATINE=$(AVR_CTRL_PLATINE)
endif
/branches/MK3Mag V0.14 Code Redesign Killagreg/timer0.c
114,7 → 114,7
// disable PWM when bad compass heading value
if(Heading < 0)
{
PORTB &= ~(PORTB2);
PORTB &= ~(1<<PORTB2);
cmps_cnt = 0;
}
else
123,7 → 123,7
if(++cmps_cnt >= 380)
{
// set PWM out to high
PORTB |= PORTB2;
PORTB |= (1<<PORTB2);
// reset periode counter
cmps_cnt = 0;
}
131,7 → 131,7
else if(cmps_cnt >= (Heading + 10))
{
// set PWM out to low
PORTB &= ~(PORTB2);
PORTB &= ~(1<<PORTB2);
}
}
 
/branches/MK3Mag V0.14 Code Redesign Killagreg/twislave.c
70,6 → 70,7
 
volatile uint8_t I2C_PrimRxBuffer[10];
 
uint8_t NC_Connected = 0;
struct I2C_Heading_t I2C_Heading;
struct I2C_WriteAttitude_t I2C_WriteAttitude;
struct I2C_Mag_t I2C_Mag;
187,6 → 188,8
I2C_RxBufferSize = sizeof(I2C_WriteAttitude);
I2C_Heading.Heading = Heading;
AttitudeSource = ATTITUDE_SOURCE_I2C;
Orientation = ORIENTATION_NC;
NC_Connected = 255;
break;
default: // unknown command id
I2C_RxBuffer = 0;
/branches/MK3Mag V0.14 Code Redesign Killagreg/twislave.h
48,12 → 48,13
int16_t Heading;
} ;
 
extern uint8_t NC_Connected;
extern struct I2C_Heading_t I2C_Heading;
extern struct I2C_WriteAttitude_t I2C_WriteAttitude;
extern struct I2C_Mag_t I2C_Mag;
extern struct I2C_Version_t I2C_Version;
extern struct I2C_WriteCal_t I2C_WriteCal;
 
struct I2C_Heading_t I2C_Heading;
struct I2C_WriteAttitude_t I2C_WriteAttitude;
struct I2C_Mag_t I2C_Mag;
struct I2C_Version_t I2C_Version;
struct I2C_WriteCal_t I2C_WriteCal;
 
void I2C_Init(void);
 
/branches/MK3Mag V0.14 Code Redesign Killagreg/uart.c
66,8 → 66,9
#define FALSE 0
#define TRUE 1
 
// keep buffers as small as possible
#define TXD_BUFFER_LEN 100
#define RXD_BUFFER_LEN 100
#define RXD_BUFFER_LEN 30
 
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
volatile uint8_t rxd_buffer_locked = FALSE;
83,6 → 84,8
uint8_t RequestFlags = 0x00;
uint8_t RequestDebugLabel = 0;
 
uint8_t PC_Connected = 0;
uint8_t FC_Connected = 0;
 
uint8_t MySlaveAddr = 0;
 
137,7 → 140,7
/****************************************************************/
void USART0_Init (void)
{
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * BAUD_RATE) - 1);
uint16_t ubrr = (uint16_t) ((uint32_t) F_CPU/(8 * BAUD_RATE) - 1);
 
// disable all interrupts before configuration
cli();
202,15 → 205,16
 
 
// 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');
USART0_putchar ('\n');
USART0_putchar ('C');
USART0_putchar ('P');
USART0_putchar (':');
USART0_putchar ('V');
USART0_putchar (0x30 + VERSION_MAJOR);
USART0_putchar ('.');
USART0_putchar (0x30 + VERSION_MINOR/10);
USART0_putchar (0x30 + VERSION_MINOR%10);
USART0_putchar ('\n');
}
 
// ---------------------------------------------------------------------------------
393,11 → 397,11
 
 
// --------------------------------------------------------------------------
int uart_putchar (int8_t c)
int16_t USART0_putchar (int8_t c)
{
// if tx is not enabled return immediatly
if(!(UCSR0B & (1 << TXEN0))) return (0);
if (c == '\n') uart_putchar('\r');
if (c == '\n') USART0_putchar('\r');
// wait until previous character was send
loop_until_bit_is_set(UCSR0A, UDRE0);
// send character
418,10 → 422,10
{
case 'w':// Attitude
Decode64((uint8_t *) &ExternData, sizeof(ExternData), 3, ReceivedBytes);
I2C_WriteAttitude.Nick = ExternData.Attitude[NICK];
I2C_WriteAttitude.Nick = ExternData.Attitude[ROLL];
RequestFlags |= COMPASS_HEADING;
AttitudeSource = ATTITUDE_SOURCE_UART;
RequestFlags |= COMPASS_HEADING;
Orientation = ExternData.Orientation;
FC_Connected = 255;
break;
 
case 'b': // extern control
510,3 → 514,13
RequestFlags &= ~COMPASS_HEADING;
}
}
 
 
void USART0_Print(int8_t *msg)
{
uint8_t i = 0;
while(msg[i] != 0)
{
USART0_putchar(msg[i++]);
}
}
/branches/MK3Mag V0.14 Code Redesign Killagreg/uart.h
18,7 → 18,8
void USART0_DisableTXD(void);
void USART0_TransmitTxData(void);
void USART0_ProcessRxData(void);
int uart_putchar (int8_t c);
int16_t USART0_putchar(int8_t c);
void USART0_Print(int8_t *msg);
 
 
 
70,5 → 71,7
 
extern struct VersionInfo_t VersionInfo;
 
extern uint8_t PC_Connected;
extern uint8_t FC_Connected;
 
#endif //_UART_H_