/branches/MK3Mag V0.14 Code Redesign Killagreg/analog.c |
---|
54,19 → 54,14 |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <util/delay.h> |
//#include <stdlib.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 as tri state |
// set PortC 0,1,2,3 and 6,7 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 |
83,22 → 78,6 |
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)); |
} |
105,11 → 84,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,17 → 21,6 |
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,7 → 68,6 |
AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC; |
Orientation_t Orientation = ORIENTATION_FC; |
uint16_t Led_Timer = 0; |
86,7 → 85,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 |
96,16 → 95,16 |
int16_t RawMagnet2a, RawMagnet2b; |
int16_t RawMagnet3a, RawMagnet3b; |
int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling |
int16_t MagX = 0, MagY = 0, MagZ = 0; // rescaled magnetic field readings |
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; |
// 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; |
int16_t Heading = -1; // the current compass heading in deg |
void CalcFields(void) |
{ |
UncalMagX = (RawMagnet1a - RawMagnet1b); |
118,46 → 117,12 |
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 = 0.0, Cy = 0.0, Cz = 0.0; |
double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz; |
int16_t heading = -1; |
// blink code for normal operation |
167,20 → 132,15 |
Led_Timer = SetDelay(500); |
} |
switch(Orientation) |
{ |
case ORIENTATION_NC: |
Cx = MagX; |
Cy = MagY; |
Cz = MagZ; |
break; |
Cx = MagX; |
Cy = MagY; |
Cz = MagZ; |
case ORIENTATION_FC: |
// rotation of 90 deg compared to NC setup |
Cx = MagY; |
Cy = -MagX; |
Cz = MagZ; |
break; |
if(ExternData.Orientation == 1) |
{ |
Cx = MagX; |
Cy = -MagY; |
Cz = MagZ; |
} |
// calculate nick and roll angle in rad |
197,14 → 157,22 |
break; |
case ATTITUDE_SOURCE_ACC: |
nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0); |
roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0); |
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); |
} |
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 |
211,10 → 179,6 |
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 |
305,7 → 269,7 |
// Save values |
if(cal != calold) // avoid continously writing of eeprom! |
{ |
Calibration.MagX.Range = Xmax - Xmin; |
Calibration.MagY.Range = Xmax - Xmin; |
Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
Calibration.MagY.Range = Ymax - Ymin; |
Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
316,9 → 280,12 |
// indicate write process by setting the led |
LED_GRN_ON; |
eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
Led_Timer = SetDelay(2000); |
Delay_ms(2000); |
// reset led state |
LED_GRN_OFF; |
// reset blinkcode |
blinkcount = 0; |
Led_Timer = SetDelay(1000); |
} |
} |
break; |
341,8 → 308,7 |
switch(AttitudeSource) |
{ |
case ATTITUDE_SOURCE_ACC: |
DebugOut.Analog[6] = AccAttitudeNick; |
DebugOut.Analog[7] = AccAttitudeRoll; |
break; |
case ATTITUDE_SOURCE_UART: |
375,7 → 341,6 |
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) |
382,9 → 347,9 |
{ |
if(AccPresent) |
{ |
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; |
RawAccX = (RawAccX + (int16_t)ADC_GetValue(ADC2))/2; |
RawAccY = (RawAccY + (int16_t)ADC_GetValue(ADC3))/2; |
RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ADC6))/2; |
} |
else |
{ |
392,8 → 357,13 |
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 |
408,11 → 378,6 |
sei(); // enable globale interrupts |
if(AccPresent) |
{ |
USART0_Print("ACC present\n"); |
} |
LED_GRN_ON; |
Debug_Timer = SetDelay(200); |
421,6 → 386,7 |
// read calibration info from eeprom |
eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
ExternData.Orientation = 0; |
ExternData.CalState = 0; |
I2C_WriteCal.CalByte = 0; |
430,17 → 396,17 |
{ |
FLIP_LOW; |
Delay_ms(2); |
RawMagnet1a = ADC_GetValue(MAG_X); |
RawMagnet2a = -ADC_GetValue(MAG_Y); |
RawMagnet3a = ADC_GetValue(MAG_Z); |
RawMagnet1a = ADC_GetValue(ADC0); |
RawMagnet2a = -ADC_GetValue(ADC1); |
RawMagnet3a = ADC_GetValue(ADC7); |
AccMeasurement(); |
Delay_ms(1); |
FLIP_HIGH; |
Delay_ms(2); |
RawMagnet1b = ADC_GetValue(MAG_X); |
RawMagnet2b = -ADC_GetValue(MAG_Y); |
RawMagnet3b = ADC_GetValue(MAG_Z); |
RawMagnet1b = ADC_GetValue(ADC0); |
RawMagnet2b = -ADC_GetValue(ADC1); |
RawMagnet3b = ADC_GetValue(ADC7); |
AccMeasurement(); |
Delay_ms(1); |
451,16 → 417,8 |
// 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,6 → 6,9 |
#include <inttypes.h> |
#define SYSCLK 8000000L //Quarz Frequenz in Hz |
typedef enum |
{ |
ATTITUDE_SOURCE_I2C, |
14,17 → 17,9 |
} AttitudeSource_t; |
typedef enum |
{ |
ORIENTATION_NC = 0, |
ORIENTATION_FC = 1 |
} Orientation_t; |
extern uint8_t PC_Connected; |
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,12 → 1,9 |
#-------------------------------------------------------------------- |
# MCU name |
MCU = atmega168 |
F_CPU = 8000000 |
#------------------------------------------------------------------- |
BOARD = 10 |
#BOARD = 11 |
VERSION_MAJOR = 0 |
VERSION_MINOR = 16 |
VERSION_MINOR = 14 |
VERSION_COMPATIBLE = 8 # PC-Kompatibilität |
#------------------------------------------------------------------- |
42,8 → 39,6 |
EXTRAINCDIRS = |
CDEFS = -DF_CPU=$(F_CPU)UL |
# Optional compiler flags. |
# -g: generate debugging information (for GDB, or for COFF conversion) |
# -O*: optimization level |
65,9 → 60,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 &= ~(1<<PORTB2); |
PORTB &= ~(PORTB2); |
cmps_cnt = 0; |
} |
else |
123,7 → 123,7 |
if(++cmps_cnt >= 380) |
{ |
// set PWM out to high |
PORTB |= (1<<PORTB2); |
PORTB |= PORTB2; |
// reset periode counter |
cmps_cnt = 0; |
} |
131,7 → 131,7 |
else if(cmps_cnt >= (Heading + 10)) |
{ |
// set PWM out to low |
PORTB &= ~(1<<PORTB2); |
PORTB &= ~(PORTB2); |
} |
} |
/branches/MK3Mag V0.14 Code Redesign Killagreg/twislave.c |
---|
70,7 → 70,6 |
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; |
188,8 → 187,6 |
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,13 → 48,12 |
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,9 → 66,8 |
#define FALSE 0 |
#define TRUE 1 |
// keep buffers as small as possible |
#define TXD_BUFFER_LEN 100 |
#define RXD_BUFFER_LEN 30 |
#define RXD_BUFFER_LEN 100 |
volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
volatile uint8_t rxd_buffer_locked = FALSE; |
84,8 → 83,6 |
uint8_t RequestFlags = 0x00; |
uint8_t RequestDebugLabel = 0; |
uint8_t PC_Connected = 0; |
uint8_t FC_Connected = 0; |
uint8_t MySlaveAddr = 0; |
140,7 → 137,7 |
/****************************************************************/ |
void USART0_Init (void) |
{ |
uint16_t ubrr = (uint16_t) ((uint32_t) F_CPU/(8 * BAUD_RATE) - 1); |
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * BAUD_RATE) - 1); |
// disable all interrupts before configuration |
cli(); |
205,16 → 202,15 |
// Version beim Start ausgeben (nicht schön, aber geht... ) |
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'); |
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'); |
} |
// --------------------------------------------------------------------------------- |
397,11 → 393,11 |
// -------------------------------------------------------------------------- |
int16_t USART0_putchar (int8_t c) |
int uart_putchar (int8_t c) |
{ |
// if tx is not enabled return immediatly |
if(!(UCSR0B & (1 << TXEN0))) return (0); |
if (c == '\n') USART0_putchar('\r'); |
if (c == '\n') uart_putchar('\r'); |
// wait until previous character was send |
loop_until_bit_is_set(UCSR0A, UDRE0); |
// send character |
422,10 → 418,10 |
{ |
case 'w':// Attitude |
Decode64((uint8_t *) &ExternData, sizeof(ExternData), 3, ReceivedBytes); |
I2C_WriteAttitude.Nick = ExternData.Attitude[NICK]; |
I2C_WriteAttitude.Nick = ExternData.Attitude[ROLL]; |
AttitudeSource = ATTITUDE_SOURCE_UART; |
RequestFlags |= COMPASS_HEADING; |
AttitudeSource = ATTITUDE_SOURCE_UART; |
Orientation = ExternData.Orientation; |
FC_Connected = 255; |
break; |
case 'b': // extern control |
514,13 → 510,3 |
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,8 → 18,7 |
void USART0_DisableTXD(void); |
void USART0_TransmitTxData(void); |
void USART0_ProcessRxData(void); |
int16_t USART0_putchar(int8_t c); |
void USART0_Print(int8_t *msg); |
int uart_putchar (int8_t c); |
71,7 → 70,5 |
extern struct VersionInfo_t VersionInfo; |
extern uint8_t PC_Connected; |
extern uint8_t FC_Connected; |
#endif //_UART_H_ |