Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 13:24:03 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-30 02:48:31 +00:00
parent 34590e8236
commit ff102470d5
9 changed files with 278 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/arm7/max1167_hw.c
+76
View File
@@ -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();
}
sw/airborne/arm7/max1167_hw.h
+76
View File
@@ -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 */
sw/airborne/booz_inter_mcu.c
+22
View File
@@ -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
sw/airborne/booz_inter_mcu.h
+33
View File
@@ -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 */
sw/airborne/max1167.c
+15
View File
@@ -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; i<MAX1167_NB_CHAN; i++)
max1167_values[i] = 0;
// max1167_data_available = FALSE;
max1167_status = STA_MAX1167_IDLE;
}
sw/airborne/max1167.h
+24
View File
@@ -0,0 +1,24 @@
#ifndef MAX1167_H
#define MAX1167_H
#include "std.h"
#define MAX1167_NB_CHAN 4
extern void max1167_init( void );
extern void max1167_read( void );
//extern volatile uint8_t max1167_data_available;
#define STA_MAX1167_IDLE 0
#define STA_MAX1167_SENDING_REQ 1
#define STA_MAX1167_WAIT_EOC 2
#define STA_MAX1167_READING_RES 3
#define STA_MAX1167_DATA_AVAILABLE 4
extern volatile uint8_t max1167_status;
extern uint16_t max1167_values[MAX1167_NB_CHAN];
extern void max1167_hw_init( void );
#include "max1167_hw.h"
#endif /* MAX1167_H */
sw/airborne/sim/max1167_hw.c
+17
View File
@@ -0,0 +1,17 @@
#include "max1167.h"
#include "../../simulator/booz_sensors_model.h"
void max1167_hw_init( void ) {}
void max1167_read( void ) {
if (max1167_status == STA_MAX1167_IDLE) {
max1167_values[0] = bsm.gyro->ve[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 */
}
}
sw/airborne/sim/max1167_hw.h
+12
View File
@@ -0,0 +1,12 @@
#ifndef MAX1167_HW_H
#define MAX1167_HW_H
/*
Simulated max1167 external ADC
*/
#include "std.h"
#endif /* MAX1167_WH */
sw/airborne/sim/spi_hw.c
+3
View File
@@ -0,0 +1,3 @@
#include "spi.h"
void spi_init( void ) {}