| /trunk/analog.c |
|---|
| 67,7 → 67,7 |
| // 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 |
| PORTC &= ~((1<<PORTC3)|(1<<PORTC2)|(1<<PORTC1)|(1<<PORTC0)); |
| PORTC &= ~((1<<PORTC3)|(1<<PORTC2)|(1<<PORTC1)|(1<<PORTC0)); |
| // port PD5 and PD6 control the current direction of the test coils of the sensors |
| DDRD |= ((1<<DDD5)|(1<<DDD6)); |
| /trunk/led.c |
|---|
| 62,6 → 62,6 |
| // Set PD7 as output |
| DDRD |= (1<<DDD7); |
| // set port pon to GND |
| PORTD &= ~((1<<PORTD7)); |
| PORTD &= ~((1<<PORTD7)); |
| } |
| /trunk/led.h |
|---|
| 3,9 → 3,9 |
| #include <avr/io.h> |
| #define LED_GRN_ON PORTD |= (1<<PORTD7) |
| #define LED_GRN_OFF PORTD &= ~(1<<PORTD7) |
| #define LED_GRN_TOGGLE PORTD ^= (1<<PORTD7) |
| #define LED_GRN_ON PORTD |= (1<<PORTD7) |
| #define LED_GRN_OFF PORTD &= ~(1<<PORTD7) |
| #define LED_GRN_TOGGLE PORTD ^= (1<<PORTD7) |
| void LED_Init(void); |
| /trunk/main.c |
|---|
| 94,10 → 94,10 |
| Calibration_t Calibration; // calibration data in RAM |
| // magnet sensor variable |
| int16_t RawMagnet1a, RawMagnet1b; // raw magnet sensor data |
| int16_t RawMagnet1a, RawMagnet1b; // raw magnet sensor data |
| int16_t RawMagnet2a, RawMagnet2b; |
| int16_t RawMagnet3a, RawMagnet3b; |
| int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling |
| int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling |
| int16_t MagX = 0, MagY = 0, MagZ = 0; // rescaled magnetic field readings |
| // acceleration sensor variables |
| 165,7 → 165,7 |
| // blink code for normal operation |
| if(CheckDelay(Led_Timer)) |
| { |
| if(Calibration.Version != CALIBRATION_VERSION) LED_GRN_TOGGLE; |
| if(Calibration.Version != CALIBRATION_VERSION) LED_GRN_TOGGLE; |
| else LED_GRN_ON; |
| Led_Timer = SetDelay(150); |
| } |
| 189,20 → 189,20 |
| switch(AttitudeSource) |
| { |
| case ATTITUDE_SOURCE_I2C: |
| cli(); // stop interrupts |
| nick = I2C_WriteAttitude.Nick; |
| cli(); // stop interrupts |
| nick = I2C_WriteAttitude.Nick; |
| roll = I2C_WriteAttitude.Roll; |
| sei(); // start interrupts |
| sei(); // start interrupts |
| break; |
| case ATTITUDE_SOURCE_UART: |
| cli(); // stop interrupts |
| nick = ExternData.Attitude[NICK]; |
| roll = ExternData.Attitude[ROLL]; |
| sei(); // start interrupts |
| cli(); // stop interrupts |
| nick = ExternData.Attitude[NICK]; |
| roll = ExternData.Attitude[ROLL]; |
| sei(); // start interrupts |
| break; |
| case ATTITUDE_SOURCE_ACC: |
| nick = AccAttitudeNick; |
| roll = AccAttitudeRoll; |
| nick = AccAttitudeNick; |
| roll = AccAttitudeRoll; |
| break; |
| default: |
| nick = 0; |
| 210,8 → 210,8 |
| break; |
| } |
| nick_rad = ((double)nick) * M_PI / (double)(1800.0); |
| roll_rad = ((double)roll) * M_PI / (double)(1800.0); |
| nick_rad = ((double)nick) * M_PI / (double)(1800.0); |
| roll_rad = ((double)roll) * M_PI / (double)(1800.0); |
| // calculate attitude correction |
| Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
| 246,8 → 246,8 |
| 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(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte; |
| else cal = ExternData.CalState; |
| if(cal > 5) cal = 0; |
| 290,8 → 290,8 |
| Zmax = -10000; |
| Calibration.AccX.Offset = RawAccX; |
| Calibration.AccY.Offset = RawAccY; |
| Calibration.AccZ.Offset = RawAccZ; |
| invert_blinking = 0; |
| Calibration.AccZ.Offset = RawAccZ; |
| invert_blinking = 0; |
| break; |
| case 2: // 2nd step of calibration |
| 300,12 → 300,12 |
| if(UncalMagX > Xmax) Xmax = UncalMagX; |
| if(UncalMagY < Ymin) Ymin = UncalMagY; |
| if(UncalMagY > Ymax) Ymax = UncalMagY; |
| invert_blinking = 1; |
| invert_blinking = 1; |
| break; |
| case 3: // 3rd step of calibration |
| // used to change the orientation of the MK3MAG vertical to the horizontal plane |
| invert_blinking = 0; |
| invert_blinking = 0; |
| break; |
| case 4: |
| 312,7 → 312,7 |
| // find Min and Max of the Z-Sensor |
| if(UncalMagZ < Zmin) Zmin = UncalMagZ; |
| if(UncalMagZ > Zmax) Zmax = UncalMagZ; |
| invert_blinking = 1; |
| invert_blinking = 1; |
| break; |
| case 5: |
| 336,7 → 336,7 |
| blinkcount = 0; |
| } |
| } |
| invert_blinking = 0; |
| invert_blinking = 0; |
| break; |
| default: |
| 391,8 → 391,8 |
| DebugOut.Analog[23] = RawAccZ; |
| DebugOut.Analog[24] = Calibration.AccX.Offset; |
| DebugOut.Analog[25] = Calibration.AccY.Offset; |
| DebugOut.Analog[26] = Calibration.AccZ.Offset; |
| DebugOut.Analog[29] = AttitudeSource; |
| DebugOut.Analog[26] = Calibration.AccZ.Offset; |
| DebugOut.Analog[29] = AttitudeSource; |
| } |
| void AccMeasurement(void) |
| 417,34 → 417,34 |
| DDRC &=~((1<<DDC6)); |
| PORTC |= (1<<PORTC6); |
| LED_Init(); |
| TIMER0_Init(); |
| USART0_Init(); |
| ADC_Init(); |
| LED_Init(); |
| TIMER0_Init(); |
| USART0_Init(); |
| ADC_Init(); |
| I2C_Init(); |
| sei(); // enable globale interrupts |
| sei(); // enable globale interrupts |
| if(AccPresent) |
| { |
| if(AccPresent) |
| { |
| USART0_Print("ACC present\n"); |
| } |
| LED_GRN_ON; |
| LED_GRN_ON; |
| Debug_Timer = SetDelay(200); |
| Led_Timer = SetDelay(200); |
| Debug_Timer = SetDelay(200); |
| Led_Timer = SetDelay(200); |
| // read calibration info from eeprom |
| eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
| ExternData.CalState = 0; |
| I2C_WriteCal.CalByte = 0; |
| ExternData.CalState = 0; |
| I2C_WriteCal.CalByte = 0; |
| // main loop |
| while (1) |
| { |
| while (1) |
| { |
| FLIP_LOW; |
| Delay_ms(2); |
| RawMagnet1a = ADC_GetValue(MAG_X); |
| 467,7 → 467,7 |
| else CalcHeading(); |
| // check data from USART |
| USART0_ProcessRxData(); |
| USART0_ProcessRxData(); |
| if(NC_Connected) NC_Connected--; |
| if(FC_Connected) FC_Connected--; |
| 478,11 → 478,11 |
| Orientation = ORIENTATION_FC; |
| } |
| if(PC_Connected) |
| { |
| USART0_EnableTXD(); |
| USART0_TransmitTxData(); |
| PC_Connected--; |
| if(PC_Connected) |
| { |
| USART0_EnableTXD(); |
| USART0_TransmitTxData(); |
| PC_Connected--; |
| } |
| else |
| { |
| /trunk/main.h |
|---|
| 32,7 → 32,3 |
| #endif //_MAIN_H_ |
| /trunk/timer0.c |
|---|
| 76,9 → 76,9 |
| // Timer/Counter 0 Control Register A |
| // Normal Timer Counter Mode (Bits WGM02 = 0, WGM01 = 0, WGM00 = 0) |
| // OC0A disconnected (Bits COM0A1 = 0, COM0A0 = 0) |
| // OC0B disconnected (Bits COM0B1 = 0, COM0B0 = 0) |
| TCCR0A &= ~((1<<COM0A1)|(1<<COM0A0)|(1<<COM0B1)|(1<<COM0B0)|(1<<WGM01)|(1<<WGM00)); |
| // OC0A disconnected (Bits COM0A1 = 0, COM0A0 = 0) |
| // OC0B disconnected (Bits COM0B1 = 0, COM0B0 = 0) |
| TCCR0A &= ~((1<<COM0A1)|(1<<COM0A0)|(1<<COM0B1)|(1<<COM0B0)|(1<<WGM01)|(1<<WGM00)); |
| // Timer/Counter 0 Control Register B |
| 88,10 → 88,10 |
| // divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0) |
| TCCR0B &= ~((1<<FOC0A)|(1<<FOC0B)|(1<<WGM02)|(1<<CS02)); |
| TCCR0B = (1<<CS01)|(0<<CS00); |
| TCCR0B = (1<<CS01)|(0<<CS00); |
| // init Timer/Counter 0 Register |
| TCNT0 = 0; |
| TCNT0 = 0; |
| // Timer/Counter 0 Interrupt Mask Register |
| // enable timer overflow interrupt only |
| 148,7 → 148,7 |
| // ----------------------------------------------------------------------- |
| uint16_t SetDelay (uint16_t t) |
| { |
| return(CountMilliseconds + t - 1); |
| return(CountMilliseconds + t - 1); |
| } |
| // ----------------------------------------------------------------------- |
| 165,6 → 165,3 |
| while (!CheckDelay(t_stop)); |
| } |
| /trunk/timer0.h |
|---|
| 15,8 → 15,3 |
| #endif //_TIMER0_H |
| /trunk/twislave.c |
|---|
| 153,7 → 153,7 |
| case TW_SR_DATA_ACK: // data has been received and ack has been returned |
| data = TWDR; |
| if (Rx_Idx == 0xFF) |
| { // if the fisrt byte after slave addressing was received |
| { // if the first byte after slave addressing was received |
| switch(data) |
| { |
| case I2C_CMD_VERSION: |
| /trunk/twislave.h |
|---|
| 33,7 → 33,7 |
| { |
| int16_t Nick; |
| int16_t Roll; |
| } I2C_WriteAttitude_t; |
| } I2C_WriteAttitude_t; |
| typedef struct |
| /trunk/uart.c |
|---|
| 62,6 → 62,10 |
| #include "timer0.h" |
| #include "twislave.h" |
| // slave addresses |
| #define FC_ADDRESS 1 |
| #define NC_ADDRESS 2 |
| #define MK3MAG_ADDRESS 3 |
| #define FALSE 0 |
| #define TRUE 1 |
| 76,11 → 80,14 |
| volatile uint8_t txd_complete = TRUE; |
| volatile uint8_t ReceivedBytes = 0; |
| #define VERSION_INFO 0x01 |
| #define DEBUG_DATA 0x02 |
| #define DEBUG_LABEL 0x04 |
| #define COMPASS_HEADING 0x08 |
| // send flags |
| #define RQST_VERSION_INFO 0x01 |
| #define RQST_DEBUG_DATA 0x02 |
| #define RQST_DEBUG_LABEL 0x04 |
| #define RQST_COMPASS_HEADING 0x08 |
| #define RQST_EXTERN_CTRL 0x10 |
| uint8_t RequestFlags = 0x00; |
| uint8_t RequestDebugLabel = 0; |
| 87,7 → 94,7 |
| uint8_t PC_Connected = 0; |
| uint8_t FC_Connected = 0; |
| uint8_t MySlaveAddr = 0; |
| uint8_t MySlaveAddr = MK3MAG_ADDRESS; |
| DebugOut_t DebugOut; |
| 99,39 → 106,39 |
| const uint8_t ANALOG_LABEL[32][16] = |
| { |
| //1234567890123456 |
| "Magnet X ", //0 |
| "Magnet Y ", |
| "Magnet Z ", |
| "RawMagnet X ", |
| "RawMagnet Y ", |
| "RawMagnet Z ", //5 |
| "Attitude Nick ", |
| "Attitude Roll ", |
| "Magnet X Offset ", |
| "Magnet X Range ", |
| "Magnet Y Offset ", //10 |
| "Magnet Y Range ", |
| "Magnet Z Offset ", |
| "Magnet Z Range ", |
| "Calstate ", |
| "Heading ", //15 |
| "User0 ", |
| "User1 ", |
| "Acc X ", |
| "Acc Y ", |
| "Acc Z ", //20 |
| "RawAcc X ", |
| "RawAcc Y ", |
| "RawAcc Z ", |
| "Acc X Offset ", |
| "Acc Y Offset ", //25 |
| "Acc Z Offset ", |
| "Analog27 ", |
| "Analog28 ", |
| "Analog29 ", |
| "I2C Error ", //30 |
| "I2C Okay " |
| //1234567890123456 |
| "Magnet X ", //0 |
| "Magnet Y ", |
| "Magnet Z ", |
| "RawMagnet X ", |
| "RawMagnet Y ", |
| "RawMagnet Z ", //5 |
| "Attitude Nick ", |
| "Attitude Roll ", |
| "Magnet X Offset ", |
| "Magnet X Range ", |
| "Magnet Y Offset ", //10 |
| "Magnet Y Range ", |
| "Magnet Z Offset ", |
| "Magnet Z Range ", |
| "Calstate ", |
| "Heading ", //15 |
| "User0 ", |
| "User1 ", |
| "Acc X ", |
| "Acc Y ", |
| "Acc Z ", //20 |
| "RawAcc X ", |
| "RawAcc Y ", |
| "RawAcc Z ", |
| "Acc X Offset ", |
| "Acc Y Offset ", //25 |
| "Acc Z Offset ", |
| "Analog27 ", |
| "Analog28 ", |
| "Analog29 ", |
| "I2C Error ", //30 |
| "I2C Okay " |
| }; |
| 165,7 → 172,7 |
| UBRR0H = (uint8_t)(ubrr >> 8); |
| UBRR0L = (uint8_t)ubrr; |
| // USART0 Control and Status Register A, B, C |
| // USART0 Control and Status Register A, B, C |
| // enable double speed operation |
| UCSR0A |= (1 << U2X0); |
| 201,10 → 208,11 |
| VersionInfo.Major = VERSION_MAJOR; |
| VersionInfo.Minor = VERSION_MINOR; |
| VersionInfo.PCCompatible = VERSION_COMPATIBLE; |
| VersionInfo.PCCompatible = VERSION_COMPATIBLE; |
| MySlaveAddr = MK3MAG_ADDRESS; |
| // Version beim Start ausgeben (nicht schön, aber geht... ) |
| // Version beim Start ausgeben (nicht schön, aber geht... ) |
| USART0_putchar ('\n'); |
| USART0_putchar ('C'); |
| USART0_putchar ('P'); |
| 420,14 → 428,15 |
| switch(rxd_buffer[2]) |
| { |
| case 'w':// Attitude |
| case 'w':// Attitude info from FC |
| Decode64((uint8_t *) &ExternData, sizeof(ExternData), 3, ReceivedBytes); |
| RequestFlags |= COMPASS_HEADING; |
| RequestFlags |= RQST_COMPASS_HEADING; |
| AttitudeSource = ATTITUDE_SOURCE_UART; |
| Orientation = ExternData.Orientation; |
| FC_Connected = 255; |
| break; |
| // used only for debug proposes at serial port |
| case 'b': // extern control |
| case 'c': // extern control with debug request |
| Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3,ReceivedBytes); |
| 442,17 → 451,22 |
| ExternData.CalState++; |
| if(ExternData.CalState == 6) ExternData.CalState = 0; |
| } |
| //ExternData.Attitude[0] = ExternControl.Par1; |
| //ExternData.Attitude[1] = ExternControl.Par2; |
| PC_Connected = 255; |
| PC_Connected = 255; |
| break; |
| // get debug values |
| case 'f': |
| RequestFlags |= RQST_DEBUG_DATA; |
| PC_Connected = 255; |
| break; |
| // no reaction for display line request |
| case 'h':// x-1 display columns |
| PC_Connected = 255; |
| break; |
| case 'v': // get version and board release |
| RequestFlags |= VERSION_INFO; |
| RequestFlags |= RQST_VERSION_INFO; |
| PC_Connected = 255; |
| break; |
| 459,12 → 473,12 |
| case 'a':// Labels of the Analog Debug outputs |
| Decode64((uint8_t *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes); |
| RequestDebugLabel = tmp_char_arr2[0]; |
| RequestFlags |= DEBUG_LABEL; |
| RequestFlags |= RQST_DEBUG_LABEL; |
| PC_Connected = 255; |
| break; |
| case 'g':// get debug data |
| RequestFlags |= DEBUG_DATA; |
| case 'g':// get extern control data |
| RequestFlags |= RQST_EXTERN_CTRL; |
| PC_Connected = 255; |
| break; |
| } |
| 481,39 → 495,40 |
| if(!txd_complete) return; |
| if(CheckDelay(Debug_Timer)) |
| { |
| if(CheckDelay(Debug_Timer) || (RequestFlags & RQST_DEBUG_DATA)) |
| { |
| SetDebugValues(); |
| SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut)); |
| Debug_Timer = SetDelay(250); |
| } |
| RequestFlags &= ~RQST_DEBUG_DATA; |
| } |
| if(RequestFlags & DEBUG_LABEL) |
| { |
| if(RequestFlags & RQST_DEBUG_LABEL) |
| { |
| SendOutData('A',RequestDebugLabel + '0',(uint8_t *) ANALOG_LABEL[RequestDebugLabel],16); |
| RequestDebugLabel = 255; |
| RequestFlags &= ~DEBUG_LABEL; |
| RequestFlags &= ~RQST_DEBUG_LABEL; |
| Debug_Timer = SetDelay(500); |
| } |
| if(RequestFlags & VERSION_INFO) |
| { |
| if(RequestFlags & RQST_VERSION_INFO) |
| { |
| SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo, sizeof(VersionInfo)); |
| RequestFlags &= ~VERSION_INFO; |
| } |
| RequestFlags &= ~RQST_VERSION_INFO; |
| } |
| if(RequestFlags & DEBUG_DATA) |
| if(RequestFlags & RQST_EXTERN_CTRL) |
| { |
| SetDebugValues(); |
| SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl)); |
| RequestFlags &= ~DEBUG_DATA; |
| RequestFlags &= ~RQST_EXTERN_CTRL; |
| } |
| if(RequestFlags & COMPASS_HEADING) |
| if(RequestFlags & RQST_COMPASS_HEADING) |
| { |
| SendOutData('K',MySlaveAddr,(uint8_t *) &I2C_Heading, sizeof(I2C_Heading)); |
| RequestFlags &= ~COMPASS_HEADING; |
| } |
| RequestFlags &= ~RQST_COMPASS_HEADING; |
| } |
| } |
| 520,8 → 535,8 |
| void USART0_Print(int8_t *msg) |
| { |
| uint8_t i = 0; |
| while(msg[i] != 0) |
| { |
| USART0_putchar(msg[i++]); |
| } |
| while(msg[i] != 0) |
| { |
| USART0_putchar(msg[i++]); |
| } |
| } |