diff --git a/sw/airborne/arm7/max1167_hw.c b/sw/airborne/arm7/max1167_hw.c new file mode 100644 index 0000000000..75030c1ce8 --- /dev/null +++ b/sw/airborne/arm7/max1167_hw.c @@ -0,0 +1,76 @@ +#include "max1167.h" + + +static void EXTINT0_ISR(void) __attribute__((naked)); + +extern void max1167_hw_init( void ) { + /* CS pin is output */ + SetBit(MAX1167_SS_IODIR, MAX1167_SS_PIN); + /* unselected max1167 */ + Max1167Unselect(); + + /* connect P0.16 to extint0 (EOC) */ + MAX1167_EOC_PINSEL |= MAX1167_EOC_PINSEL_VAL << MAX1167_EOC_PINSEL_BIT; + /* extint0 is edge trigered */ + SetBit(EXTMODE, MAX1167_EOC_EINT); + /* extint0 is trigered on falling edge */ + ClearBit(EXTPOLAR, MAX1167_EOC_EINT); + /* clear pending extint0 before enabling interrupts */ + SetBit(EXTINT, MAX1167_EOC_EINT); + + /* initialize interrupt vector */ + VICIntSelect &= ~VIC_BIT( VIC_EINT0 ); // EXTINT0 selected as IRQ + VICIntEnable = VIC_BIT( VIC_EINT0 ); // EXTINT0 interrupt enabled + VICVectCntl8 = VIC_ENABLE | VIC_EINT0; + VICVectAddr8 = (uint32_t)EXTINT0_ISR; // address of the ISR + + + +} + +void max1167_read( void ) { + if (max1167_status == STA_MAX1167_IDLE) { + /* select max1167 */ + Max1167Select(); + /* enable SPI */ + SpiEnable(); + /* write control byte - wait for RTI in spi handler */ + const uint8_t control_byte = 1 << 0 | 1 << 3 | 2 << 5; + SpiSend(control_byte); + /* enable timeout interrupt */ + SpiEnableRti(); + max1167_status = STA_MAX1167_SENDING_REQ; + } + else { + /* report overrun error */ + } +} + +void EXTINT0_ISR(void) { + ISR_ENTRY(); + + if (max1167_status == STA_MAX1167_WAIT_EOC) { + /* Max1167Select(); maybe... */ + SpiEnable(); + /* trigger 6 bytes read */ + SpiSend(0); + SpiSend(0); + SpiSend(0); + SpiSend(0); + SpiSend(0); + SpiSend(0); + SpiClearRti(); + SpiEnableRti(); + /* here we could disable extint0 */ + /* and it would be re enabled when we enter WAIT_EOC */ + max1167_status = STA_MAX1167_READING_RES; + } + else { + /* report error */ + } + + SetBit(EXTINT, MAX1167_EOC_EINT); /* clear extint0 */ + VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ + + ISR_EXIT(); +} diff --git a/sw/airborne/arm7/max1167_hw.h b/sw/airborne/arm7/max1167_hw.h new file mode 100644 index 0000000000..b3c9604ce7 --- /dev/null +++ b/sw/airborne/arm7/max1167_hw.h @@ -0,0 +1,76 @@ +#ifndef MAX1167_HW_H +#define MAX1167_HW_H + +/* + MAX1167 SPI ADC connected on SPI1 + SS on P1.29 + EOC on P0.16 ( EINT0 ) +*/ + +#include "std.h" +#include "LPC21xx.h" +#include "interrupt_hw.h" +#include "spi_hw.h" + + +#define MAX1167_SS_PIN 29 +#define MAX1167_SS_IODIR IO1DIR +#define MAX1167_SS_IOSET IO1SET +#define MAX1167_SS_IOCLR IO1CLR + +#define MAX1167_EOC_PIN 16 +#define MAX1167_EOC_PINSEL PINSEL1 +#define MAX1167_EOC_PINSEL_BIT 0 +#define MAX1167_EOC_PINSEL_VAL 1 +#define MAX1167_EOC_EINT 0 + +#define Max1167Unselect() SetBit(MAX1167_SS_IOSET, MAX1167_SS_PIN) +#define Max1167Select() SetBit(MAX1167_SS_IOCLR, MAX1167_SS_PIN) + +#define Max1167OnSpiInt() { \ + if (bit_is_set(SSPMIS, RTMIS)) { \ + switch (max1167_status) { \ + \ + case STA_MAX1167_IDLE: \ + /* should not happen */ \ + break; \ + \ + case STA_MAX1167_SENDING_REQ: { \ + /* read dummy control byte reply */ \ + uint8_t foo __attribute__ ((unused)); \ + foo = SSPDR; \ + SpiClearRti(); \ + SpiDisableRti(); \ + SpiDisable(); \ + /* Max1167Unselect(); maybe... */ \ + /* here we might enable extint0 */ \ + max1167_status = STA_MAX1167_WAIT_EOC; \ + } \ + break; \ + \ + case STA_MAX1167_WAIT_EOC: \ + /* should not happen */ \ + break; \ + \ + case STA_MAX1167_READING_RES: \ + /* store convertion result */ \ + max1167_values[0] = SSPDR << 8; \ + max1167_values[0] += SSPDR; \ + max1167_values[1] = SSPDR << 8; \ + max1167_values[1] += SSPDR; \ + max1167_values[2] = SSPDR << 8; \ + max1167_values[2] += SSPDR; \ + SpiClearRti(); \ + SpiDisableRti(); \ + SpiDisable(); \ + Max1167Unselect(); \ + max1167_status = STA_MAX1167_DATA_AVAILABLE; \ + break; \ + case STA_MAX1167_DATA_AVAILABLE : \ + /* should not happen */ \ + break; \ + } \ + } \ +} + +#endif /* MAX1167_WH */ diff --git a/sw/airborne/booz_inter_mcu.c b/sw/airborne/booz_inter_mcu.c new file mode 100644 index 0000000000..f235f30187 --- /dev/null +++ b/sw/airborne/booz_inter_mcu.c @@ -0,0 +1,22 @@ +#include "booz_inter_mcu.h" + +#include "imu_v3.h" +#include "multitilt.h" + + +struct booz_inter_mcu_state inter_mcu_state; + +#ifdef BOOZ_FILTER_MCU +void inter_mcu_fill_state() { + inter_mcu_state.r_rates[AXIS_P] = imu_vs_gyro_unbiased[AXIS_P] * RATE_PI_S/M_PI; + inter_mcu_state.r_rates[AXIS_Q] = imu_vs_gyro_unbiased[AXIS_Q] * RATE_PI_S/M_PI; + inter_mcu_state.r_rates[AXIS_R] = imu_vs_gyro_unbiased[AXIS_R] * RATE_PI_S/M_PI; + inter_mcu_state.f_rates[AXIS_P] = mtt_p * RATE_PI_S/M_PI; + inter_mcu_state.f_rates[AXIS_Q] = mtt_q * RATE_PI_S/M_PI; + inter_mcu_state.f_rates[AXIS_R] = mtt_r * RATE_PI_S/M_PI; + inter_mcu_state.f_eulers[AXIS_X] = mtt_phi * ANGLE_PI/M_PI; + inter_mcu_state.f_eulers[AXIS_Y] = mtt_theta* ANGLE_PI/M_PI; + inter_mcu_state.f_eulers[AXIS_Z] = mtt_psi * ANGLE_PI/M_PI; + inter_mcu_state.status = mtt_status; +} +#endif diff --git a/sw/airborne/booz_inter_mcu.h b/sw/airborne/booz_inter_mcu.h new file mode 100644 index 0000000000..5be52c1cae --- /dev/null +++ b/sw/airborne/booz_inter_mcu.h @@ -0,0 +1,33 @@ +#ifndef BOOZ_INTER_MCU_H +#define BOOZ_INTER_MCU_H + +#include "std.h" +#include "6dof.h" + +/* angles are transmitted as int16 : PI -> 16384 */ +/* rates are transmitted as int16 : 1 PI s-1 -> 16384 */ +#define ANGLE_PI 0X3FFF +#define RATE_PI_S 0X3FFF + +#define IMU_UNINIT MT_STATUS_UNINIT +#define IMU_RUNNING MT_STATUS_RUNNING +#define IMU_CRASHED MT_STATUS_CRASHED +#define IMU_NO_LINK 3 + +struct booz_inter_mcu_state { + int16_t r_rates [AXIS_NB]; + int16_t f_rates [AXIS_NB]; + int16_t f_eulers[AXIS_NB]; + int16_t pad0; + uint8_t status; + uint8_t pad1; + uint16_t crc; +}; + +extern struct booz_inter_mcu_state inter_mcu_state; + +#ifdef BOOZ_FILTER_MCU +extern void inter_mcu_fill_state(void); +#endif + +#endif /* BOOZ_INTER_MCU_H */ diff --git a/sw/airborne/max1167.c b/sw/airborne/max1167.c new file mode 100644 index 0000000000..77b14689c0 --- /dev/null +++ b/sw/airborne/max1167.c @@ -0,0 +1,15 @@ +#include "max1167.h" + + +//volatile uint8_t max1167_data_available; +volatile uint8_t max1167_status; +uint16_t max1167_values[MAX1167_NB_CHAN]; + +extern void max1167_init( void ) { + uint8_t i; + for (i=0; ive[AXIS_P]; + max1167_values[1] = bsm.gyro->ve[AXIS_Q]; + max1167_values[2] = bsm.gyro->ve[AXIS_R]; + max1167_status = STA_MAX1167_DATA_AVAILABLE; + } + else { + /* report overrun error */ + } +} diff --git a/sw/airborne/sim/max1167_hw.h b/sw/airborne/sim/max1167_hw.h new file mode 100644 index 0000000000..8203b7f191 --- /dev/null +++ b/sw/airborne/sim/max1167_hw.h @@ -0,0 +1,12 @@ +#ifndef MAX1167_HW_H +#define MAX1167_HW_H + +/* + Simulated max1167 external ADC +*/ + +#include "std.h" + + + +#endif /* MAX1167_WH */ diff --git a/sw/airborne/sim/spi_hw.c b/sw/airborne/sim/spi_hw.c new file mode 100644 index 0000000000..cd0b3ece6e --- /dev/null +++ b/sw/airborne/sim/spi_hw.c @@ -0,0 +1,3 @@ +#include "spi.h" + +void spi_init( void ) {}