/trunk/GPS.c |
---|
216,3 → 216,8 |
{ |
return; |
} |
void CalcHeadFree(void) |
{ |
return; |
} |
/trunk/Navi-Ctrl.Uv2 |
---|
43,6 → 43,7 |
File 1,1,<.\params.c><params.c> |
File 1,1,<.\printf_P.c><printf_P.c> |
File 1,1,<.\GPS.c><GPS.c> |
File 1,1,<.\mk3mag.c><mk3mag.c> |
File 2,5,<.\ramfunc.h><ramfunc.h> |
File 2,5,<.\main.h><main.h> |
File 2,5,<.\menu.h><menu.h> |
77,6 → 78,7 |
File 2,5,<.\libstr91x\include\91x_conf.h><91x_conf.h> |
File 2,5,<.\libstr91x\include\91x_it.h><91x_it.h> |
File 2,5,<.\printf_P.h><printf_P.h> |
File 2,5,<.\mk3mag.h><mk3mag.h> |
File 3,2,<.\startup912.s><startup912.s> |
File 4,1,<.\libstr91x\src\91x_scu.c><91x_scu.c> |
File 4,1,<.\libstr91x\src\91x_gpio.c><91x_gpio.c> |
/trunk/i2c.c |
---|
60,41 → 60,21 |
#include "uart1.h" |
#include "timer1.h" |
#include "config.h" |
#include "main.h" |
#include "led.h" |
#include "spi_slave.h" |
#define I2C_SLAVE_ADDRESS 0x50 |
volatile u8 I2C_State = I2C_STATE_OFF; // only one byte, because of sync by nesting irqs |
// I2C states |
#define I2C_UNDEF 0 |
#define I2C_IDLE 1 |
#define I2C_TX_PROGRESS 2 |
#define I2C_RX_PENDING 3 |
#define I2C_RX_PROGRESS 4 |
#define I2C_OFF 5 |
volatile u8 I2C_State = I2C_OFF; // only on byte! because of sync by nesting irqs |
u8 I2C_StopPolling = 1; |
// rxbuffer |
volatile u8 I2C_RxBufferSize; |
volatile u8 *I2C_RxBuffer; |
volatile u8 I2C_RxBuffer[I2C_RXBUFFER_LEN]; |
// txbuffer |
volatile u8 I2C_TxBufferSize; |
volatile u8 *I2C_TxBuffer; |
volatile u8 I2C_TxBuffer[I2C_TXBUFFER_LEN]; |
volatile u8 I2C_Direction; |
volatile u8 I2C_Command; |
// I2C Transfer buffers |
volatile I2C_Heading_t I2C_Heading; |
volatile I2C_WriteAttitude_t I2C_WriteAttitude; |
volatile I2C_Mag_t I2C_Mag; |
volatile I2C_Version_t MK3MAG_Version; |
volatile I2C_Cal_t I2C_WriteCal; |
volatile I2C_Cal_t I2C_ReadCal; |
volatile u8 I2C_SlaveAddress = 0x00; |
I2C_pRxHandler_t I2C_pRxHandler = NULL; |
#define I2C1_TIMEOUT 500 // 500 ms |
volatile u32 I2C1_Timeout = 0; |
//-------------------------------------------------------------- |
103,7 → 83,7 |
I2C_InitTypeDef I2C_Struct; |
GPIO_InitTypeDef GPIO_InitStructure; |
I2C_State = I2C_OFF; |
I2C_State = I2C_STATE_OFF; |
UART1_PutString("\r\n I2C init..."); |
// enable Port 2 peripherie |
176,10 → 156,8 |
I2C_Struct.I2C_OwnAddress = 0x00; |
I2C_Init(I2C1, &I2C_Struct); |
I2C_TxBuffer = NULL; |
// empty rx and tx buffer |
I2C_TxBufferSize = 0; |
I2C_RxBuffer = NULL; |
I2C_RxBufferSize = 0; |
I2C_Cmd(I2C1, ENABLE); |
186,14 → 164,11 |
I2C_ITConfig(I2C1, ENABLE); |
VIC_Config(I2C1_ITLine, VIC_IRQ , PRIORITY_I2C1); |
I2C_Heading.Heading = -1; |
I2C1_Timeout = SetDelay(10*I2C1_TIMEOUT); |
I2C_GenerateSTOP(I2C1, ENABLE); |
I2C_State = I2C_IDLE; |
I2C_State = I2C_STATE_IDLE; |
I2C_StopPolling = 0; // start polling |
UART1_PutString("ok"); |
} |
202,12 → 177,11 |
void I2C1_Deinit(void) |
{ |
GPIO_InitTypeDef GPIO_InitStructure; |
I2C_StopPolling = 1;// stop polling |
UART1_PutString("\r\n I2C deinit..."); |
I2C_GenerateStart(I2C1, DISABLE); |
I2C_GenerateSTOP(I2C1, ENABLE); |
VIC_ITCmd(I2C1_ITLine, DISABLE); |
I2C_State = I2C_OFF; |
I2C_State = I2C_STATE_OFF; |
I2C_ITConfig(I2C1, DISABLE); |
I2C_Cmd(I2C1, DISABLE); |
I2C_DeInit(I2C1); |
223,14 → 197,11 |
GPIO_InitStructure.GPIO_Alternate = GPIO_InputAlt1; |
GPIO_Init(GPIO2, &GPIO_InitStructure); |
I2C_TxBuffer = NULL; |
// empty rx and tx buffer |
I2C_TxBufferSize = 0; |
I2C_RxBuffer = NULL; |
I2C_RxBufferSize = 0; |
I2C1_Timeout = SetDelay(10*I2C1_TIMEOUT); |
I2C_Heading.Heading = -1; |
UART1_PutString("ok"); |
} |
239,8 → 210,7 |
//-------------------------------------------------------------- |
void I2C1_IRQHandler(void) |
{ |
static u8 Rx_Idx = 0, Tx_Idx = 0, crc = 0; |
static u8 I2C_PrimRxBuffer[10]; // must be larger than any of the secondary rx buffers |
static u8 Rx_Idx = 0, Tx_Idx = 0; |
u16 status; |
//IENABLE; // do not enable IRQ nesting for I2C!!!! |
255,7 → 225,7 |
I2C_GenerateSTOP (I2C1, ENABLE); // free the bus |
I2C_GenerateSTOP (I2C1, DISABLE); // free the bus |
} |
I2C_State = I2C_IDLE; |
I2C_State = I2C_STATE_IDLE; |
VIC_ITCmd(I2C1_ITLine, DISABLE); |
LED_GRN_OFF; |
return; |
262,30 → 232,31 |
} |
else |
{ // depending on current i2c state |
switch (status) |
switch(status) |
{ |
// the start condition was initiated on the bus |
case I2C_EVENT_MASTER_MODE_SELECT: |
LED_GRN_ON; |
// update current bus state variable |
// jump to rx state if there is nothing to send |
switch(I2C_Direction) |
{ |
case I2C_MODE_TRANSMITTER: |
I2C_State = I2C_TX_PROGRESS; |
I2C_State = I2C_STATE_TX_PROGRESS; |
break; |
case I2C_MODE_RECEIVER: |
if ((I2C_RxBuffer == NULL) || (I2C_RxBufferSize == 0)) |
if (I2C_RxBufferSize == 0) // nothingto send? |
{ |
I2C_GenerateSTOP (I2C1, ENABLE); |
VIC_ITCmd(I2C1_ITLine, DISABLE); |
LED_GRN_OFF; |
I2C_State = I2C_IDLE; |
I2C_State = I2C_STATE_IDLE; |
return; |
} |
else |
{ |
I2C_State = I2C_RX_PROGRESS; |
I2C_State = I2C_STATE_RX_PROGRESS; |
} |
break; |
293,13 → 264,13 |
I2C_GenerateSTOP (I2C1, ENABLE); |
VIC_ITCmd(I2C1_ITLine, DISABLE); |
LED_GRN_OFF; |
I2C_State = I2C_IDLE; |
I2C_State = I2C_STATE_IDLE; |
return; |
} |
// enable acknowledge |
I2C_AcknowledgeConfig (I2C1, ENABLE); |
// send address/direction byte on the bus |
I2C_Send7bitAddress(I2C1, I2C_SLAVE_ADDRESS, I2C_Direction); |
I2C_Send7bitAddress(I2C1, I2C_SlaveAddress, I2C_Direction); |
break; |
// the address byte was send |
306,21 → 277,21 |
case I2C_EVENT_MASTER_MODE_SELECTED: |
// Clear EV6 by set again the PE bit |
I2C_Cmd(I2C1, ENABLE); |
// reset checksum |
crc = 0; |
switch(I2C_State) |
{ |
case I2C_TX_PROGRESS: |
// send command 1st data byte (allways the command id) |
I2C_SendData(I2C1, I2C_Command); |
crc += I2C_Command; |
case I2C_STATE_TX_PROGRESS: |
// send 1st data byte |
Tx_Idx = 0; |
I2C_SendData(I2C1, I2C_TxBuffer[Tx_Idx]); |
Tx_Idx++; |
// reset timeout |
I2C1_Timeout = SetDelay(I2C1_TIMEOUT); // after inactivity the I2C1 bus will be reset |
break; |
case I2C_RX_PROGRESS: |
case I2C_STATE_RX_PROGRESS: |
Rx_Idx = 0; |
// disable acknoledge if only one byte has to be read |
if(I2C_RxBufferSize == 1) I2C_AcknowledgeConfig (I2C1, DISABLE); |
break; |
default: // unknown I2C state |
328,7 → 299,7 |
I2C_GenerateSTOP (I2C1, ENABLE); |
LED_GRN_OFF; |
VIC_ITCmd(I2C1_ITLine, DISABLE); |
I2C_State = I2C_IDLE; |
I2C_State = I2C_STATE_IDLE; |
return; |
break; |
} |
340,30 → 311,13 |
// some bytes have to be transmitted |
if(Tx_Idx < I2C_TxBufferSize) |
{ |
if(I2C_TxBuffer != NULL) |
{ |
I2C_SendData(I2C1, I2C_TxBuffer[Tx_Idx]); |
crc += I2C_TxBuffer[Tx_Idx]; |
} |
else |
{ |
I2C_SendData(I2C1, 0x00); |
} |
I2C_SendData(I2C1, I2C_TxBuffer[Tx_Idx]); |
Tx_Idx++; |
} |
else if(Tx_Idx == I2C_TxBufferSize) // the last tx buffer byte was send |
{ |
// send crc byte at the end |
crc = ~crc; // flip all bits in the checksum |
I2C_SendData(I2C1, crc); |
} |
else if(Tx_Idx == (I2C_TxBufferSize+1) ) |
{ |
I2C_SendData(I2C1, 0xAA); // send a dummybyte |
} |
else // last byte was send |
{ |
// generate stop or repeated start condition |
if ((I2C_RxBuffer != NULL) && (I2C_RxBufferSize > 0)) // is any answer byte expected? |
if (I2C_RxBufferSize > 0) // is any answer byte expected? |
{ |
I2C_Direction = I2C_MODE_RECEIVER; // switch to master receiver after repeated start condition |
I2C_GenerateStart(I2C1, ENABLE); // initiate repeated start condition on the bus |
373,193 → 327,79 |
I2C_GenerateSTOP(I2C1, ENABLE); // generate stop condition to free the bus |
VIC_ITCmd(I2C1_ITLine, DISABLE); |
LED_GRN_OFF; |
DebugOut.Analog[15]++; |
I2C_State = I2C_IDLE; // ready for new actions |
I2C_State = I2C_STATE_IDLE; // ready for new actions |
} |
} |
Tx_Idx++; |
break; |
// the master has received a byte from the slave |
case I2C_EVENT_MASTER_BYTE_RECEIVED: |
// some bytes have to be received |
if (Rx_Idx < I2C_RxBufferSize) |
if ( Rx_Idx+1 < I2C_RxBufferSize) |
{ // copy received byte from the data register to the rx-buffer |
I2C_PrimRxBuffer[Rx_Idx] = I2C_ReceiveData(I2C1); |
// update checksum |
crc += I2C_PrimRxBuffer[Rx_Idx]; |
I2C_RxBuffer[Rx_Idx] = I2C_ReceiveData(I2C1); |
} |
// if the last byte (crc) was received |
else if ( Rx_Idx == I2C_RxBufferSize) |
else // if the last byte was received |
{ |
// generate a STOP condition on the bus before reading data register |
I2C_GenerateSTOP(I2C1, ENABLE); |
// compare last byte with checksum |
crc = ~crc;// flip all bits in calulated checksum |
if(crc == I2C_ReceiveData(I2C1)) |
{ |
// copy primary rx buffer content to rx buffer if exist |
if(I2C_RxBuffer != NULL) |
{ |
memcpy((u8 *)I2C_RxBuffer, (u8 *)I2C_PrimRxBuffer, I2C_RxBufferSize); |
} |
I2C1_Timeout = SetDelay(I2C1_TIMEOUT); |
DebugOut.Analog[15]++; |
} |
else // checksum error detected |
{ |
DebugOut.Analog[14]++; |
} |
I2C_RxBuffer[Rx_Idx] = I2C_ReceiveData(I2C1); |
// call the rx handler function to process recieved data |
if(I2C_pRxHandler != NULL) (*I2C_pRxHandler)((u8*)I2C_RxBuffer, I2C_RxBufferSize); |
I2C1_Timeout = SetDelay(I2C1_TIMEOUT); |
DebugOut.Analog[15]++; |
VIC_ITCmd(I2C1_ITLine, DISABLE); |
LED_GRN_OFF; |
I2C_State = I2C_IDLE; |
I2C_State = I2C_STATE_IDLE; |
return; |
} |
Rx_Idx++; |
// if the 2nd last byte was received disable acknowledge for the last one |
if ( Rx_Idx == I2C_RxBufferSize ) |
if ( (Rx_Idx + 1) == I2C_RxBufferSize ) |
{ |
I2C_AcknowledgeConfig (I2C1, DISABLE); |
I2C_AcknowledgeConfig(I2C1, DISABLE); |
} |
break; |
default: |
default:// unknown event |
// should never happen |
I2C_GenerateSTOP (I2C1, ENABLE); |
LED_GRN_OFF; |
VIC_ITCmd(I2C1_ITLine, DISABLE); |
I2C_State = I2C_STATE_IDLE; |
break; |
} |
} |
//IDISABLE; // do not enable IRQ nesting for I2C!!!! |
} |
//---------------------------------------------------------------- |
void I2C1_SendCommand(u8 command) |
// ---------------------------------------------------------------------------------------- |
// initate an i2c transmission |
u8 I2C_Transmission(u8 SlaveAddr, I2C_pRxHandler_t pRxHandler, u8 RxBytes) |
{ |
// disable I2C IRQ to check state |
VIC_ITCmd(I2C1_ITLine, DISABLE); |
// If I2C transmission is in progress |
if(I2C_State == I2C_IDLE) |
u8 retval = 0; |
if(I2C_State == I2C_STATE_IDLE) |
{ |
// update current command id |
I2C_Command = command; |
// set pointers to data area with respect to the command id |
switch (command) |
I2C_RxBufferSize = RxBytes; |
// set direction to master transmitter |
if( (I2C_TxBufferSize > 0) && (I2C_TxBufferSize < I2C_TXBUFFER_LEN) ) I2C_Direction = I2C_MODE_TRANSMITTER; |
else if (( I2C_RxBufferSize > 0 ) && (I2C_RxBufferSize < I2C_RXBUFFER_LEN) ) I2C_Direction = I2C_MODE_RECEIVER; |
else // nothing to send or receive |
{ |
case I2C_CMD_VERSION: |
I2C_RxBuffer = (u8 *)&MK3MAG_Version; |
I2C_RxBufferSize = sizeof(MK3MAG_Version); |
I2C_TxBuffer = NULL; |
I2C_TxBufferSize = 0; |
break; |
case I2C_CMD_WRITE_CAL: |
I2C_RxBuffer = (u8 *)&I2C_ReadCal; |
I2C_RxBufferSize = sizeof(I2C_ReadCal); |
I2C_TxBuffer = (u8 *)&I2C_WriteCal; |
I2C_TxBufferSize = sizeof(I2C_WriteCal); |
break; |
case I2C_CMD_READ_MAG: |
I2C_RxBuffer = (u8 *)&I2C_Mag; |
I2C_RxBufferSize = sizeof(I2C_Mag); |
I2C_TxBuffer = NULL; |
I2C_TxBufferSize = 0; |
break; |
case I2C_CMD_READ_HEADING: |
I2C_RxBuffer = (u8 *)&I2C_Heading; |
I2C_RxBufferSize = sizeof(I2C_Heading); |
I2C_TxBuffer = (u8 *)&I2C_WriteAttitude; |
I2C_TxBufferSize = sizeof(I2C_WriteAttitude); |
// update attitude from spi rx buffer |
VIC_ITCmd(SSP0_ITLine, DISABLE); // avoid spi buffer update during copy |
I2C_WriteAttitude.Roll = FromFlightCtrl.AngleRoll; |
I2C_WriteAttitude.Nick = FromFlightCtrl.AngleNick; |
VIC_ITCmd(SSP0_ITLine, ENABLE); |
break; |
default: // unknown command id |
I2C_RxBuffer = NULL; |
I2C_RxBufferSize = 0; |
I2C_TxBuffer = NULL; |
I2C_TxBufferSize = 0; |
break; |
I2C_TxBufferSize = 0; |
I2C_RxBufferSize = 0; |
// enable I2C IRQ again |
VIC_ITCmd(I2C1_ITLine, ENABLE); |
return(retval); |
} |
// set direction to master transmitter |
I2C_Direction = I2C_MODE_TRANSMITTER; |
// update slave address and rx data handler funbction pointer |
I2C_SlaveAddress = SlaveAddr; |
I2C_pRxHandler = pRxHandler; |
// test on busy flag and clear it |
I2C_CheckEvent( I2C1, I2C_FLAG_BUSY ); |
// enable I2C IRQ again |
VIC_ITCmd(I2C1_ITLine, ENABLE); |
// initiate start condition on the bus |
I2C_GenerateStart(I2C1, ENABLE); |
// to be continued in the I2C1_IRQHandler() above |
} // EOF I2C_State == I2C_IDLE |
else // I2C_State != I2C_IDLE |
{ |
// re-enable I2C IRQ again |
VIC_ITCmd(I2C1_ITLine, ENABLE); |
} |
retval = 1; |
} |
return(retval); |
} |
//---------------------------------------------------------------- |
void I2C1_GetMK3MagVersion(void) |
{ |
u8 msg[64]; |
u8 repeat; |
u32 timeout; |
UART1_PutString("\r\n Getting Version from MK3MAG"); |
// stop polling of other commands |
I2C_StopPolling = 1; |
MK3MAG_Version.Major = 0xFF; |
MK3MAG_Version.Minor = 0xFF; |
MK3MAG_Version.Patch = 0xFF; |
MK3MAG_Version.Compatible = 0xFF; |
// polling of version info |
repeat = 0; |
do |
{ |
I2C1_SendCommand(I2C_CMD_VERSION); |
timeout = SetDelay(250); |
do |
{ |
if (MK3MAG_Version.Major != 0xFF) break; // break loop on success |
}while (!CheckDelay(timeout)); |
UART1_PutString("."); |
repeat++; |
}while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s |
// if we got it |
if (MK3MAG_Version.Major != 0xFF) |
{ |
sprintf(msg, "\r\n MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch); |
UART1_PutString(msg); |
sprintf(msg, " Compatible: %d", MK3MAG_Version.Compatible); |
UART1_PutString(msg); |
} |
else UART1_PutString("\n\r No version information from MK3Mag."); |
I2C_StopPolling = 0; // enable polling of heading command |
} |
//---------------------------------------------------------------- |
void I2C1_UpdateCompass(void) |
{ |
static u32 TimerCompassUpdate = 0; |
if( (I2C_State == I2C_OFF) || I2C_StopPolling ) return; |
if(CheckDelay(TimerCompassUpdate)) |
{ |
// check for incomming compass calibration request |
// update CalByte from spi input queue |
fifo_get(&CompassCalcStateFiFo, (u8 *)&(I2C_WriteCal.CalByte)); |
// send new calstate |
if(I2C_ReadCal.CalByte != I2C_WriteCal.CalByte) |
{ |
I2C1_SendCommand(I2C_CMD_WRITE_CAL); |
} |
else // request current heading |
{ |
I2C1_SendCommand(I2C_CMD_READ_HEADING); |
} |
TimerCompassUpdate = SetDelay(20); // every 20 ms are 50 Hz |
} |
} |
/trunk/i2c.h |
---|
1,58 → 1,41 |
#ifndef __I2C_H |
#define __I2C_H |
#define I2C_CMD_VERSION 0x01 |
#define I2C_CMD_READ_MAG 0x02 |
#define I2C_CMD_READ_HEADING 0x03 |
#define I2C_CMD_WRITE_CAL 0x04 |
extern volatile u32 I2C1_Timeout; |
// I2C states |
#define I2C_STATE_UNDEF 0 |
#define I2C_STATE_IDLE 1 |
#define I2C_STATE_TX_PENDING 2 |
#define I2C_STATE_TX_PROGRESS 3 |
#define I2C_STATE_RX_PENDING 4 |
#define I2C_STATE_RX_PROGRESS 5 |
#define I2C_STATE_OFF 6 |
typedef struct |
{ |
u8 Major; |
u8 Minor; |
u8 Patch; |
u8 Compatible; |
} __attribute__((packed)) I2C_Version_t; |
// current I2C state |
extern volatile u8 I2C_State; |
typedef struct |
{ |
s16 MagX; |
s16 MagY; |
s16 MagZ; |
} __attribute__((packed)) I2C_Mag_t; |
#define I2C1_TIMEOUT 500 // 500 ms |
// define the sizte of the rx and tx buffer |
#define I2C_TXBUFFER_LEN 50 |
#define I2C_RXBUFFER_LEN 50 |
// transfer buffer should be filled before start of transmission only when I2C_State is I2C_STATE_IDLE |
extern volatile u8 I2C_TxBufferSize; |
extern volatile u8 I2C_TxBuffer[]; |
typedef struct |
{ |
s16 Nick; |
s16 Roll; |
} __attribute__((packed)) I2C_WriteAttitude_t; |
void I2C1_Init(void); |
void I2C1_Deinit(void); |
typedef struct |
{ |
u8 CalByte; |
u8 Dummy1; |
u8 Dummy2; |
} __attribute__((packed)) I2C_Cal_t; |
// the pointer to the rxbuffer handler function |
// called by the IRQ routine after all bytes are recieved from slave |
typedef void (*I2C_pRxHandler_t) (u8* pRxBuffer, u8 RxBufferSize); |
// initiate i2c transmission |
// first all bytes from TxBuffer are send to slave |
// and then all bytes are read from slave and the RxHandler function is called |
// returns 1 if a transmission was started, otherwise 0 |
u8 I2C_Transmission(u8 SlaveAddr, I2C_pRxHandler_t pRxHandler, u8 RxBytes); |
typedef struct |
{ |
s16 Heading; |
} __attribute__((packed)) I2C_Heading_t; |
extern volatile u32 I2C1_Timeout; |
extern volatile I2C_Heading_t I2C_Heading; |
extern volatile I2C_Mag_t I2C_Mag; |
extern volatile I2C_Version_t MK3MAG_Version; |
void I2C1_Init(void); |
void I2C1_Deinit(void); |
void I2C1_SendCommand(u8 command); |
void I2C1_GetMK3MagVersion(void); |
void I2C1_UpdateCompass(void); |
#endif // I2C_H |
/trunk/main.c |
---|
65,6 → 65,7 |
#include "uart2.h" |
#include "gps.h" |
#include "i2c.h" |
#include "mk3mag.h" |
#include "timer1.h" |
#include "timer2.h" |
#include "analog.h" |
152,7 → 153,7 |
{ |
UART_VersionInfo.HardwareError[0] = 0; |
if((MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) || CheckDelay(I2C1_Timeout) || (I2C_Heading.Heading < 0)) DebugOut.Status[1] |= 0x08; |
if((MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) || CheckDelay(I2C1_Timeout) || (MK3MAG_Heading.Heading < 0)) DebugOut.Status[1] |= 0x08; |
else DebugOut.Status[1] &= ~0x08; // MK3Mag green status |
if((FC_ErrorCode[1] & DEFEKT_I2C) || (FC_ErrorCode[1] & DEFEKT_BL_MISSING)) DebugOut.Status[1] |= 0x02; |
265,7 → 266,7 |
StopNavigation = 1; |
UART_VersionInfo.HardwareError[0] |= ERROR_GPS_COMMUNICATION; |
} |
else if(I2C_Heading.Heading < 0) |
else if(MK3MAG_Heading.Heading < 0) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"bad compass value "); |
312,7 → 313,7 |
VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq |
VIC_SWITCmd(EXTIT3_ITLine,DISABLE); // clear pending bit |
I2C1_UpdateCompass(); // update compass communication |
MK3MAG_UpdateCompass(); // update compass communication |
Analog_Update(); // get new ADC values |
VIC_ITCmd(EXTIT3_ITLine, ENABLE); // enable irq |
350,7 → 351,7 |
// initialize SPI0 to FC |
SPI0_Init(); |
// initialize i2c bus to MK3MAG (needs Timer 1) |
I2C_Heading.Heading = -1; |
MK3MAG_Init(); |
I2C1_Init(); |
// initialize the gps position controller (needs Timer 1) |
Fat16_Init(); |
369,7 → 370,7 |
DebugOut.Status[0] = 0x04 | 0x08; // NC and MK3Mag |
DebugOut.Status[1] = 0x00; |
I2C1_GetMK3MagVersion(); |
MK3MAG_GetVersion(); |
if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE) |
{ |
UART1_PutString("\n\r MK3Mag not compatible"); |
/trunk/main.h |
---|
2,8 → 2,8 |
#define _MAIN_H |
#define VERSION_MAJOR 0 |
#define VERSION_MINOR 20 |
#define VERSION_PATCH 2 |
#define VERSION_MINOR 21 |
#define VERSION_PATCH 0 |
#define VERSION_SERIAL_MAJOR 11 |
#define VERSION_SERIAL_MINOR 0 |
10,7 → 10,7 |
#define FC_SPI_COMPATIBLE 14 |
#define MK3MAG_I2C_COMPATIBLE 3 |
// FC FLAGS |
#define FCFLAG_MOTOR_RUN 0x01 |
#define FCFLAG_FLY 0x02 |
95,6 → 95,6 |
extern Param_t Parameter; |
extern volatile FC_t FC; |
extern s8 ErrorMSG[25]; |
extern u8 ErrorCode; |
extern u8 ErrorCode; |
extern u8 StopNavigation; |
#endif // _MAIN_H |
/trunk/menu.c |
---|
60,7 → 60,7 |
#include "ubx.h" |
#include "GPS.h" |
#include "timer1.h" |
#include "i2c.h" |
#include "mk3mag.h" |
#include "spi_slave.h" |
#include "menu.h" |
#include "uart1.h" |
314,7 → 314,7 |
case 13: // Remote Control Level from FC |
LCD_printfxy(0,0,"RC-Level: %3i", FC.RC_Quality); |
LCD_printfxy(0,1,"Ubat: %2i.%1i V", FC.BAT_Voltage/10, FC.BAT_Voltage%10); |
LCD_printfxy(0,2,"CompHeading: %3i", I2C_Heading.Heading); |
LCD_printfxy(0,2,"CompHeading: %3i", MK3MAG_Heading.Heading); |
if(GeoMagDec < 0) sign = '-'; |
else sign = '+'; |
LCD_printfxy(0,3,"GeoMagDec: %c%i.%1i", sign, abs(GeoMagDec)/10,abs(GeoMagDec)%10); |
326,11 → 326,11 |
LCD_printfxy(0,3,"UP7:%3i UP8:%3i",Parameter.User7,Parameter.User8); |
break; |
case 15: // MK3MAG |
I2C1_SendCommand(I2C_CMD_READ_MAG); |
MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); |
LCD_printfxy(0,0,"MK3MAG V%i.%i%c",MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a'+ MK3MAG_Version.Patch); |
LCD_printfxy(0,1,"MagX: %4i ",I2C_Mag.MagX); |
LCD_printfxy(0,2,"MagY: %4i ",I2C_Mag.MagY); |
LCD_printfxy(0,3,"MagZ: %4i ",I2C_Mag.MagZ); |
LCD_printfxy(0,1,"MagX: %4i ",MK3MAG_MagVector.X); |
LCD_printfxy(0,2,"MagY: %4i ",MK3MAG_MagVector.Y); |
LCD_printfxy(0,3,"MagZ: %4i ",MK3MAG_MagVector.Z); |
break; |
default: |
//MaxMenuItem = MenuItem - 1; |
/trunk/mk3mag.c |
---|
0,0 → 1,290 |
/*#######################################################################################*/ |
/* !!! THIS IS NOT FREE SOFTWARE !!! */ |
/*#######################################################################################*/ |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Copyright (c) 2008 Ingo Busker, Holger Buss |
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY |
// + FOR NON COMMERCIAL USE ONLY |
// + www.MikroKopter.com |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
// + bzgl. der Nutzungsbedingungen aufzunehmen. |
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
// + Verkauf von Luftbildaufnahmen, usw. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
// + eindeutig als Ursprung verlinkt werden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
// + Benutzung auf eigene Gefahr |
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
// + mit unserer Zustimmung zulässig |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
// + this list of conditions and the following disclaimer. |
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
// + from this software without specific prior written permission. |
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted |
// + for non-commercial use (directly or indirectly) |
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
// + with our written permission |
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
// + clearly linked as origin |
// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed |
// |
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <string.h> |
#include "91x_lib.h" |
#include "mk3mag.h" |
#include "i2c.h" |
#include "timer1.h" |
#include "spi_slave.h" |
#include "main.h" |
#include "uart1.h" |
#define MK3MAG_SLAVE_ADDRESS 0x50 // i2C slave address |
u8 MK3MAG_StopPolling = 1; |
// Transfer buffers |
volatile MK3MAG_Heading_t MK3MAG_Heading; |
volatile MK3MAG_WriteAttitude_t MK3MAG_WriteAttitude; |
volatile MK3MAG_MagVector_t MK3MAG_MagVector; |
volatile MK3MAG_Version_t MK3MAG_Version; |
volatile MK3MAG_Cal_t MK3MAG_WriteCal; |
volatile MK3MAG_Cal_t MK3MAG_ReadCal; |
void MK3MAG_Init(void) |
{ |
MK3MAG_Version.Major = 0xFF; |
MK3MAG_Version.Minor = 0xFF; |
MK3MAG_Version.Patch = 0xFF; |
MK3MAG_Version.Compatible = 0xFF; |
MK3MAG_Heading.Heading = -1; |
} |
// ------------------------------------------------------------------------------- |
// the calculation of the MK3MAG packet checksum |
// calculate the crc of bytecount bytes in buffer |
u8 MK3MAG_CalcCRC(u8* pBuffer, u8 bytecount) |
{ |
u8 i, crc = 0; |
for(i=0; i < bytecount; i++) |
{ |
crc += pBuffer[i]; |
} |
crc = ~crc; |
return crc; |
} |
// assuming the last byte in buffer is the crc byte |
u8 MK3MAG_CheckCRC(u8* pBuffer, u8 BuffLen) |
{ |
u8 crc = 0, retval = 0; |
if(BuffLen == 0) return(retval); |
crc = MK3MAG_CalcCRC(pBuffer, BuffLen-1); |
if(crc == pBuffer[BuffLen-1]) |
{ |
retval = 1; |
DebugOut.Analog[15]++; // count mk3mag ok |
} |
else |
{ |
retval = 0; |
DebugOut.Analog[14]++; // count mk3mag crc error |
} |
return(retval); |
} |
// ------------------------------------------------------------------------------- |
// the I2C rx data handler functions |
// rx data handler for version info request |
void MK3MAG_UpdateVersion(u8* pRxBuffer, u8 RxBufferSize) |
{ // if crc is ok and number of byte are matching |
if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_Version)+1)) ) |
{ |
memcpy((u8 *)&MK3MAG_Version, pRxBuffer, sizeof(MK3MAG_Version)); |
} |
} |
// rx data handler for calibration request |
void MK3MAG_UpdateCalibration(u8* pRxBuffer, u8 RxBufferSize) |
{ // if crc is ok and number of byte are matching |
if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_ReadCal)+1)) ) |
{ |
memcpy((u8 *)&MK3MAG_ReadCal, pRxBuffer, sizeof(MK3MAG_ReadCal)); |
} |
} |
// rx data handler for magnetic vector request |
void MK3MAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize) |
{ // if crc is ok and number of byte are matching |
if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_MagVector)+1)) ) |
{ |
memcpy((u8 *)&MK3MAG_MagVector, pRxBuffer, sizeof(MK3MAG_MagVector)); |
} |
} |
// rx data handler for heading request |
void MK3MAG_UpdateHeading(u8* pRxBuffer, u8 RxBufferSize) |
{ // if crc is ok and number of byte are matching |
if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_Heading)+1)) ) |
{ |
memcpy((u8 *)&MK3MAG_Heading, pRxBuffer, sizeof(MK3MAG_Heading)); |
} |
} |
//---------------------------------------------------------------- |
void MK3MAG_SendCommand(u8 command) |
{ |
// disable I2C IRQ to check state |
//VIC_ITCmd(I2C1_ITLine, DISABLE); |
// If I2C no transmission is in progress |
if(I2C_State == I2C_STATE_IDLE) |
{ |
I2C_TxBufferSize = 0; |
I2C_pRxHandler_t pRxHandlerFunc = NULL; |
u8 RxBytes = 0; |
// update current command id |
I2C_TxBuffer[I2C_TxBufferSize++] = command; |
// set pointers to data area with respect to the command id |
switch (command) |
{ |
case MK3MAG_CMD_VERSION: |
RxBytes = sizeof(MK3MAG_Version)+1; |
pRxHandlerFunc = &MK3MAG_UpdateVersion; |
break; |
case MK3MAG_CMD_WRITE_CAL: |
RxBytes = sizeof(MK3MAG_ReadCal)+1; |
pRxHandlerFunc = &MK3MAG_UpdateCalibration; |
memcpy((u8*)I2C_TxBuffer+1, (u8*)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal)); |
I2C_TxBufferSize += sizeof(MK3MAG_WriteCal); |
break; |
case MK3MAG_CMD_READ_MAGVECT: |
RxBytes = sizeof(MK3MAG_MagVector)+1; |
pRxHandlerFunc = &MK3MAG_UpdateMagVector; |
break; |
case MK3MAG_CMD_READ_HEADING: |
RxBytes = sizeof(MK3MAG_Heading)+1; |
pRxHandlerFunc = &MK3MAG_UpdateHeading; |
// update attitude from spi rx buffer |
VIC_ITCmd(SSP0_ITLine, DISABLE); // avoid spi buffer update during copy |
MK3MAG_WriteAttitude.Roll = FromFlightCtrl.AngleRoll; |
MK3MAG_WriteAttitude.Nick = FromFlightCtrl.AngleNick; |
VIC_ITCmd(SSP0_ITLine, ENABLE); |
memcpy((u8*)I2C_TxBuffer+1, (u8*)&MK3MAG_WriteAttitude, sizeof(MK3MAG_WriteAttitude)); |
I2C_TxBufferSize += sizeof(MK3MAG_WriteAttitude); |
break; |
default: // unknown command id |
RxBytes = 0; |
pRxHandlerFunc = NULL; |
break; |
} |
// update packet checksum |
I2C_TxBuffer[I2C_TxBufferSize] = MK3MAG_CalcCRC((u8*)I2C_TxBuffer, I2C_TxBufferSize); |
I2C_TxBufferSize++; |
// enable I2C IRQ again |
VIC_ITCmd(I2C1_ITLine, ENABLE); |
// initiate I2C transmission |
I2C_Transmission(MK3MAG_SLAVE_ADDRESS, pRxHandlerFunc, RxBytes); |
} // EOF I2C_State == I2C_IDLE |
else // I2C_State != I2C_IDLE |
{ |
// re-enable I2C IRQ again |
//VIC_ITCmd(I2C1_ITLine, ENABLE); |
} |
} |
//---------------------------------------------------------------- |
void MK3MAG_GetVersion(void) |
{ |
u8 msg[64]; |
u8 repeat; |
u32 timeout; |
UART1_PutString("\r\n Getting Version from MK3MAG"); |
// stop polling of other commands |
MK3MAG_StopPolling = 1; |
MK3MAG_Version.Major = 0xFF; |
MK3MAG_Version.Minor = 0xFF; |
MK3MAG_Version.Patch = 0xFF; |
MK3MAG_Version.Compatible = 0xFF; |
// polling of version info |
repeat = 0; |
do |
{ |
MK3MAG_SendCommand(MK3MAG_CMD_VERSION); |
timeout = SetDelay(250); |
do |
{ |
if (MK3MAG_Version.Major != 0xFF) break; // break loop on success |
}while (!CheckDelay(timeout)); |
UART1_PutString("."); |
repeat++; |
}while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s |
// if we got it |
if (MK3MAG_Version.Major != 0xFF) |
{ |
sprintf(msg, "\r\n MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch); |
UART1_PutString(msg); |
sprintf(msg, " Compatible: %d", MK3MAG_Version.Compatible); |
UART1_PutString(msg); |
} |
else UART1_PutString("\n\r No version information from MK3Mag."); |
MK3MAG_StopPolling = 0; // enable polling of heading command |
} |
//---------------------------------------------------------------- |
void MK3MAG_UpdateCompass(void) |
{ |
static u32 TimerCompassUpdate = 0; |
if( (I2C_State == I2C_STATE_OFF) || MK3MAG_StopPolling ) return; |
if(CheckDelay(TimerCompassUpdate)) |
{ |
// check for incomming compass calibration request |
// update CalByte from spi input queue |
fifo_get(&CompassCalcStateFiFo, (u8 *)&(MK3MAG_WriteCal.CalByte)); |
// send new calstate |
if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte) |
{ |
MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL); |
} |
else // request current heading |
{ |
MK3MAG_SendCommand(MK3MAG_CMD_READ_HEADING); |
} |
TimerCompassUpdate = SetDelay(20); // every 20 ms are 50 Hz |
} |
} |
/trunk/mk3mag.h |
---|
0,0 → 1,52 |
#ifndef __MK3MAG_H |
#define __MK3MAG_H |
typedef struct |
{ |
u8 Major; |
u8 Minor; |
u8 Patch; |
u8 Compatible; |
} __attribute__((packed)) MK3MAG_Version_t; |
typedef struct |
{ |
s16 X; |
s16 Y; |
s16 Z; |
} __attribute__((packed)) MK3MAG_MagVector_t; |
typedef struct |
{ |
s16 Nick; |
s16 Roll; |
} __attribute__((packed)) MK3MAG_WriteAttitude_t; |
typedef struct |
{ |
u8 CalByte; |
u8 Dummy1; |
u8 Dummy2; |
} __attribute__((packed)) MK3MAG_Cal_t; |
typedef struct |
{ |
s16 Heading; |
} __attribute__((packed)) MK3MAG_Heading_t; |
#define MK3MAG_CMD_VERSION 0x01 |
#define MK3MAG_CMD_READ_MAGVECT 0x02 |
#define MK3MAG_CMD_READ_HEADING 0x03 |
#define MK3MAG_CMD_WRITE_CAL 0x04 |
extern volatile MK3MAG_Heading_t MK3MAG_Heading; |
extern volatile MK3MAG_MagVector_t MK3MAG_MagVector; |
extern volatile MK3MAG_Version_t MK3MAG_Version; |
void MK3MAG_Init(void); |
void MK3MAG_SendCommand(u8 command); |
void MK3MAG_GetVersion(void); |
void MK3MAG_UpdateCompass(void); |
#endif // MK3MAG_H |
/trunk/spi_slave.c |
---|
61,7 → 61,7 |
#include "gps.h" |
#include "uart1.h" |
#include "spi_slave.h" |
#include "i2c.h" |
#include "mk3mag.h" |
#include "timer1.h" |
#include "timer2.h" |
#include "config.h" |
282,7 → 282,7 |
{ |
// avoid sending data via SPI during the update of the ToFlightCtrl structure |
VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
ToFlightCtrl.CompassHeading = I2C_Heading.Heading; |
ToFlightCtrl.CompassHeading = MK3MAG_Heading.Heading; |
DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
// cycle spi commands |