void booz2_analog_init_hw( void ) {
/* start ADC0 */
/* select P0.29 as AD0.2 for bat meas*/
PINSEL1 |= 0x01 << 26;
/* sample AD0.2 - PCLK/4 ( 3.75MHz) - ON */
AD0CR = 1 << 2 | 0x03 << 8 | 1 << 21;
/* AD0 selected as IRQ */
VICIntSelect &= ~VIC_BIT(VIC_AD0);
/* AD0 interrupt enabled */
VICIntEnable = VIC_BIT(VIC_AD0);
/* AD0 interrupt as VIC2 */
_VIC_CNTL(ADC0_VIC_SLOT) = VIC_ENABLE | VIC_AD0;
_VIC_ADDR(ADC0_VIC_SLOT) = (uint32_t)ADC0_ISR;
/* start convertion on T0M1 match */
AD0CR |= 4 << 24;
/* clear match 1 */
T0EMR &= ~TEMR_EM1;
/* set high on match 1 */
T0EMR |= TEMR_EMC1_2;
/* first match in a while */
T0MR1 = 1024;
/* start ADC1 */
/* select P0.10 as AD1.2 for baro*/
ANALOG_BARO_PINSEL |= ANALOG_BARO_PINSEL_VAL << ANALOG_BARO_PINSEL_BIT;
/* sample AD1.2 - PCLK/4 ( 3.75MHz) - ON */
AD1CR = 1 << 2 | 0x03 << 8 | 1 << 21;
/* AD0 selected as IRQ */
VICIntSelect &= ~VIC_BIT(VIC_AD1);
/* AD0 interrupt enabled */
VICIntEnable = VIC_BIT(VIC_AD1);
/* AD0 interrupt as VIC2 */
_VIC_CNTL(ADC1_VIC_SLOT) = VIC_ENABLE | VIC_AD1;
_VIC_ADDR(ADC1_VIC_SLOT) = (uint32_t)ADC1_ISR;
/* start convertion on T0M3 match */
AD1CR |= 5 << 24;
/* clear match 2 */
T0EMR &= ~TEMR_EM3;
/* set high on match 2 */
T0EMR |= TEMR_EMC3_2;
/* first match in a while */
T0MR3 = 512;
/* turn on DAC pins */
PINSEL1 |= 2 << 18;
}
void max3100_init( void ) {
max3100_status = MAX3100_STATUS_IDLE;
max3100_data_available = false;
max3100_transmit_buffer_empty = true;
max3100_tx_insert_idx = 0;
max3100_tx_extract_idx = 0;
max3100_rx_insert_idx = 0;
max3100_rx_extract_idx = 0;
/* setup pins for SSP (SCK, MISO, MOSI) */
PINSEL1 |= PINSEL1_SCK | PINSEL1_MISO | PINSEL1_MOSI;
/* setup SSP */
SSPCR0 = SSP_DSS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR;
SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD;
SSPCPSR = SSPCPSR_VAL; /* Prescaler */
/* From arm7/max1167_hw.c */
/* SS pin is output */
SetBit(MAX3100_SS_IODIR, MAX3100_SS_PIN);
/* unselected max3100 */
Max3100Unselect();
/* connect extint (IRQ) */
MAX3100_IRQ_PINSEL |= MAX3100_IRQ_PINSEL_VAL << MAX3100_IRQ_PINSEL_BIT;
/* extint is edge trigered */
SetBit(EXTMODE, MAX3100_IRQ_EINT);
/* extint is trigered on falling edge */
ClearBit(EXTPOLAR, MAX3100_IRQ_EINT);
/* clear pending extint0 before enabling interrupts */
SetBit(EXTINT, MAX3100_IRQ_EINT);
/* Configure interrupt vector for external pin interrupt */
VICIntSelect &= ~VIC_BIT( MAX3100_VIC_EINT ); // EXTINT selected as IRQ
VICIntEnable = VIC_BIT( MAX3100_VIC_EINT ); // EXTINT interrupt enabled
VICVectCntl8 = VIC_ENABLE | MAX3100_VIC_EINT;
VICVectAddr8 = (uint32_t)EXTINT_ISR; // address of the ISR
/* Configure interrupt vector for SPI */
VICIntSelect &= ~VIC_BIT(VIC_SPI1); /* SPI1 selected as IRQ */
VICIntEnable = VIC_BIT(VIC_SPI1); /* SPI1 interrupt enabled */
_VIC_CNTL(SPI1_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
_VIC_CNTL(SPI1_VIC_SLOT) = (uint32_t)SPI1_ISR; /* address of the ISR */
/* Write configuration */
//Max3100TransmitConf(MAX3100_BAUD_RATE | MAX3100_BIT_NOT_TM);
Max3100TransmitConf(MAX3100_BAUD_RATE | MAX3100_BIT_NOT_RM | MAX3100_BIT_NOT_TM);
}
void
imu_init(void)
{
/* initialises neutrals */
RATES_ASSIGN(booz_imu.gyro_neutral, IMU_NEUTRAL_GYRO_P, IMU_NEUTRAL_GYRO_Q, IMU_NEUTRAL_GYRO_R);
VECT3_ASSIGN(booz_imu.accel_neutral, IMU_NEUTRAL_ACCEL_X, IMU_NEUTRAL_ACCEL_Y, IMU_NEUTRAL_ACCEL_Z);
VECT3_ASSIGN(booz_imu.mag_neutral, IMU_NEUTRAL_MAG_X, IMU_NEUTRAL_MAG_Y, IMU_NEUTRAL_MAG_Z);
/* initialise IMU alignment */
imu_adjust_alignment(IMU_ALIGNMENT_BODY_TO_IMU_PHI, IMU_ALIGNMENT_BODY_TO_IMU_THETA, IMU_ALIGNMENT_BODY_TO_IMU_PSI);
imu_spi_selected = SPI_NONE;
do_max1168_read = FALSE;
/* setup pins for SSP (SCK, MISO, MOSI) */
PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI;
/* setup SSP */
SSPCR0 = SSPCR0_VAL;;
SSPCR1 = SSPCR1_VAL;
SSPCPSR = 0x02;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
_VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
_VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */
max1168_init();
micromag_init();
}
void sys_time_arch_init( void ) {
/* setup Timer 0 to count forever */
/* reset & disable timer 0 */
T0TCR = TCR_RESET;
/* set the prescale divider */
T0PR = T0_PCLK_DIV - 1;
/* enable interrupt on match0 for sys_ticks */
T0MCR = TMCR_MR0_I;
/* disable capture registers */
T0CCR = 0;
/* disable external match register */
T0EMR = 0;
/* set first sys tick interrupt */
T0MR0 = SYS_TIME_RESOLUTION_CPU_TICKS;
/* enable timer 0 */
T0TCR = TCR_ENABLE;
/* select TIMER0 as IRQ */
VICIntSelect &= ~VIC_BIT(VIC_TIMER0);
/* enable TIMER0 interrupt */
VICIntEnable = VIC_BIT(VIC_TIMER0);
/* on slot vic slot 1 */
_VIC_CNTL(TIMER0_VIC_SLOT) = VIC_ENABLE | VIC_TIMER0;
/* address of the ISR */
_VIC_ADDR(TIMER0_VIC_SLOT) = (uint32_t)TIMER0_ISR;
}
void uart0_init( void ) {
uart_periph_init(&uart0);
uart0.reg_addr = UART0_BASE;
#ifdef USE_UART0_RX_ONLY
// only use the RX0 P0.1 pin, no TX
PINSEL0 = (PINSEL0 & ~U0_PINMASK_RX) | U0_PINSEL_RX;
#else
// set port pins for UART0
PINSEL0 = (PINSEL0 & ~U0_PINMASK) | U0_PINSEL;
#endif
// initialize uart parameters
uart_disable_interrupts(&uart0);
uart_set_baudrate(&uart0, UART0_BAUD);
// initialize the interrupt vector
VICIntSelect &= ~VIC_BIT(VIC_UART0); // UART0 selected as IRQ
VICIntEnable = VIC_BIT(VIC_UART0); // UART0 interrupt enabled
_VIC_CNTL(UART0_VIC_SLOT) = VIC_ENABLE | VIC_UART0;
_VIC_ADDR(UART0_VIC_SLOT) = (uint32_t)uart0_ISR; // address of the ISR
uart_enable_interrupts(&uart0);
}
void ms2001_arch_init( void ) {
ms2001_cur_axe = 0;
/* configure SS pin */
Ms2001Unselect(); /* pin idles high */
SetBit(MS2001_SS_IODIR, MS2001_SS_PIN); /* pin is output */
/* configure RESET pin */
Ms2001Reset(); /* pin idles low */
SetBit(MS2001_RESET_IODIR, MS2001_RESET_PIN); /* pin is output */
/* configure DRDY pin */
/* connected pin to EXINT */
MS2001_DRDY_PINSEL |= MS2001_DRDY_PINSEL_VAL << MS2001_DRDY_PINSEL_BIT;
SetBit(EXTMODE, MS2001_DRDY_EINT); /* EINT is edge trigered */
SetBit(EXTPOLAR,MS2001_DRDY_EINT); /* EINT is trigered on rising edge */
SetBit(EXTINT,MS2001_DRDY_EINT); /* clear pending EINT */
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( MS2001_DRDY_VIC_IT ); /* select EINT as IRQ source */
VICIntEnable = VIC_BIT( MS2001_DRDY_VIC_IT ); /* enable it */
_VIC_CNTL(MS2001_DRDY_VIC_SLOT) = VIC_ENABLE | MS2001_DRDY_VIC_IT;
_VIC_ADDR(MS2001_DRDY_VIC_SLOT) = (uint32_t)EXTINT_ISR; // address of the ISR
}
void sys_time_init( void ) {
/* setup Timer 0 to count forever */
/* reset & disable timer 0 */
T0TCR = TCR_RESET;
/* set the prescale divider */
T0PR = T0_PCLK_DIV - 1;
/* disable match registers */
T0MCR = 0;
/* disable compare registers */
T0CCR = 0;
/* disable external match register */
T0EMR = 0;
/* enable timer 0 */
T0TCR = TCR_ENABLE;
cpu_time_sec = 0;
cpu_time_ticks = 0;
last_periodic_event = 0;
/* select TIMER0 as IRQ */
VICIntSelect &= ~VIC_BIT(VIC_TIMER0);
/* enable TIMER0 interrupt */
VICIntEnable = VIC_BIT(VIC_TIMER0);
/* on slot vic slot 1 */
_VIC_CNTL(TIMER0_VIC_SLOT) = VIC_ENABLE | VIC_TIMER0;
/* address of the ISR */
_VIC_ADDR(TIMER0_VIC_SLOT) = (uint32_t)TIMER0_ISR;
}
void uart1_init( void ) {
uart_periph_init(&uart1);
uart1.reg_addr = UART1_BASE;
#ifdef USE_UART1_RX_ONLY
// only use the RX1 P0.9 pin, no TX
PINSEL0 = (PINSEL0 & ~U1_PINMASK_RX) | U1_PINSEL_RX;
#else
// set port pins for UART1
PINSEL0 = (PINSEL0 & ~U1_PINMASK) | U1_PINSEL;
#endif
uart_disable_interrupts(&uart1);
uart_set_baudrate(&uart1, UART1_BAUD);
// initialize the interrupt vector
VICIntSelect &= ~VIC_BIT(VIC_UART1); // UART1 selected as IRQ
VICIntEnable = VIC_BIT(VIC_UART1); // UART1 interrupt enabled
_VIC_CNTL(UART1_VIC_SLOT) = VIC_ENABLE | VIC_UART1;
_VIC_ADDR(UART1_VIC_SLOT) = (uint32_t)uart1_ISR; // address of the ISR
// enable receiver interrupts
uart_enable_interrupts(&uart1);
}
void max11040_hw_init( void ) {
int i;
/* *** configure SPI *** */
/* setup pins for SSP (SCK, MISO, MOSI, SSEL) */
PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI | SSP_PINSEL1_SSEL;
/* setup SSP */
SSPCR0 = SSPCR0_VAL;;
SSPCR1 = SSPCR1_VAL;
SSPCPSR = 0x02;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
_VIC_CNTL(SPI1_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
_VIC_ADDR(SPI1_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */
/* *** configure DRDY pin*** */
/* connected pin to EXINT */
MAXM_DRDY_PINSEL |= MAXM_DRDY_PINSEL_VAL << MAXM_DRDY_PINSEL_BIT;
SetBit(EXTMODE, MAXM_DRDY_EINT); /* EINT is edge trigered */
ClearBit(EXTPOLAR, MAXM_DRDY_EINT); /* EINT is trigered on falling edge */
SetBit(EXTINT, MAXM_DRDY_EINT); /* clear pending EINT */
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( MAXM_DRDY_VIC_IT ); /* select EINT as IRQ source */
VICIntEnable = VIC_BIT( MAXM_DRDY_VIC_IT ); /* enable it */
_VIC_CNTL(MAX11040_DRDY_VIC_SLOT) = VIC_ENABLE | MAXM_DRDY_VIC_IT;
_VIC_ADDR(MAX11040_DRDY_VIC_SLOT) = (uint32_t)EXTINT_ISR; /* address of the ISR */
/* write configuration register */
SSP_Send(0x60); /* wr conf */
for (i=0; i<MAXM_NB_ADCS; i++) {
SSP_Send(0x40); /* adcx: reset */
}
SSP_Enable();
SSP_ClearRti();
SSP_EnableRti();
}
void i2c_init(void)
{
/* setup pins for I2C0 (SCL0, SDA0) and I2C1 (SCL1, SDA1) */
PINSEL0 |= I2C0_PINSEL0_SCL | I2C0_PINSEL0_SDA | I2C1_PINSEL0_SCL
| I2C1_PINSEL0_SDA;
/* setup I2C0 */
I2C0ADR = LPC_I2C_ADR;
I2C0SCLH = I2C0SCLH_VAL;
I2C0SCLL = I2C0SCLL_VAL;
I2C0CONSET = I2C0CONSET_VAL;
/* initialize I2C0 interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_I2C0 ); /* I2C0 selected as IRQ */
VICIntEnable = VIC_BIT( VIC_I2C0 ); /* enable it */
_VIC_CNTL(I2C0_VIC_SLOT) = VIC_ENABLE | VIC_I2C0;
_VIC_ADDR(I2C0_VIC_SLOT) = (unsigned int) I2C0_ISR; /* address of the I2C0 ISR */
/* setup I2C1 */
I2C1ADR = LPC_I2C_ADR;
I2C1SCLH = I2C1SCLH_VAL;
I2C1SCLL = I2C1SCLL_VAL;
I2C1CONSET = I2C1CONSET_VAL;
/* initialize I2C1 interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_I2C1 ); /* I2C1 selected as IRQ */
VICIntEnable = VIC_BIT( VIC_I2C1 ); /* enable it */
_VIC_CNTL(I2C_VIC_SLOT) = VIC_ENABLE | VIC_I2C1;
_VIC_ADDR(I2C_VIC_SLOT) = (unsigned int) I2C1_ISR; /* address of the I2C1 ISR */
// insertion and processing pointers for I2C0
i2c_package_buffer0_insert_idx = 0;
i2c_package_buffer0_current_idx = 0;
i2c_package_buffer1_insert_idx = 0;
i2c_package_buffer1_current_idx = 0;
// status variables for I2C Bus 0 and 1
i2c0_busy = 0;
i2c1_busy = 0;
}
void csc_me4_link_init(void) {
/* setup pin mux for SPI1 (SCK, MISO, MOSI, SS) */
PINSEL1 |= PINSEL1_SCK | PINSEL1_MISO | PINSEL1_MOSI | PINSEL1_SSEL;
/* configure SPI1 */
S0SPCR = S1SPCR_VAL;
S1SPCCR = S1SPCCR_VAL;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ
VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled
_VIC_CNTL(CSC_ME4_LINK_VIC_SLOT) = VIC_ENABLE | VIC_SPI0;
_VIC_ADDR(CSC_ME4_LINK_VIC_SLOT) = (uint32_t)SPI1_ISR; // address of the ISR
}
void micromag_hw_init( void ) {
/* setup pins for SSP (SCK, MISO, MOSI, SSEL) */
PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI;
/* setup SSP */
SSPCR0 = SSPCR0_VAL;;
SSPCR1 = SSPCR1_VAL;
SSPCPSR = 0x02;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
_VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
_VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */
MmUnselect(); /* pin idles high */
/* configure SS pin */
SetBit(MM_SS_IODIR, MM_SS_PIN); /* pin is output */
/* configure RESET pin */
SetBit(MM_RESET_IODIR, MM_RESET_PIN); /* pin is output */
MmReset(); /* pin idles low */
/* configure DRDY pin */
/* connected pin to EXINT */
MM_DRDY_PINSEL |= MM_DRDY_PINSEL_VAL << MM_DRDY_PINSEL_BIT;
SetBit(EXTMODE, MM_DRDY_EINT); /* EINT is edge trigered */
SetBit(EXTPOLAR,MM_DRDY_EINT); /* EINT is trigered on rising edge */
SetBit(EXTINT,MM_DRDY_EINT); /* clear pending EINT */
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( MM_DRDY_VIC_IT ); /* select EINT as IRQ source */
VICIntEnable = VIC_BIT( MM_DRDY_VIC_IT ); /* enable it */
_VIC_CNTL(MICROMAG_DRDY_VIC_SLOT) = VIC_ENABLE | MM_DRDY_VIC_IT;
_VIC_ADDR(MICROMAG_DRDY_VIC_SLOT) = (uint32_t)EXTINT_ISR; // address of the ISR
}
void baro_scp_init(void)
{
/* setup pins for SSP (SCK, MISO, MOSI) */
PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6;
/* setup SSP */
SSPCR0 = SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR;
SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD;
/* set prescaler for SSP clock */
SSPCPSR = PCLK / SSP_CLOCK;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ
VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled
_VIC_CNTL(SPI1_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
_VIC_CNTL(SPI1_VIC_SLOT) = (uint32_t)SPI1_ISR; /* address of the ISR */
/* configure SS pin */
SetBit(SS_IODIR, SS_PIN); /* pin is output */
ScpUnselect(); /* pin idles high */
/* configure DRDY pin */
/* connected pin to EXINT */
SPI1_DRDY_PINSEL |= SPI1_DRDY_PINSEL_VAL << SPI1_DRDY_PINSEL_BIT;
SetBit(EXTMODE, SPI1_DRDY_EINT); /* EINT is edge trigered */
SetBit(EXTPOLAR, SPI1_DRDY_EINT); /* EINT is trigered on rising edge */
SetBit(EXTINT, SPI1_DRDY_EINT); /* clear pending EINT */
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(SPI1_DRDY_VIC_IT); /* select EINT as IRQ source */
VICIntEnable = VIC_BIT(SPI1_DRDY_VIC_IT); /* enable it */
VICVectCntl11 = VIC_ENABLE | SPI1_DRDY_VIC_IT;
VICVectAddr11 = (uint32_t)EXTINT_ISR; // address of the ISR
baro_scp_status = STA_UNINIT;
}
void max1168_arch_init( void ) {
/* connect P0.16 to extint0 (EOC) */
MAX1168_EOC_PINSEL |= MAX1168_EOC_PINSEL_VAL << MAX1168_EOC_PINSEL_BIT;
/* extint0 is edge trigered */
SetBit(EXTMODE, MAX1168_EOC_EINT);
/* extint0 is trigered on falling edge */
ClearBit(EXTPOLAR, MAX1168_EOC_EINT);
/* clear pending extint0 before enabling interrupts */
SetBit(EXTINT, MAX1168_EOC_EINT);
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_EINT0 ); // EXTINT0 selected as IRQ
VICIntEnable = VIC_BIT( VIC_EINT0 ); // EXTINT0 interrupt enabled
_VIC_CNTL(MAX1168_EOC_VIC_SLOT) = VIC_ENABLE | VIC_EINT0;
_VIC_ADDR(MAX1168_EOC_VIC_SLOT) = (uint32_t)EXTINT0_ISR; // address of the ISR
}
void spi_init(void){
/* setup pins for SSP (SCK, MISO, MOSI) */
PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI;
/* setup SSP */
SSPCR0 = SSPCR0_VAL;
SSPCR1 = SSPCR1_VAL;
SSPCPSR = 0x02;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
_VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
_VIC_ADDR(SSP_VIC_SLOT) = (unsigned int)SSP_ISR; /* address of the ISR */
spi_transmit_running = 0;
spi_package_buffer_insert_idx = 0;
spi_package_buffer_extract_idx = 0;
}
void actuators_4015_init ( void ) {
/* PWM selected as IRQ */
VICIntSelect &= ~VIC_BIT(VIC_PWM);
/* PWM interrupt enabled */
VICIntEnable = VIC_BIT(VIC_PWM);
_VIC_CNTL(PWM_VIC_SLOT) = VIC_ENABLE | VIC_PWM;
/* address of the ISR */
_VIC_ADDR(PWM_VIC_SLOT) = (uint32_t)PWM_ISR;
/* set clock, data and reset pins as output */
IO0DIR |= _BV(SERV0_CLOCK_PIN);
IO1DIR |= _BV(SERV0_DATA_PIN) | _BV(SERV0_RESET_PIN);
IO0DIR |= _BV(SERV1_CLOCK_PIN);
IO1DIR |= _BV(SERV1_DATA_PIN) | _BV(SERV1_RESET_PIN);
/* Configure the two servo clocks as PWM */
SERV0_CLOCK_PINSEL |= SERV0_CLOCK_PINSEL_VAL << SERV0_CLOCK_PINSEL_BIT;
SERV1_CLOCK_PINSEL |= SERV1_CLOCK_PINSEL_VAL << SERV1_CLOCK_PINSEL_BIT;
/* set first pulse to be very long */
PWMMR0 = 0XFFFFFF;
PWMMR_SERV1 = 0XFFF;
PWMMR_SERV0 = 0X0;
/* commit above changes */
PWMLER = PWMLER_LATCH0 | PWMLER_LATCH_SERV0 | PWMLER_LATCH_SERV1;
/* enable interrupt on serv1 PWM match */
PWMMCR = PWMMCR_MR0R | PWMMCR_MRI_SERV1;
/* enable PWM ouptputs */
PWMPCR = PWMPCR_ENA_SERV0 | PWMPCR_ENA_SERV1;
/* Prescaler */
PWMPR = PWM_PRESCALER-1;
/* enable PWM timer counter and PWM mode */
PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
/* Load failsafe values */
/* Set all servos at their midpoints */
/* compulsory for unaffected servos */
uint8_t i;
for( i=0 ; i < _4015_NB_CHANNELS ; i++ )
servos_values[i] = SERVOS_TICS_OF_USEC(1500);
#ifdef SERVO_GAZ
servos_values[SERVO_GAZ] = SERVOS_TICS_OF_USEC(SERVO_GAZ_NEUTRAL);
#endif
}
void imu_b2_arch_init(void) {
imu_ssp_status = IMU_SSP_STA_IDLE;
/* setup pins for SSP (SCK, MISO, MOSI) */
PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI;
/* setup SSP */
SSPCR0 = SSPCR0_VAL16;
SSPCR1 = SSPCR1_VAL;
SSPCPSR = 0x02;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */
VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */
_VIC_CNTL(SSP_VIC_SLOT) = VIC_ENABLE | VIC_SPI1;
_VIC_ADDR(SSP_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */
}
/* SCL0 on P0.2 */
void i2c0_hw_init ( void ) {
i2c0.reg_addr = I2C0;
/* set P0.2 and P0.3 to I2C0 */
PINSEL0 |= 1 << 4 | 1 << 6;
/* clear all flags */
I2C0CONCLR = _BV(AAC) | _BV(SIC) | _BV(STAC) | _BV(I2ENC);
/* enable I2C */
I2C0CONSET = _BV(I2EN);
/* set bitrate */
I2C0SCLL = I2C0_SCLL_D;
I2C0SCLH = I2C0_SCLH_D;
// initialize the interrupt vector
VICIntSelect &= ~VIC_BIT(VIC_I2C0); // I2C0 selected as IRQ
VICIntEnable = VIC_BIT(VIC_I2C0); // I2C0 interrupt enabled
_VIC_CNTL(I2C0_VIC_SLOT) = VIC_ENABLE | VIC_I2C0;
_VIC_ADDR(I2C0_VIC_SLOT) = (uint32_t)i2c0_ISR; // address of the ISR
}
/* SCL1 on P0.11 */
void i2c1_hw_init ( void ) {
i2c1.reg_addr = I2C1;
/* set P0.11 and P0.14 to I2C1 */
PINSEL0 |= 3 << 22 | 3 << 28;
/* clear all flags */
I2C1CONCLR = _BV(AAC) | _BV(SIC) | _BV(STAC) | _BV(I2ENC);
/* enable I2C */
I2C1CONSET = _BV(I2EN);
/* set bitrate */
I2C1SCLL = I2C1_SCLL_D;
I2C1SCLH = I2C1_SCLH_D;
// initialize the interrupt vector
VICIntSelect &= ~VIC_BIT(VIC_I2C1); // I2C1 selected as IRQ
VICIntEnable = VIC_BIT(VIC_I2C1); // I2C1 interrupt enabled
_VIC_CNTL(I2C1_VIC_SLOT) = VIC_ENABLE | VIC_I2C1;
_VIC_ADDR(I2C1_VIC_SLOT) = (uint32_t)i2c1_ISR; // address of the ISR
}
void scp1000_init(void) {
scp1000_conf_package.bit_mode = SPI_8_BIT_MODE;
scp1000_conf_package.data[0] = 0x03 << 2 | 0x02;
scp1000_conf_package.data[1] = SCP_MEAS_MODE;
scp1000_conf_package.length = 2;
scp1000_conf_package.slave_select = &scp1000_select;
scp1000_conf_package.slave_unselect = &scp1000_unselect;
scp1000_conf_package.spi_interrupt_handler = &scp1000_on_spi_int_conf;
scp1000_read_package.bit_mode = SPI_8_BIT_MODE;
scp1000_read_package.data[0] = 0x1F << 2;
scp1000_read_package.data[1] = 0;
scp1000_read_package.data[2] = 0x20 << 2;
scp1000_read_package.data[3] = 0;
scp1000_read_package.data[4] = 0;
scp1000_read_package.length = 5;
scp1000_read_package.slave_select = &scp1000_select;
scp1000_read_package.slave_unselect = &scp1000_unselect;
scp1000_read_package.spi_interrupt_handler = &scp1000_on_spi_int_read;
/* configure SS pin */
SCP_SS_IODIR |= 1 << SCP_SS_PIN; /* pin is output */
scp1000_unselect(); /* pin idles high */
/* configure DRDY pin */
/* connected pin to EXINT */
SCP_DRDY_PINSEL |= SCP_DRDY_PINSEL_VAL << SCP_DRDY_PINSEL_BIT;
EXTMODE |= 1 << SCP_DRDY_EINT; /* EINT is edge trigered */
EXTPOLAR |= 1 << SCP_DRDY_EINT; /* EINT is trigered on rising edge */
EXTINT |= 1 << SCP_DRDY_EINT; /* clear pending EINT */
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(SCP_DRDY_VIC_IT); /* select EINT as IRQ source */
VICIntEnable = VIC_BIT(SCP_DRDY_VIC_IT); /* enable it */
_VIC_CNTL( SCP1000_EOC_VIC_SLOT) = VIC_ENABLE | SCP_DRDY_VIC_IT;
_VIC_ADDR( SCP1000_EOC_VIC_SLOT) = (unsigned int) EXTINT_ISR; // address of the ISR
}
void uart1_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) {
// set port pins for UART1
PINSEL0 = (PINSEL0 & ~U1_PINMASK) | U1_PINSEL;
U1IER = 0x00; // disable all interrupts
U1IIR; // clear interrupt ID
U1RBR; // clear receive register
U1LSR; // clear line status register
// set the baudrate
U1LCR = ULCR_DLAB_ENABLE; // select divisor latches
U1DLL = (uint8_t)baud; // set for baud low byte
U1DLM = (uint8_t)(baud >> 8); // set for baud high byte
// set the number of characters and other
// user specified operating parameters
U1LCR = (mode & ~ULCR_DLAB_ENABLE);
U1FCR = fmode;
// initialize the interrupt vector
VICIntSelect &= ~VIC_BIT(VIC_UART1); // UART1 selected as IRQ
VICIntEnable = VIC_BIT(VIC_UART1); // UART1 interrupt enabled
_VIC_CNTL(UART1_VIC_SLOT) = VIC_ENABLE | VIC_UART1;
_VIC_ADDR(UART1_VIC_SLOT) = (uint32_t)uart1_ISR; // address of the ISR
// initialize the transmit data queue
uart1_tx_extract_idx = 0;
uart1_tx_insert_idx = 0;
uart1_tx_running = 0;
// initialize the receive data queue
uart1_rx_extract_idx = 0;
uart1_rx_insert_idx = 0;
// enable receiver interrupts
U1IER = UIER_ERBFI;
}
void sys_time_arch_init( void ) {
sys_time.cpu_ticks_per_sec = PCLK / T0_PCLK_DIV;
/* cpu ticks per desired sys_time timer step */
sys_time.resolution_cpu_ticks = (uint32_t)(sys_time.resolution_sec * sys_time.cpu_ticks_per_sec + 0.5);
/* set final sys_time resolution in seconds from resolution in cpu_ticks */
sys_time.resolution_sec = (float)sys_time.resolution_cpu_ticks / sys_time.cpu_ticks_per_sec;
sys_time.ticks_per_sec = (uint32_t)(sys_time.cpu_ticks_per_sec / sys_time.resolution_cpu_ticks + 0.5);
/* setup Timer 0 to count forever */
/* reset & disable timer 0 */
T0TCR = TCR_RESET;
/* set the prescale divider */
T0PR = T0_PCLK_DIV - 1;
/* enable interrupt on match0 for sys_ticks */
T0MCR = TMCR_MR0_I;
/* disable capture registers */
T0CCR = 0;
/* disable external match register */
T0EMR = 0;
/* set first sys tick interrupt */
T0MR0 = sys_time.resolution_cpu_ticks;
/* enable timer 0 */
T0TCR = TCR_ENABLE;
/* select TIMER0 as IRQ */
VICIntSelect &= ~VIC_BIT(VIC_TIMER0);
/* enable TIMER0 interrupt */
VICIntEnable = VIC_BIT(VIC_TIMER0);
/* on slot vic slot 1 */
_VIC_CNTL(TIMER0_VIC_SLOT) = VIC_ENABLE | VIC_TIMER0;
/* address of the ISR */
_VIC_ADDR(TIMER0_VIC_SLOT) = (uint32_t)TIMER0_ISR;
}