void ImuInit(BYTE prio, OS_SEM * pSem)
{
I2CInit();
#ifdef LITTLE_CAR
CPU_Pins[27].function(CPUPIN27_SCL);
CPU_Pins[28].function(CPUPIN28_SDA);
CPU_Pins[79].function(CPUPIN79_UTXD2 );
CPU_Pins[80].function(CPUPIN80_URXD2 );
#elif BIG_CAR
CPU_Pins[27].function(CPUPIN27_GPIO);
CPU_Pins[28].function(CPUPIN28_GPIO);
CPU_Pins[79].function(CPUPIN79_SCL);
CPU_Pins[80].function(CPUPIN80_SDA);
#else
#error No Car defined
#endif
I2CFirstThing();
LocalSem=pSem;
SmOSSimpleTaskCreate(IMU_Task,prio);
}
void main(void)
{
//Set System clock to 72MHz
xSysCtlClockSet(72000000, xSYSCTL_OSC_MAIN | xSYSCTL_XTAL_8MHZ);
SysCtlDelay(10000);
xSysCtlPeripheralEnable2(I2C1_BASE);
xSysCtlPeripheralEnable2(xGPIO_PORTB_BASE);
//Map I2C1 Pin
//SCK --> PB6
//SDA --> PB7
xSPinTypeI2C(I2C1SCK, PB6);
xSPinTypeI2C(I2C1SDA, PB7);
//Initialize I2C Module 100K
I2CInit(I2C1_BASE, 100000);
I2CEnable(I2C1_BASE);
//Enable ADXL345 Measure Function
I2CMasterWriteS1(I2C1_BASE, I2C_SLAVE_ADDR, 0x2D, xfalse);
I2CMasterWriteS2(I2C1_BASE, 0x08, xtrue);
SysCtlDelay(100000);
//Read Accelerometer XYZ data in continue mode
I2CMasterWriteS1(I2C1_BASE, I2C_SLAVE_ADDR, 0x32, xfalse);
I2CMasterReadBufS1(I2C1_BASE, I2C_SLAVE_ADDR, RecvBuf, 6, xtrue);
while(1);
}
static void prvSetupHardware( void )
{
SystemCoreClockUpdate();
// enable clock to RAM1
LPC_SYSCON->SYSAHBCLKCTRL |= (1<<26);
// init GPIO
GPIOInit();
// init I2C
I2CInit(I2CMASTER);
//init ADC
ADCInit(ADC_CLK);
// init SPI ports
SSP_IOConfig( 0 );
SSP_Init( 0 );
SSP_IOConfig( 1 );
SSP_Init( 1 );
// init keyboard
CAP_KEY_Init();
// init MOSFET pin
LPC_IOCON->PIO1_25=0x90;
GPIOSetDir(PIN_MOSFET,1);
}
MPU6050::MPU6050( I2C_MODULE new_i2cBusId, UINT8 new_devAddress )
{
// Full scale range of 500 º/s => 65.5 LSB/(º/s)
// See MPU-6050 Product Specification page 12 for more information.
GYRO_XSCALE = 65.5;
GYRO_YSCALE = 65.5;
GYRO_ZSCALE = 65.5;
ACCEL_XSCALE = 16384;
ACCEL_YSCALE = 16384;
ACCEL_ZSCALE = 16384;
XANGLE = 0.0;
YANGLE = 0.0;
ZANGLE = 0.0;
// Set the I2C interface used
//this->i2cBusId = new_i2cBusId;
this->i2cBusId = new_i2cBusId;
// Set the device address
//this->deviceAddress = new_devAddress;
this->deviceAddress = new_devAddress;
// TODO: Fault handling! What to do if function fails?
I2CInit();
}
MPU6050::MPU6050(void)
{
// Full scale range of 500 º/s => 65.5 LSB/(º/s)
// See MPU-6050 Product Specification page 12 for more information.
GYRO_XSCALE = 65.5;
GYRO_YSCALE = 65.5;
GYRO_ZSCALE = 65.5;
ACCEL_XSCALE = 16384;
ACCEL_YSCALE = 16384;
ACCEL_ZSCALE = 16384;
XANGLE = 0.0;
YANGLE = 0.0;
ZANGLE = 0.0;
// Set the I2C interface used
this->i2cBusId = I2C1;
// Set the device address
this->deviceAddress = 0x68;
// TODO: Fault handling! What to do if function fails?
I2CInit();
}
/* Hardware initialization */
static void hwInit(void)
{
extFunc = eeprom_read_byte((uint8_t*)EEPROM_EXT_FUNC);
#if 0
loadTempParams();
if (extFunc == USE_DS18B20) {
ds18x20SearchDevices();
tempInit(); /* Init temperature control */
}
#endif
I2CInit(); /* I2C bus */
displayInit(); /* Load params and text labels before fb scan started */
rcInit(); /* IR Remote control */
inputInit(); /* Buttons/encoder polling */
adcInit(); /* Analog-to-digital converter */
sei(); /* Gloabl interrupt enable */
//tunerInit(extFunc); /* Tuner */
DDR(STMU_STBY) |= STMU_STBY_LINE; /* Standby port */
DDR(STMU_MUTE) |= STMU_MUTE_LINE; /* Mute port */
sndInit(extFunc); /* Load labels/icons/etc */
setStbyTimer(0);
return;
}
void main()
{
/* Buffer where we will read/write our data */
unsigned char I2CData[] = {0x00, 0x00, 0x00, 0x01, 0x01, 0x01, 0x09, 0x00};
unsigned char i;
/* Initialize I2C Port */
I2CInit();
/* Send Start condition */
I2CStart();
/* Send DS1307 slave address with write operation */
I2CSend(0xD0);
/* Send subaddress 0x00, we are writing to this location */
I2CSend(0x00);
/* Loop to write 8 bytes */
for (i = 0; i < 8; i++)
{
/* send I2C data one by one */
//I2CSend(I2CInitval[i]);
I2CSend(I2CData[i]);
}
/* Send a stop condition - as transfer finishes */
I2CStop();
/* We will now read data from DS1307 */
/* Reading for a memory based device always starts with a dummy write */
/* Send a start condition */
I2CStart();
/* Send slave address with write */
I2CSend(0xD0);
/* Send address for dummy write operation */
/* this address is actually where we are going to read from */
I2CSend(0x00);
/* Send a repeated start, after a dummy write to start reading */
I2CRestart();
/* send slave address with read bit set */
I2CSend(0xD1);
/* Loop to read 8 bytes from I2C slave */
for (i = 8; i > 0; i--) {
/* read a byte */
I2CData[i] = I2CRead();
/* ACK if its not the last byte to read */
/* if its the last byte then send a NAK */
if (i - 1)
I2CAck();
else
I2CNak();
}
/* Send stop */
I2CStop();
/* end of program */
while(1);
}
/*********************************************************************************************************
** 函数名称: InitFRamWR
** 函数名称: InitFRamWR
**
** 功能描述： 系统初始化时，需要看看FRAM里面是否有存储过往的数据，如果有，而且匹配，则加载
**
**
** 输 出: INT8U
**
** 全局变量:
** 调用模块: 无
**
** 作 者: LiJin
** 日 期: 2008年9月19日
** 备 注:
**-------------------------------------------------------------------------------------------------------
** 修改人:
** 日 期:
** 备 注:
**------------------------------------------------------------------------------------------------------
********************************************************************************************************/
extern INT8U InitFRamMgrTsk( void )
{
//0.启动硬件部分
//I2C中断设置
SetVICIRQ(VIC_I2C2, 6, (int)i2c1IRQ);
//初始化I2C0
I2CInit(1, 100000,NULL);
//启动定时写任务
OSTaskCreate (FRamSave_Task, (void *)0, &FRamWRTaskStk[FRAMWR_TASK_STK_SIZE-1], PRIO_FRAM_SAVE);
return TRUE;
}
void init_board(void)
{
/* clkout of 10mhz on 1.27 */
LPC_PINCON->PINSEL3 &=~(3<<22);
LPC_PINCON->PINSEL3 |= (1<<22);
LPC_SC->CLKOUTCFG = (1<<8)|(14<<4); //enable and divide by 12
UART0_Init(921600);
if (I2CInit((uint32_t) I2CMASTER) == 0) {
printf("Fatal error!\n");
while (1);
}
}
/*******************************************************************************
* Function Name : Axis3_Test
* Description : Light Sensor Test.
* Input : None
* Output : None
* Return : None
*******************************************************************************/
void Axis3_Test(void)
{
char buf[24];
int32_t xoff = 0;
int32_t yoff = 0;
int32_t zoff = 0;
int8_t x = 0;
int8_t y = 0;
int8_t z = 0;
OLED_ClearScreen();
OLED_DisStrLine(0, 0, "Axis-3");
I2CInit(I2CMASTER, 0);
acc_init();
/* Assume base board in zero-g position when reading first value. */
acc_read(&x, &y, &z);
xoff = 0-x;
yoff = 0-y;
zoff = 0-z;
// while(1)
// {
/* Accelerometer */
acc_read(&x, &y, &z);
x = x+xoff;
y = y+yoff;
z = z+zoff;
snprintf(buf, 20, "Acc x: %d ", x);
OLED_DisStrLine(2, 0, (uint8_t *)buf);
printf("\r\nAcc x: %d, ", x);
snprintf(buf, 20, "Acc y: %d ", y);
OLED_DisStrLine(3, 0, (uint8_t *)buf);
printf("Acc y: %d, ", y);
snprintf(buf, 20, "Acc z: %d ", z);
OLED_DisStrLine(4, 0, (uint8_t *)buf);
printf("Acc z: %d. ", z);
delay_ms(250);
// if(KEY_Read() == KEY_ESC)
// break;
// }
}
/**
* @brief The main function for the charger.
* @details This function is entered automatically at reset. There is no exit
* from this function, it is declared as _int_ to avoid compiler warning.
*/
int main()
{
uint32_t i;
FastVoltage = RawVoltage = RawCurrent = 0;
PWM_Stop(); // Disable PWM output (set it low)
ADC_Init(); // Initialize the A/D converters
if (BUTTON1_PRESSED) // If button pressed at reset, calibrate only
State = CALIBRATE;
else // else wait for the START button
State = WAIT4BUTTON;
//
// Wait at least 100 ms for the LCD to wake up
// Assume that the loop takes 4 clocks, wait 125 ms
//
for (i = 0; i < (SystemCoreClock / 32); i++) {
}
LCD_Init();
//
// Set up the System Tick
//
FLAG1_PORT->DIR |= FLAG1_Msk;
FLAG1_PORT->DATA &= ~FLAG1_Msk;
Ticks = 0;
SysTick_Config((SystemCoreClock / TICKS_PER_SEC) - 1);
//
// Set up the I2C interface for the temperature sensor
//
I2CInit((uint32_t) I2CMASTER);
for (;;) {
if (0 == Ticks) {
for (i = 0; i < BUFSIZE; i++) {
I2CSlaveBuffer[i] = 0x00;
}
I2CWriteLength = 0;
I2CReadLength = 2;
I2CMasterBuffer[0] = LM75_ADDR | RD_BIT;
I2CEngine();
Temperature = I2CSlaveBuffer[0];
// Wait until Ticks becomes non-zero to read the sensor again
while (0 == Ticks) {
}
}
}
}
/** Configures the board hardware and chip peripherals for the demo's functionality. */
static void SetupHardware(void)
{
SystemInit();
Board_Init();
MyUSB_Init();
UARTInit(115200);
#ifdef _MY_UNIT_TEST_
main_blinky();
#else
Timer_Init();
#endif // #ifdef _MY_UNIT_TEST_
I2CInit();
ADCInit();
}
void SoftReset() {
BYTE ipl_backup = SRbits.IPL;
SRbits.IPL = 7; // disable interrupts
log_printf("SoftReset()");
TimersInit();
PinsInit();
PWMInit();
ADCInit();
UARTInit();
SPIInit();
I2CInit();
InCapInit();
// TODO: reset all peripherals!
SRbits.IPL = ipl_backup; // enable interrupts
}
int main(void)
{
float tempf;
InitRCC();
InitGPIO();
InitADC();
I2CInit();
InitIT();
Delay_Init(24);
// reset pot value
PotWrite(0x00);
while(1) {
if (mode == MODE_CAL) {
LED_GREEN_ON();
LED_RED_OFF();
uint16_t av = ADC_GetConversionValue(AN_ADCx);
if (av > trig_level) {
trig_level = av;
tempf = GetADCVolt(trig_level)*VOLT_DIV; // get voltage from piezo
tempf = GetTargetK(tempf); // get target amp coefficient
tempf = GetTargetR(tempf); // get R2 for opamp feedback
tempf = GetPotR(tempf); // get target resistance for pot as part of R2
PotWrite(GetWStep(tempf)); // write step to pot
}
} else {
// turn on red for working mode indication, turn green if mcu gets opamp output high
LED_RED_ON();
if (GPIO_ReadInputDataBit(OP_PORT, OP_PIN) == SET) {
LED_GREEN_ON();
delay_ms(500);
} else {
LED_GREEN_OFF();
}
}
}
}
EFI_STATUS
InitializeDS3231 (
VOID
)
{
EFI_STATUS Status;
I2C_DEVICE Dev;
RTC_DS3231_CONTROL Temp;
RTC_DS3231_HOURS Hours;
// Prepare the hardware
(VOID)IdentifyDS3231();
(VOID) CopyMem(&Dev, &gDS3231RtcDevice, sizeof(Dev));
Status = I2CInit(Dev.Socket,Dev.Port,Normal);
if (EFI_ERROR (Status)) {
goto EXIT;
}
// Ensure interrupts are masked. We do not want RTC interrupts in UEFI
Status = I2CRead(&Dev,DS3231_REGADDR_CONTROL,1,&Temp.u8);
if (EFI_ERROR (Status)) {
goto EXIT;
}
Temp.bits.INTCN = 0;
Status = I2CWrite(&Dev,DS3231_REGADDR_CONTROL,1,&Temp.u8);
if (EFI_ERROR (Status)) {
goto EXIT;
}
MicroSecondDelay(2000);
Status = I2CRead(&Dev,DS3231_REGADDR_HOURS,1,&Hours.u8);
if (EFI_ERROR (Status)) {
goto EXIT;
}
Hours.bits.Hour24_n = 0;
Status = I2CWrite(&Dev,DS3231_REGADDR_HOURS,1,&Hours.u8);
if (EFI_ERROR (Status)) {
goto EXIT;
}
mDS3231Initialized = TRUE;
EXIT:
return Status;
}
static void prvSetupHardware( void )
{
SystemCoreClockUpdate();
// enable clock to RAM1
LPC_SYSCON->SYSAHBCLKCTRL |= (1<<26);
// init GPIO
GPIOInit();
// init I2C
I2CInit(I2CMASTER);
//init ADC
ADCInit(ADC_CLK);
// init SPI ports
SSP_IOConfig( 0 );
SSP_Init( 0 );
SSP_IOConfig( 1 );
SSP_Init( 1 );
if (GPIOGetPinValue( PIN_CAL_DET ))
{
m_eAppCFGStatus = appANTENNA_CAL;
// init CLKOUT pin
LPC_IOCON->PIO0_1=1; // enable CLKOUT
LPC_SYSCON->CLKOUTSEL=3; // select main clock
LPC_SYSCON->CLKOUTUEN=1; // update clock source
LPC_SYSCON->CLKOUTDIV=2; // divide by 2
}
else
{
m_eAppCFGStatus = appNFC_KEYB_TEMP_SWITCH;
// init keyboard
CAP_KEY_Init();
// init MOSFET pin
LPC_IOCON->PIO1_25=0x90;
GPIOSetDir(PIN_MOSFET,1);
}
}
int main (void){
float acc_x, acc_z, gyro_x;
float acc_angle,kal_angle;
/* Init Systick to 1ms */
SysTick_Config( SystemCoreClock / 1000);
/* Initialize GPIO (sets up clock) */
LPC_SYSCON->SYSAHBCLKCTRL |= (1<<6);
SERVO_init();
if(I2CInit(I2CMASTER) == FALSE){
while(1); /* fatal error */
}
if(MPU6050_init()){
return 0;
}
MPU6050_setZero();
kalman_init();
for(;;){
/* 100Hz loop */
if(gSysTick_10 >= 9){
gSysTick_10 = 0;
/* get sensor values */
gyro_x = MPU6050_getGyroRoll_degree();
acc_x = MPU6050_getAccel_x();
acc_z = -MPU6050_getAccel_z();
/* acc angle */
acc_angle = atan2(acc_x , -acc_z) * 180/3.14159 ; // calculate accel angle
kal_angle = kalman_update(acc_angle,gyro_x, 0.01);
SERVO_set_slew((-kal_angle) - MECH_OFFSET);
}
}
}
void MEMSInit() {
I2CInit();
writeRegister(MPU6050_ADDRESS_AD0_LOW, MPU6050_RA_SMPLRT_DIV, MPU6050_SMPLRT_DIV);
writeRegister(MPU6050_ADDRESS_AD0_LOW, MPU6050_RA_GYRO_CONFIG, MPU6050_FS_SEL0);
writeRegister(MPU6050_ADDRESS_AD0_LOW, MPU6050_RA_ACCEL_CONFIG, MPU6050_AFS_SEL0);
writeRegister(MPU6050_ADDRESS_AD0_LOW, MPU6050_RA_INT_PIN_CFG, MPU6050_INT_LEVEL_LOW|MPU6050_LATCH_INT_ENABLE|MPU6050_INT_RD_SET);
writeRegister(MPU6050_ADDRESS_AD0_LOW, MPU6050_RA_INT_EN, MPU6050_DATA_RDY_EN);
writeRegister(MPU6050_ADDRESS_AD0_LOW, MPU6050_RA_PWR_MGMT_1, MPU6050_WAKE);
/*Interrupt triggers when touched to ground*/
//Therefore it is a idle high, interrupt triggers when pin is low
// use port0_5 as input event, interrupt test.
GPIOSetDir(PORT0, 21, INPUT );
NVIC_SetPriority(FLEX_INT0_IRQn, GPIOINT0PRIORITY);
// port2_1, single edge trigger, active high.
//channel #, port #, bit position, sense, event(polarity)
GPIOSetFlexInterrupt(CHANNEL0, PORT0, 21, 0, 0 );
}
static bsp_init (void)
{
rg_led_init ();
g_led (0);
#if FS_EN == 1
fs_init ();
#endif
#if SHELL_EN == 1
shell_init ();
#endif
#if TCP_EN == 1
init_TcpNet ();
#endif
#if KEY_EN == 1
key_init ();
#endif
#if RESET_KEY_EN == 1
reset_key_init ();
#endif
#if GUI_EN == 1
lcd_init ();
lcd_write_led (0);
widget_init ();
#endif
I2CInit(0);
#if RTC_EN == 1
pcf8563_init ();
#endif
}
//==================
// Initialize RTC
//==================
void Init_RTC(uint32_t do_adj, uint8_t year, uint8_t month, uint8_t day,
uint8_t week, uint8_t hour, uint8_t minute, uint8_t second)
{
// Initialize I2C
if (I2CInit((uint32_t) I2CMASTER) == FALSE )
{
while (1); // Error Trap
}
// Initialize RTC
if (do_adj)
{
Wait_N_Ticks(100); // wait 1000ms
//
RTC_Write_Reg(RTC_CONTROL1, 0x20); // STOP
RTC_Write_Reg(RTC_CONTROL2, 0x00);
//
RTC_Write_Reg(RTC_HOURS, BCD_INT(hour));
RTC_Write_Reg(RTC_MINUTES, BCD_INT(minute));
RTC_Write_Reg(RTC_SECONDS, BCD_INT(second));
//
RTC_Write_Reg(RTC_YEARS, BCD_INT(year));
RTC_Write_Reg(RTC_C_MONTHS, BCD_INT(month));
RTC_Write_Reg(RTC_DAYS, BCD_INT(day));
RTC_Write_Reg(RTC_WEEKDAYS, BCD_INT(week));
//
RTC_Write_Reg(RTC_MINUTE_ALARM, 0x00);
RTC_Write_Reg(RTC_HOUR_ALARM, 0x00);
RTC_Write_Reg(RTC_DAY_ALARM, 0x00);
RTC_Write_Reg(RTC_WEEKDAY_ALARM, 0x00);
//
RTC_Write_Reg(RTC_CLKOUT_FREQ, 0x00);
RTC_Write_Reg(RTC_TIMER_CONTROL,0x00);
RTC_Write_Reg(RTC_TIMER, 0x00);
//
RTC_Write_Reg(RTC_CONTROL1, 0x00); // START
}
}
/****************************************************************************
* DibBridgeTargetInit
****************************************************************************/
DIBSTATUS DibBridgeTargetInit(struct DibBridgeContext *pContext)
{
DIBSTATUS rc = DIBSTATUS_SUCCESS;
/*** Initialize Mutex to avoid concurrent access ***/
DibAllocateLock(&pContext->DibTargetAccessLock);
DibInitLock(&pContext->DibTargetAccessLock);
DibAllocateLock(&pContext->BridgeTargetCtx.IrqLock);
DibInitLock(&pContext->BridgeTargetCtx.IrqLock);
/*** Configure Driver for I2C interface ***/
DibBridgeSetHostIfMode(pContext, eI2C);
/*** Use a global variable to call in DibWaitForEvent ***/
pLocalContext = pContext;
/*** Initialize I2C by Parallel port **i*/
I2CInit();
return rc;
}
struct Interface *attachSensorToI2CBus(void *_board,int i2cbus,int add)
{
struct Board *self = _board;
struct Interface *p;
if(!self->i2cstart)
{
self->i2cstart = (struct Interface *)malloc(sizeof(struct Interface));//punta al primo bus I2C
self->i2cnext = self->i2cstart;
self->i2cnext->port = NULL;
I2CInit(LOW_SPEED);//da togliere
}
else
{
self->i2cnext = self->i2cstart;
p = self->i2cnext;
while(p != NULL)
{
p = p->next;
}
if(p == NULL)
{
p = (struct Interface *)malloc(sizeof(struct Interface));//punta al primo bus I2C
p->port = NULL;
}
}
if(p->port == NULL)//collego il bus al groveport scelta
{
p->port = (GrovePort *) malloc(sizeof (GrovePort));
p->port->Pin1 = g_I2CSCL[0];
p->port->Pin2 = g_I2CSDA[0];
p->port->Use = DOUBLE_PORT;
p->next = NULL;
}
return p;
}
void setup() {
SYSTEMConfigPerformance(SYSCLK);
// Initialize hardware UART2 and establish communication at 9600 bps
UARTInit(9600);
// Init i2c
I2CInit();
// if (I2CInit((uint32_t) I2CMASTER) == 0) {
// printf("Fatal error!\n");
// while (1);
// }
ov7670_init();
// включить таймер
//T1CON = 0x8030;
//OpenTimer2( T2_ON | T2_SOURCE_INT | T2_PS_1_8, 0x0008);
//OpenTimer2( T2_ON | T2_SOURCE_INT | T2_PS_1_256, 0xFFFF);
// This statement says: turn on timer2 | have it use an internal clock source | have it
// use a prescaler of 1:256, and use a period of 0xFFFF or 2^16 cycles
// Timer2 as configured would trigger an interrupt at a frequency of (80MHZ/256/65535), or 4.77
// times a second.
//ConfigIntTimer2( T2_INT_ON | T2_INT_PRIOR_2);
// This statement configured the timer to produce an interrupt with a priority of 2
//INTEnableSystemMultiVectoredInt();
// Use multi-vectored interrupts
OpenTimer2(T2_ON | T2_PS_1_8, 0);
OpenOC4( OC_ON | OC_TIMER_MODE32 | OC_TIMER2_SRC | OC_CONTINUE_PULSE | OC_LOW_HIGH , 0, 0 );
}
void MSInit()
{
//turn off A lot of crap
CM1CON0bits.C1ON=0;
CM1CON0=0;
CM2CON0=0;
SRCON0bits.SRLEN=0;
//disable analog
ANSEL=0;
ANSELH=0;
ADCON0bits.ADON=0;
UARTInit();
RTCInit();
EPInit();
USBInit();
I2CInit();
LED_LATCH=0;
LED_TRIS=0;
SetLED(0);
EnableOperation();
}
int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//*******************************************************************
// Switch 1 controls the Serial Data Logger (SDL) communication speed
//-------------------------------------------------------------------
if (_SW1)
// Switch 1 is ON - Configuring SDL for PIC-to-PIC
// high-speed communication at 1 MBaud
SDLInit(3, BAUD_1M);
else
// Switch 1 is OFF - Configuring SDL for ZigBEE
// wireless communication at 115.2 KBaud
SDLInit(3, BAUD_115200);
//*******************************************************************
I2CInit(5, 1); // First param: IL = 5 (interrupt request priority
// Second param: I2C speed
// 0 - lowest (123 kHz at Fcy = 64MHz)
// 1 - 200 kHz
// 2 - 400 kHz
// 3 - 1 MHz
//-------------------------------------------------------------------
uint RC = 0;
ulong Alarm = 0;
//==================================================================
BLIAsyncStart(100,100);
TMRDelay(2000);
BLIAsyncStop();
//==================================================================
BLIAsyncStart(50,50);
if (_SW2)
// Switch 2 is ON - Configuring MPU fo Alt. sensitivity
RC = MPUInit(0, 3, MPU_GYRO_1000ds, MPU_ACC_4g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec)
else
// Switch 2 is OFF - Configuring MPU fo normal sensitivity
RC = MPUInit(0, 3, MPU_GYRO_2000ds, MPU_ACC_2g);
// Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
if (RC) BLIDeadStop("EG", 2);
BLIAsyncStop();
//*******************************************************************
BLISignalOFF();
//====================================================
// byte mpuID;
// byte mpuDLPF;
// byte mpuINT;
// byte mpuPWRM1;
// //---------------------------
// RC = MPUReadID(2, &mpuID);
// RC = MPUGetPWRM1(2, &mpuPWRM1);
// RC = MPUGetDLPF(2, &mpuDLPF);
// RC = MPUGetINT(2, &mpuINT);
//-----------------------------------------------------
//====================================================
// Synchronous interface
//-----------------------------------------------------
struct
{
MPUData Sample1;
MPUData Sample2;
} MPU;
//-----------------------------------------------------
while (TRUE)
{
Alarm = TMRSetAlarm(500);
//------------------------------------
if ( (RC = MPUReadSample(1, &MPU.Sample1)) )
BLIDeadStop("SOS", 3);
//------------------------
if ( (RC = MPUReadSample(2, &MPU.Sample2)) )
BLIDeadStop("SOS", 3);
//------------------------------------
BLISignalFlip();
//-------------------------
SDLPostIfReady((byte*)&MPU, sizeof(MPU));
//-------------------------
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 0;
}
int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
I2CInit(5, 0); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz
//--------------------------
TMRDelay(1000); // Wait for 1 sec so that the shake from turning on
// power switch dissipates...
//--------------------------
if (MPUInit(3, 1)) // Initialize motion Sensor - 1 kHz/4 (250 Hz)
BLIDeadStop("EA", 2);
//--------------------------
#ifdef __MAG_Use__
if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor
// ODR = 6 (max, 75 Hz),
// Gain = 2 (1.3 Gs)
// DLPF = 0 (no averaging)
BLIDeadStop("EM", 2);
#endif
//--------------------------
UARTInitTX(6, 48); // Initialize UART1 for TX on IPL=6 at 115200 bps
// This initialization routine accepts BaudRate in multiples
// of 2400 bps; Thus:
// BaudRate = 1 => 2400 bps
// BaudRate = 2 => 4800 bps
// ...
// BaudRate = 48 => 115200 bps
//------------------------------------------------------------
// High speed
//------------------------------------------------------------
// BaudRate = 100 => 250,000 bps
// BaudRate = 200 => 500,000 bps
// BaudRate = 250 => 625,000 bps
// BaudRate = 350 => 833,333 bps
// BaudRate = 500 => 1,250,000 bps
// BaudRate = 1000 => 2,500,000 bps
//*******************************************************************
uint RC = 0;
//--------------------------
MPUSample AGSample;
#ifdef __MAG_Use__
HMCSample MSample;
#endif
//--------------------------
if (MPUAsyncStart())
BLIDeadStop("A", 1);
//--------------------------
#ifdef __MAG_Use__
if (HMCAsyncStart())
BLIDeadStop("M", 1);
#endif
//--------------------------
struct
{
ulong TS; // Timestamp of the cycle
//-----------------------------------------------
ulong MPUCount; // Sequential number of MPU sample
#ifdef __MAG_Use__
ulong MAGCount; // Sequential number of MAG sample
#endif
//-----------------------------------------------
// Accelerometer (in units of G)
//-----------------------------------------------
Vector A;
//-----------------------------------------------
// Gyroscopes (in Rad/sec)
//-----------------------------------------------
Vector G;
#ifdef __MAG_Use__
//-----------------------------------------------
// Magnetometer (in mGs)
//-----------------------------------------------
Vector M;
#endif
} UData;
//*******************************************************************
BLISignalON();
while(1)
{
TMRDelay(100);
//------------------------
#ifdef __MAG_Use__
RC = HMCAsyncReadWhenReady(&MSample);
if (RC) BLIDeadStop("M", 1);
#endif
//------------------------
RC = MPUAsyncReadWhenReady(&AGSample);
if (RC) BLIDeadStop("A", 1);
//---------------------------------------------
UData.MPUCount = AGSample.Count;
#ifdef __MAG_Use__
UData.MAGCount = MSample.Count;
#endif
//------------------------
VectorCopy(&AGSample.A, &UData.A);
VectorCopy(&AGSample.G, &UData.G);
#ifdef __MAG_Use__
VectorCopy(&MSample.M, &UData.M);
#endif
//------------------------
UData.TS = AGSample.TS;
//---------------------------------------------
UARTPostWhenReady((uchar*)&UData, sizeof(UData));
//---------------------------------------------
BLISignalFlip();
}
return 1;
}
int main(void)
{
// <editor-fold defaultstate="collapsed" desc="Initialization of HW components/modules">
//*******************************************************************
// Initialization of HW components/modules
//===================================================================
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//--------------------------
BLIAsyncMorse("S", 1); // dit-dit-dit
MCMInitT(2.5, 2500); // Initialize Motor Control for PPM with setting
// Throttle to HIGH for delay interval to let ESC
// capture Throttle range
BLIAsyncMorse("O", 1); // dah-dah-dah
//--------------------------
ADCInit(3); // Initialize ADC to control battery
//--------------------------
RCInit(4); // Initialize Receiver interface with Priority=4
//--------------------------
I2CInit(5, 1); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz
//--------------------------
UARTInitTX(6, 350); // Initialize UART1 for TX on IPL=6 at
// BaudRate = 48 => 115,200 bps - ZigBEE
//--------------------------------------
// BaudRate = 100 => 250,000 bps
// BaudRate = 200 => 500,000 bps
// BaudRate = 250 => 625,000 bps
// BaudRate = 350 => 833,333 bps - SD Logger
// BaudRate = 500 => 1,250,000 bps
// BaudRate = 1000 => 2,500,000 bps
//*******************************************************************
// <editor-fold defaultstate="collapsed" desc="Initializing IMU">
//==================================================================
#ifdef __MAG_Use__
//--------------------------------------------------------------
// Initialize Magnetometer
//--------------------------------------------------------------
if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor
// ODR = 6 (max, 75 Hz),
// Gain = 2 (1.3 Gs)
// DLPF = 0 (no averaging)
BLIDeadStop("EM", 2);
#endif
//*******************************************************************
BLIAsyncMorse( "I", 1); // dit - dit
//==================================================================
#ifdef __MXB_Use__
//--------------------------------------------------------------
// Initialize MaxBotix range finder
//--------------------------------------------------------------
if ( 0 == MXBInit(3, &TM.MXB) )
BLIDeadStop("ES", 2);
#endif
//==================================================================
// Initialize motion sensor - rotation rate baseline established at
// this time - QUAD should be motionless!!!
//------------------------------------------------------------------
if ( MPUInit(0, 3 ) ) // Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec)
BLIDeadStop("EA", 2);
//------------------------------------------------------------------
BLIAsyncStop();
//==================================================================
// </editor-fold>
//==================================================================
BLISignalON();
TMRDelay(2000); // Wait for extra 2 sec - to let ESC arm...
// (finish the song :) )
BLISignalOFF();
//===================================================================
// </editor-fold>
//*******************************************************************
// Quadrocopter control variables
//-------------------------------------------------------------------
ulong StartTS = 0;
ulong StepTS;
RCData RCNative; // Control input from Receiver
RCData RC; // Smoothed control input used in all
// control calculations
DCMData IMU; // Orientation data from DCM
MCMData MC; // Motor control Variables
float BatNomV = ADCGetBatteryNomVoltage();
ulong Alarm = 0;
#ifdef __CB_To_Model_Front__
// RC native input rotated to adjust CB orientation to model front
RCData RC_CB_To_Model_Front;
// Control board front does not coincide with the model front
Matrix CB_To_Model;
// Build rotation matrix to adjust for orientation discrepancy
MatrixYawRotation(__CB_To_Model_Front__, &CB_To_Model);
#else
// If CB orientation coincides with the fron of the model
// we will use Native RC input as base for RC input to
// Quadrocopter control module
#define RC_CB_To_Model_Front RCNative
#endif
Re_Start:
//==================================================================
// Wait for the Receiver ARMing: Control should go Down and then Up
//------------------------------------------------------------------
BLIAsyncMorse( "O", 1); // doh - doh - doh
RCSymArm();
//==================================================================
BLIAsyncMorse("T", 1); // doh
MPUAsyncStop();
if (MPUCalibrate() != MPU_OK)
// Gyro Calibration filed
BLIDeadStop("EA", 2);
BLIAsyncStop();
//==================================================================
// Start IMU and wait until orientation estimate stabilizes
//------------------------------------------------------------------
BLIAsyncMorse( "E", 1); // dit
IMUInit();
//------------------------------------------------------------------
QCMReset(); // Initialize (reset) QCM variables
//------------------------------------------------------------------
BLIAsyncStop();
//==================================================================
//******************************************************************
// Control variables used to smooth RC receiver data
//------------------------------------------------------------------
// Reset Smothed RC data
//-------------------------------------------------------------------
RC.Roll = 0.0;
RC.Pitch = 0.0;
RC.Yaw = 0.0;
//------------------------
RC.Throttle = 0.0;
//------------------------
RC.Control = 1;
//-------------------------------------------------------------------
//*******************************************************************
// Quadrocopter Control Loop
//-------------------------------------------------------------------
BLISignalON();
while (1)
{
// Sets the "frquency" of Control Loop
Alarm = TMRSetAlarm(10); // Set Alarm 10 msec in the future
//============================================================
// Read commands from receiver - non-blocking call!
// (we will get out of this call even if connection to the
// receiver is lost!)
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process Receiver feed">
if ( RCSymRead(&RCNative) )
{
//------------------------------------------------------------
// Normalize Roll and Pitch control input from RC Receiver to
// +/- 0.35 rad (~20 degrees) and Yaw control input to
// +/- 3.00 rad (~172 degrees)
//------------------------------------------------------------
RCNative.Roll = 0.35 * RCNative.Roll;
RCNative.Pitch = 0.35 * RCNative.Pitch;
RCNative.Yaw = 3.00 * RCNative.Yaw;
#ifdef __CB_To_Model_Front__
{
// Control board front does not coincide with the model front
Vector RCInput;
Vector RCRotated;
VectorSet(RCNative.Roll, RCNative.Pitch, RCNative.Yaw, &RCInput);
MatrixTimesVector(&CB_To_Model, &RCInput, &RCRotated);
RC_CB_To_Model_Front.Roll = RCRotated.X;
RC_CB_To_Model_Front.Pitch = RCRotated.Y;
RC_CB_To_Model_Front.Yaw = RCRotated.Z;
}
#endif
}
//============================================================
// Smooth RC data
//------------------------------------------------------------
// Roll, Pitch, and Yaw are smoothed with the IIR(8)
//------------------------------------------------------------
RC.Roll = (RC.Roll * 7.0 + RC_CB_To_Model_Front.Roll ) * 0.125; // 1/8 = 0.125
RC.Pitch = (RC.Pitch * 7.0 + RC_CB_To_Model_Front.Pitch) * 0.125;
RC.Yaw = (RC.Yaw * 7.0 + RC_CB_To_Model_Front.Yaw ) * 0.125;
//------------------------------------------------------------
// Throttle is smoothed with the IIR(4) and adjusted to
// account for actual battery voltage. This is done to
// improve "hovering" when throttle stick is not moving.
//------------------------------------------------------------
// Adjust Native (from RC) throttle to a value corresponding
float BatAdjTh = RCNative.Throttle * BatNomV / ADCGetBatteryVoltage();
//-----------------------------------------
RC.Throttle = (RC.Throttle * 3 + BatAdjTh) * 0.25; // 1/4 = 0.25
//-----------------------------------------
RC.Control = RCNative.Control;
// </editor-fold>
//============================================================
//============================================================
// Implement Motor Cut-Off if RC Control is LOW
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process RC Control">
if ( 0 == RC.Control )
{
// Yes, Control is reliably low!
//--------------------------------------------
// Override motor control
//--------------------------------------------
MC.F = MC.B = MC.L = MC.R = 0;
MCMSet(&MC);
//--------------------------------------------
// Reset Timing series
StartTS = 0;
//--------------------------------------------
// Flight terminated, post EOF to Data Logger
//--------------------------------------------
TMRDelay(10); // Wait for pipe to clear
UARTPostIfReady( NULL, 0);
// ... and again - to be sure!
TMRDelay(10); // Wait for pipe to clear
UARTPostIfReady( NULL, 0);
//----------------------------------
goto Re_Start;
}
// </editor-fold>
//============================================================
//============================================================
// Obtain and process battery charge status
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process Battery level">
float BatteryCharge = QCMBatteryMgmt();
if (BatteryCharge < 0.35) // BC < 35%
{
float MaxLevel = 2.0 * BatteryCharge;
if (RC.Throttle > MaxLevel)
RC.Throttle = MaxLevel;
}
// </editor-fold>
//============================================================
//============================================================
// Read current orientation and sensor data from the IMU
// (non-blocking call)
//============================================================
if (IMU_OK == IMUGetUpdate(&IMU))
{
// Calculate motor control based
// upon IMU data
QCMPerformStep(&RC, &IMU, &MC);
}
else
{
// IMU Failed to provide update -
// set Motor Control to native throttle
MC.F = MC.B = MC.L = MC.R = RC.Throttle;
}
//============================================================
//*****************************************
// Implement Motor Cut-Off if model is
// dangerously tilted (> 60 degrees) while
// RC Throttle is low - to protect props
//----------------------------------------
if (IMU.Incl <= 0.5 && RC.Throttle <= 0.40)
{
// Override motor control
MC.F = MC.B = MC.L = MC.R = 0;
}
//----------------------------------------
//*****************************************
// Update motor control
//*****************************************
MCMSet(&MC);
//-----------------------------------------
//===========================================================
// Load and post telemetry data (non-blocking call)
//-----------------------------------------------------------
// <editor-fold defaultstate="collapsed" desc="Populating Telemetry">
StepTS = TMRGetTS();
//-----------------------------------------
if ( 0 == StartTS )
// Time stamp of cycle start!
StartTS = StepTS;
//-----------------------------------------
TM.TS = StepTS - StartTS;
//----------------------
TM.Roll = IMU.Roll;
TM.Pitch = IMU.Pitch;
TM.Yaw = IMU.Yaw;
TM.Inclination = IMU.Incl;
TM.Azimuth = IMU.Azimuth;
//----------------------
#ifdef _TMReport_GYRO_
#ifdef _TMReport_GYRO_Rotated
//------------------------------------------------------
// We rotate Gyro vector using "partial" DCM built using
// only Roll and Pitch angles as the actual Yaw does not
// affect Angualr Velocity by axis
//------------------------------------------------------
Matrix NoYawDCM;
// Generate partial rotation matrix
MatrixEulerRotation(IMU.Roll, IMU.Pitch, 0.0, &NoYawDCM);
// Rotate Gyro vector
MatrixTimesVector(&NoYawDCM, &IMU.GyroRate, &TM.Gyro);
#else
// Just report native Gyro data
VectorCopy(&IMU.GyroRate, &TM.Gyro);
#endif
#endif
//----------------------
#ifdef _TMReport_ACC_
// Just report native Gyro data
VectorCopy(&IMU.Gravity, &TM.Acc);
#endif
//----------------------
TM.RollDer = QSD.RollDer;
TM.PitchDer = QSD.PitchDer;
TM.YawDer = QSD.YawDer;
//----------------------
TM.RC_Throttle = RC.Throttle;
TM.RC_Roll = RC.Roll;
TM.RC_Pitch = RC.Pitch;
TM.RC_Yaw = RC.Yaw;
//----------------------
#ifdef _TMReport_NativeRC_
TM.RCN_Throttle = RCNative.Throttle;
TM.RCN_Roll = RCNative.Roll;
TM.RCN_Pitch = RCNative.Pitch;
TM.RCN_Yaw = RCNative.Yaw;
#endif
//----------------------
#ifdef __MXB_Use__
MXBRead(&TM.MXB);
#endif
//----------------------
#ifdef _TMReport_PID_
#ifdef _TMReport_PID_Details
TM.DRProp = QSD.DeltaRollProp;
TM.DRDiff = QSD.DeltaRollDiff;
TM.DRInt = QSD.DeltaRollInt;
#endif
TM.DRTot = QSD.DeltaRoll;
//-------------
#ifdef _TMReport_PID_Details
TM.DPProp = QSD.DeltaPitchProp;
TM.DPDiff = QSD.DeltaPitchDiff;
TM.DPInt = QSD.DeltaPitchInt;
#endif
TM.DPTot = QSD.DeltaPitch;
//-------------
#ifdef _TMReport_PID_Details
TM.DYProp = QSD.DeltaYawProp;
TM.DYDiff = QSD.DeltaYawDiff;
TM.DYInt = QSD.DeltaYawInt;
#endif
TM.DYTot = QSD.DeltaYaw;
#endif
//----------------------
TM.Throttle = QSD.Throttle; // Real Throttle
//----------------------
TM.Voltage = QSD.Voltage;
// </editor-fold>
//===========================================================
UARTPostIfReady( (unsigned char *) &TM, sizeof(TM) );
//===========================================================
// Insert controlled "delay" to slow down the Control Loop
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 1;
}
//=====================================System Initializations========================================//
void _init (void){
int i; DWORD res = 0;
//-------------------------------------General System--------------------------------------------//
SimpleUart(0,SystemBaud);assign_stdio(0); // Serial port 0 for Data
SimpleUart(1,SystemBaud); // Serial port 1 for Debug
EnableSmartTraps();
#ifdef _DEBUG
InitGDBStubNoBreak( 1, 115200 );
#endif
OSChangePrio(MAIN_PRIO); //Other
EnableSerialUpdate();
iprintf("\n\n\n\n\n\n\n\nINITIALIZATION\n");
putleds(8);
iprintf("Version Number --> %s\n", ftos(versionNo, 3));
createVERIFY_KEY(); // Using versionNo to update Flash DDBB
//--------------------------------------I2C Bus Connection---------------------------------------//
I2CInit(MCF5213_I2C_ADDRESS, I2C_FREQUENCY); // Initialize I2C with Predefined Address (0x20)
// Set frequency division to 768 (66Mhz CF clock --> 88,25 Khz I2C bus)
iprintf(" .Initialized I2C address for MCF5213: 0x%x\r\n",MCF5213_I2C_ADDRESS);
iprintf(" -Set I2C Frequency division: %x (MCF internal CLK / 1280)\r\n",I2C_FREQUENCY);
//-------------------------------------SPI Bus Connection----------------------------------------//
res=true;
for (i = 0; i<PSU_NUMBER; i++){
res &=configureSPI( INIT_SPI_ORDER, INIT_SPI_CPOL, INIT_SPI_CPHA, INIT_SPI_CLKRATE, i2CtoSPIAddress[i]);
}
if(res==I2C_OK){iprintf(" .SPI bus configured\r\n");}
else{iprintf(" .SPI configuration ERROR - Coudn't be configured\r\n");}
//-----------------------------------RDACs with minimum value------------------------------------//
res = 0;
for (i = 0; i<PSU_NUMBER; i++){
setValRDAC(INITIAL_VOLTAGE, (i&0x1?UPPER_SLAVE_SPI_ADDRESS:LOWER_SLAVE_SPI_ADDRESS), i2CtoSPIAddress[i]);
res |= getI2CResultRDAC();
}
if(res==I2C_OK){iprintf(" .RDACs set to minimum value\r\n");}
else{iprintf(" .RDAC setting ERROR - Couldn't be set to minimum value\r\n");}
//----------------------------------------DataLists----------------------------------------------//
initializeValuesPSUsSnIs(); // Load psuList values from RAM or set them to default
for (i = 0; i<PSU_NUMBER; i++){ // Start all PSU objects in OFF status, regardless of the loaded status
psuList[i].relayStatus = OFF;
psuList[i].psuStatus = OFF;
psuList[i].ReadyToConnect = false;
}
//-------------------------------------------GPIO------------------------------------------------//
RESET_RHEOSTATS.function( PIN21_GPIO ); // RDAC Reset
LED_OUT_ON.function( PIN7_GPIO ); // LED OUT
iprintf(" .Pin 21 (TIN3) set as GPIO for RDAC Reset\r\n");
iprintf(" .Pin 7 (IRQ4) set as GPIO for LED OUT\r\n");
//----------------------------------Analog to Digital Converter----------------------------------//
Pins[13].function( PIN13_AN0 ); // Configure the A2D pin nº13 as analog input
EnableAD();
initMUX(); // Initializes all the Pins used for Mux control
res=configureSPI( INIT_SPI_ORDER, INIT_SPI_CPOL, INIT_SPI_CPHA, INIT_SPI_CLKRATE, AGC_I2C_ADDRESS);
minAGC(); // Initializes the AGC with minimum gain
res = (getI2CResultAGC()==I2C_OK);
if(res){iprintf(" .ADC initialized\r\n");}
else{iprintf(" .ADC initialization ERROR\r\n");}
//--------------------------------------Button Interruption--------------------------------------//
SetUpIRQ1();
iprintf(" .Button Interruption Set\r\n");
//---------------------------------------TimerInterruptions--------------------------------------//
//SetUpPITR(0, 8294, 1); //Use PITR 0, Wait 8294 clocks, Divide by 2 from table 17-3, 2KHz - NOT IN USE, messes with RTOS Tasks. Best left unused
SetUpPITR(1, 16588, 1); // Use PITR 1, Wait 16588 clocks, Divide by 2 from table 17-3, 1KHz - Interrupt in charge of A2D sampling
iprintf(" .Timers initialized\r\n");
//----------------------------------------- RTOS Tasks-------------------------------------------//
OSSimpleTaskCreate( alarmTask, ALARM_PRIO );
iprintf(" .alarmTask initialized\r\n");
iprintf(" .MonitorSem %s\n", (allSemInit()==OS_NO_ERR?"Initialized CORRECTLY":"Initialization ERROR"));
OSSimpleTaskCreate( monitorTask, MONITOR_PRIO );
iprintf(" .monitorTask initialized\r\n");
iprintf("APPLICATION INITIALIZED\n\nPress one key to begin \n\n");
WAIT_FOR_KEYBOARD
putleds(0);
}
/*******************************************************************************
** Main Function main()
*******************************************************************************/
int main (void)
{
uint32_t i;
SystemInit();
if ( I2CInit( (uint32_t)I2CMASTER ) == FALSE ) /* initialize I2c */
{
while ( 1 ); /* Fatal error */
}
/* In order to start the I2CEngine, the all the parameters
must be set in advance, including I2CWriteLength, I2CReadLength,
I2CCmd, and the I2cMasterBuffer which contains the stream
command/data to the I2c slave device.
(1) If it's a I2C write only, the number of bytes to be written is
I2CWriteLength, I2CReadLength is zero, the content will be filled
in the I2CMasterBuffer.
(2) If it's a I2C read only, the number of bytes to be read is
I2CReadLength, I2CWriteLength is 0, the read value will be filled
in the I2CMasterBuffer.
(3) If it's a I2C Write/Read with repeated start, specify the
I2CWriteLength, fill the content of bytes to be written in
I2CMasterBuffer, specify the I2CReadLength, after the repeated
start and the device address with RD bit set, the content of the
reading will be filled in I2CMasterBuffer index at
I2CMasterBuffer[I2CWriteLength+2].
e.g. Start, DevAddr(W), WRByte1...WRByteN, Repeated-Start, DevAddr(R),
RDByte1...RDByteN Stop. The content of the reading will be filled
after (I2CWriteLength + two devaddr) bytes. */
/* Write SLA(W), address and one data byte */
I2CWriteLength = 6;
I2CReadLength = 0;
I2CMasterBuffer[0] = PCF8594_ADDR;
I2CMasterBuffer[1] = 0x00; /* address */
I2CMasterBuffer[2] = 0x55; /* Data0 */
I2CMasterBuffer[3] = 0xAA; /* Data1 */
I2CMasterBuffer[4] = 0x12; /* Data0 */
I2CMasterBuffer[5] = 0x34; /* Data1 */
I2CEngine();
/* Be careful with below fixed delay. From device to device, or
even same device with different write length, or various I2C clock,
below delay length may need to be changed accordingly. Having
a break point before Write/Read start will be helpful to isolate
the problem. */
for ( i = 0; i < 0x200000; i++ ); /* Delay after write */
for ( i = 0; i < BUFSIZE; i++ )
{
I2CSlaveBuffer[i] = 0x00;
}
/* Write SLA(W), address, SLA(R), and read one byte back. */
I2CWriteLength = 2;
I2CReadLength = 4;
I2CMasterBuffer[0] = PCF8594_ADDR;
I2CMasterBuffer[1] = 0x00; /* address */
I2CMasterBuffer[2] = PCF8594_ADDR | RD_BIT;
I2CEngine();
return 0;
}
/**
* Constructs the AHRS class using I2C communication and the default update rate.
*<p>
* This constructor should be used if communicating via I2C.
*<p>
* @param i2c_port_id I2C port to use
*/
AHRS::AHRS(I2C::Port i2c_port_id) {
I2CInit(i2c_port_id, NAVX_DEFAULT_UPDATE_RATE_HZ);
}
