Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 18:07:25 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge remote branch 'paparazzi/master' into dev

Browse Source
This commit is contained in:
Felix Ruess
2011-05-03 14:53:22 +02:00
parent 554a0cc418 817735686f
commit 1b137c5d87
20 changed files with 286 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/esden/calib/aspirin_012.xml
+2
View File
@@ -1,3 +1,4 @@
<airframe>
<section name="IMU" prefix="IMU_">
<define name="GYRO_P_NEUTRAL" value="93"/>
<define name="GYRO_Q_NEUTRAL" value="131"/>
@@ -30,3 +31,4 @@
<define name="MAG_YZ_SENS" value="0.0" integer="16"/>
</section>
</airframe>
conf/airframes/esden/lisa_m_pwm.xml
+13 -13
View File
@@ -38,8 +38,8 @@
<section name="SUPERVISION" prefix="SUPERVISION_">
<define name="MIN_MOTOR" value="1090"/>
<define name="MAX_MOTOR" value="2000"/>
<define name="MIN_MOTOR" value="1200"/>
<define name="MAX_MOTOR" value="1920"/>
<define name="TRIM_A" value="0"/>
<define name="TRIM_E" value="0"/>
<define name="TRIM_R" value="0"/>
@@ -122,12 +122,12 @@
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="RadOfDeg(600)"/>
<define name="REF_OMEGA_P" value="RadOfDeg(800)"/>
<define name="REF_ZETA_P" value="0.85"/>
<define name="REF_MAX_P" value="RadOfDeg(400.)"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="RadOfDeg(600)"/>
<define name="REF_OMEGA_Q" value="RadOfDeg(800)"/>
<define name="REF_ZETA_Q" value="0.85"/>
<define name="REF_MAX_Q" value="RadOfDeg(400.)"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
@@ -138,22 +138,22 @@
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="-550"/>
<define name="PHI_DGAIN" value="-210"/>
<define name="PHI_PGAIN" value="-1000"/>
<define name="PHI_DGAIN" value="-400"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="-550"/>
<define name="THETA_DGAIN" value="-210"/>
<define name="THETA_PGAIN" value="-1000"/>
<define name="THETA_DGAIN" value="-400"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="PSI_PGAIN" value="-600"/>
<define name="PSI_DGAIN" value="-200"/>
<define name="PSI_PGAIN" value="-1000"/>
<define name="PSI_DGAIN" value="-400"/>
<define name="PSI_IGAIN" value="-10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 200"/>
<define name="THETA_DDGAIN" value=" 200"/>
<define name="PSI_DDGAIN" value=" 200"/>
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
</section>
conf/airframes/esden/lisa_pwm_aspirin.xml
+2 -22
View File
@@ -110,26 +110,6 @@
<define name="REF_MAX_R" value="RadOfDeg(180.)"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- Old settings with exponential throttle motor controllers
<!-- feedback -->
<define name="PHI_PGAIN" value="-250"/>
<define name="PHI_DGAIN" value="-200"/>
<define name="PHI_IGAIN" value="-150"/>
<define name="THETA_PGAIN" value="-250"/>
<define name="THETA_DGAIN" value="-200"/>
<define name="THETA_IGAIN" value="-150"/>
<define name="PSI_PGAIN" value="-900"/>
<define name="PSI_DGAIN" value="-200"/>
<define name="PSI_IGAIN" value="-10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 200"/>
<define name="THETA_DDGAIN" value=" 200"/>
<define name="PSI_DDGAIN" value=" 200"/>
-->
<!-- feedback -->
<define name="PHI_PGAIN" value="-1900"/>
<define name="PHI_DGAIN" value="-380"/>
@@ -223,8 +203,8 @@
<subsystem name="gps" type="ublox">
<configure name="GPS_BAUD" value="B57600"/>
</subsystem>
<!--subsystem name="ahrs" type="cmpl"/-->
<subsystem name="ahrs" type="ic"/>
<subsystem name="ahrs" type="cmpl"/>
<!--<subsystem name="ahrs" type="ic"/>-->
</firmware>
<firmware name="lisa_l_test_progs">
conf/modules/humid_sht_i2c.xml
+1 -1
View File
@@ -10,7 +10,7 @@
<file name="humid_sht_i2c.h"/>
</header>
<init fun="humid_sht_init()"/>
<periodic fun="humid_sht_periodic()" freq="4"/>
<periodic fun="humid_sht_periodic()" freq="4" delay="0"/>
<periodic fun="humid_sht_p_temp()" freq="4" delay="6"/>
<periodic fun="humid_sht_p_humid()" freq="4" delay="9"/>
<event fun="humid_sht_event()"/>
sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c
+6 -1
View File
@@ -139,7 +139,7 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s
p->idx_buf++;
} else {
if (trans->type == I2CTransTxRx) {
//trans->type = I2CTransRx; /* FIXME should not change type */
trans->type = I2CTransRx; /* FIXME should not change type */
p->idx_buf = 0;
trans->slave_addr |= 1;
I2cSendStart(p);
@@ -270,6 +270,11 @@ void i2c0_hw_init ( void ) {
#endif
#endif
#ifndef I2C1_VIC_SLOT
#define I2C1_VIC_SLOT 11
#endif
void i2c1_ISR(void) __attribute__((naked));
void i2c1_ISR(void) {
sw/airborne/arch/lpc21/subsystems/settings_arch.c
+4
View File
@@ -40,6 +40,10 @@
#include "subsystems/settings.h"
#include "LPC21xx.h"
#include BOARD_CONFIG
#include "armVIC.h"
#define IAP_LOCATION 0x7FFFFFF1
#define IAP_PREPARE_SECTORS 50
sw/airborne/arch/lpc21/vic_slots_fw.txt
+1
View File
@@ -17,6 +17,7 @@ SSP_VIC_SLOT 9 imu_b2_v1.0, imu_b2_v1.1, imu_b2_v1.2
MICROMAG_DRDY_VIC_SLOT 9 micromag
hardcoded, no define 10 usb, e.g. telemetry_transparent_usb
hardcoded, no define 11 EXTINT in baro_scp module
I2C1_VIC_SLOT ? (11) (11 is default in mcu_periph/i2c_arch.c)
I2C1_VIC_SLOT 12 ami601 in imu_b2_v1.0, imu_crista
MS2100_DRDY_VIC_SLOT 12 ms2100 mag in imu_b2_v1.1
MAX11040_DRDY_VIC_SLOT ? max11040 adc module
sw/airborne/arch/stm32/peripherals/hmc5843_arch.c
+4
View File
@@ -40,6 +40,7 @@ void hmc5843_arch_init( void ) {
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
#ifdef HMC5843_USE_INT
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource5);
EXTI_InitTypeDef EXTI_InitStructure;
EXTI_InitStructure.EXTI_Line = EXTI_Line5;
@@ -54,6 +55,7 @@ void hmc5843_arch_init( void ) {
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
}
void hmc5843_arch_reset(void)
@@ -67,5 +69,7 @@ void exti9_5_irq_handler(void) {
if(EXTI_GetITStatus(EXTI_Line5) != RESET)
EXTI_ClearITPendingBit(EXTI_Line5);
#ifdef HMC5843_USE_INT
hmc5843.ready_for_read = TRUE;
#endif
}
sw/airborne/arch/stm32/peripherals/hmc5843_arch.h
+8
View File
@@ -24,4 +24,12 @@
#ifndef HMC5843_ARCH_H
#define HMC5843_ARCH_H
#include <stm32/gpio.h>
/* returns true if conversion done */
static inline int mag_eoc(void)
{
return GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5);
}
#endif /* HMC5843_ARCH_H */
sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
+35 -9
View File
@@ -24,11 +24,13 @@ void dma1_c4_irq_handler(void);
void imu_aspirin_arch_int_enable(void) {
NVIC_InitTypeDef NVIC_InitStructure;
#ifdef ASPIRIN_USE_GYRO_INT
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
@@ -36,16 +38,27 @@ void imu_aspirin_arch_int_enable(void) {
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
NVIC_InitTypeDef NVIC_init_struct = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = 0,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE
};
NVIC_Init(&NVIC_init_struct);
}
void imu_aspirin_arch_int_disable(void) {
NVIC_InitTypeDef NVIC_InitStructure;
#ifdef ASPIRIN_USE_GYRO_INT
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x0F;
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x0F;
@@ -53,6 +66,15 @@ void imu_aspirin_arch_int_disable(void) {
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
NVIC_Init(&NVIC_InitStructure);
/* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
NVIC_InitTypeDef NVIC_init_struct = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = 0,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = DISABLE
};
NVIC_Init(&NVIC_init_struct);
}
void imu_aspirin_arch_init(void) {
@@ -83,12 +105,14 @@ void imu_aspirin_arch_init(void) {
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
#ifdef ASPIRIN_USE_GYRO_INT
GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14);
EXTI_InitStructure.EXTI_Line = EXTI_Line14;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
#endif
/* Accel */
/* set accel slave select as output and assert it ( on PB12) */
@@ -137,14 +161,6 @@ void imu_aspirin_arch_init(void) {
SPI_InitStructure.SPI_CRCPolynomial = 7;
SPI_Init(SPI2, &SPI_InitStructure);
/* Enable DMA1 channel4 IRQ Channel ( SPI RX) */
NVIC_InitTypeDef NVIC_init_struct = {
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
.NVIC_IRQChannelPreemptionPriority = 0,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE
};
NVIC_Init(&NVIC_init_struct);
/* Enable SPI_2 DMA clock ---------------------------------------------------*/
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
@@ -233,8 +249,10 @@ void exti15_10_irq_handler(void) {
if(EXTI_GetITStatus(EXTI_Line14) != RESET)
EXTI_ClearITPendingBit(EXTI_Line14);
#ifdef ASPIRIN_USE_GYRO_INT
imu_aspirin.gyro_eoc = TRUE;
imu_aspirin.status = AspirinStatusReadingGyro;
#endif
}
@@ -260,6 +278,15 @@ void exti2_irq_handler(void) {
*/
void dma1_c4_irq_handler(void) {
Adxl345Unselect();
if (DMA_GetITStatus(DMA1_IT_TC4)) {
// clear int pending bit
DMA_ClearITPendingBit(DMA1_IT_GL4);
// mark as available
imu_aspirin.accel_available = TRUE;
}
// disable DMA Channel
DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE);
/* Disable SPI_2 Rx and TX request */
SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE);
@@ -268,5 +295,4 @@ void dma1_c4_irq_handler(void) {
DMA_Cmd(DMA1_Channel4, DISABLE);
DMA_Cmd(DMA1_Channel5, DISABLE);
imu_aspirin.accel_available = TRUE;
}
sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.h
+7 -2
View File
@@ -2,8 +2,7 @@
#define IMU_ASPIRIN_ARCH_H
#include "subsystems/imu.h"
#include "led.h"
#include <stm32/gpio.h>
extern void imu_aspirin_arch_init(void);
extern void imu_aspirin_arch_int_enable(void);
@@ -12,5 +11,11 @@ extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
extern void adxl345_clear_rx_buf(void);
extern void adxl345_start_reading_data(void);
static inline int imu_aspirin_eoc(void)
{
return !GPIO_ReadInputDataBit(GPIOC, GPIO_Pin_14);
}
#endif /* IMU_ASPIRIN_ARCH_H */
sw/airborne/firmwares/fixedwing/datalink.c
+1
View File
@@ -41,6 +41,7 @@
#if defined NAV || defined WIND_INFO
#include "estimator.h"
#include "subsystems/nav.h"
#endif
#ifdef USE_JOYSTICK
sw/airborne/modules/meteo/humid_sht_i2c.c
+4
View File
@@ -35,6 +35,10 @@
#include "messages.h"
#include "downlink.h"
#ifndef DOWNLINK_DEVICE
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
#endif
#ifndef SHT_I2C_DEV
#define SHT_I2C_DEV i2c0
#endif
sw/airborne/peripherals/hmc5843.c
+60 -35
View File
@@ -9,10 +9,10 @@ void exti9_5_irq_handler(void);
void hmc5843_init(void)
{
hmc5843.i2c_trans.status = I2CTransSuccess;
hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
hmc5843.i2c_trans.status = I2CTransSuccess;
hmc5843.i2c_trans.slave_addr = HMC5843_ADDR;
hmc5843_arch_init();
hmc5843_arch_init();
}
// blocking, only intended to be called for initialization
@@ -23,61 +23,86 @@ static void send_config(void)
hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
hmc5843.i2c_trans.len_w = 2;
i2c_submit(&i2c2,&hmc5843.i2c_trans);
while(hmc5843.i2c_trans.status == I2CTransPending);
while(hmc5843.i2c_trans.status == I2CTransPending);
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
hmc5843.i2c_trans.buf[1] = 0x01<<5;
hmc5843.i2c_trans.len_w = 2;
i2c_submit(&i2c2,&hmc5843.i2c_trans);
while(hmc5843.i2c_trans.status == I2CTransPending);
while(hmc5843.i2c_trans.status == I2CTransPending);
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE; // set to continuous mode
hmc5843.i2c_trans.buf[1] = 0x00;
hmc5843.i2c_trans.len_w = 2;
i2c_submit(&i2c2,&hmc5843.i2c_trans);
while(hmc5843.i2c_trans.status == I2CTransPending);
while(hmc5843.i2c_trans.status == I2CTransPending);
}
void hmc5843_idle_task(void)
{
if (hmc5843.initialized && hmc5843.ready_for_read && (hmc5843.i2c_trans.status == I2CTransSuccess || hmc5843.i2c_trans.status == I2CTransFailed)) {
if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
hmc5843.ready_for_read = FALSE;
hmc5843.i2c_trans.type = I2CTransRx;
hmc5843.i2c_trans.len_r = 7;
i2c_submit(&i2c2, &hmc5843.i2c_trans);
hmc5843.reading = TRUE;
}
}
if (hmc5843.i2c_trans.status == I2CTransFailed) {
hmc5843.sent_tx = 0;
hmc5843.sent_rx = 0;
}
if (hmc5843.reading && hmc5843.i2c_trans.status == I2CTransSuccess) {
hmc5843.timeout = 0;
hmc5843.data_available = TRUE;
hmc5843.reading = FALSE;
memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6);
for (int i = 0; i < 3; i++) {
hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
}
if (hmc5843.i2c_trans.status == I2CTransRunning || hmc5843.i2c_trans.status == I2CTransPending) return;
if (hmc5843.initialized && mag_eoc() && !hmc5843.sent_tx && !hmc5843.sent_rx) {
if (i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.len_w = 1;
hmc5843.i2c_trans.buf[0] = 0x3;
i2c_submit(&i2c2, &hmc5843.i2c_trans);
hmc5843.sent_tx = 1;
return;
}
}
if (hmc5843.sent_tx) {
hmc5843.i2c_trans.type = I2CTransRx;
hmc5843.i2c_trans.len_r = 6;
hmc5843.i2c_trans.len_w = 1;
hmc5843.i2c_trans.buf[0] = 0x3;
i2c_submit(&i2c2, &hmc5843.i2c_trans);
hmc5843.sent_rx = 1;
hmc5843.sent_tx = 0;
return;
}
if (hmc5843.sent_rx && hmc5843.i2c_trans.status == I2CTransSuccess) {
hmc5843.sent_rx = 0;
hmc5843.sent_tx = 0;
hmc5843.timeout = 0;
hmc5843.data_available = TRUE;
memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6);
for (int i = 0; i < 3; i++) {
hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
}
}
}
void hmc5843_periodic(void)
{
if (!hmc5843.initialized) {
send_config();
hmc5843.initialized = TRUE;
} else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){
if (!hmc5843.initialized) {
send_config();
hmc5843.initialized = TRUE;
} else if (hmc5843.timeout++ > HMC5843_TIMEOUT && i2c2.status == I2CIdle && i2c_idle(&i2c2)){
#ifdef USE_HMC59843_ARCH_RESET
hmc5843_arch_reset();
hmc5843_arch_reset();
#endif
hmc5843.i2c_trans.type = I2CTransRx;
hmc5843.i2c_trans.len_r = 7;
i2c_submit(&i2c2, &hmc5843.i2c_trans);
hmc5843.reading = TRUE;
hmc5843.ready_for_read = FALSE;
hmc5843.timeout = 0;
}
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.len_w = 1;
hmc5843.i2c_trans.buf[0] = 0x3;
i2c_submit(&i2c2, &hmc5843.i2c_trans);
while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
hmc5843.i2c_trans.type = I2CTransRx;
hmc5843.i2c_trans.len_r = 6;
i2c_submit(&i2c2, &hmc5843.i2c_trans);
while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
hmc5843.timeout = 0;
}
}
sw/airborne/peripherals/hmc5843.h
+5 -5
View File
@@ -31,15 +31,15 @@
struct Hmc5843 {
struct i2c_transaction i2c_trans;
uint32_t timeout;
uint8_t sent_tx;
uint8_t sent_rx;
uint8_t initialized;
uint8_t data_available;
union {
uint8_t buf[7];
int16_t value[3];
} data;
uint8_t initialized;
uint8_t reading;
volatile uint8_t ready_for_read;
uint8_t data_available;
uint32_t timeout;
};
extern struct Hmc5843 hmc5843;
sw/airborne/print.h
+51
View File
@@ -59,6 +59,8 @@
_PrintHex16(out_fun, low32); \
}
#ifdef USE_UART0
#define Uart0PrintHex(c) _PrintHex(uart0_transmit, c)
#define Uart0PrintHex16(c) _PrintHex16(uart0_transmit, c)
#define Uart0PrintHex32(c) _PrintHex32(uart0_transmit, c)
@@ -69,8 +71,13 @@
#define UART0PrintHex32 Uart0PrintHex32
#define UART0PrintString Uart0PrintString
#endif /* USE_UART0 */
#ifdef USE_UART1
#define Uart1PrintHex(c) _PrintHex(uart1_transmit, c)
#define Uart1PrintHex16(c) _PrintHex16(uart1_transmit, c)
#define Uart1PrintHex32(c) _PrintHex32(uart1_transmit, c)
#define Uart1PrintString(s) _PrintString(uart1_transmit, s)
#define UART1PrintHex Uart1PrintHex
@@ -78,6 +85,50 @@
#define UART1PrintHex32 Uart1PrintHex32
#define UART1PrintString Uart1PrintString
#endif /* USE_UART1 */
#ifdef USE_UART2
#define Uart2PrintHex(c) _PrintHex(uart2_transmit, c)
#define Uart2PrintHex16(c) _PrintHex16(uart2_transmit, c)
#define Uart2PrintHex32(c) _PrintHex32(uart2_transmit, c)
#define Uart2PrintString(s) _PrintString(uart2_transmit, s)
#define UART2PrintHex Uart2PrintHex
#define UART2PrintHex16 Uart2PrintHex16
#define UART2PrintHex32 Uart2PrintHex32
#define UART2PrintString Uart2PrintString
#endif /* USE_UART2 */
#ifdef USE_UART3
#define Uart3PrintHex(c) _PrintHex(uart3_transmit, c)
#define Uart3PrintHex16(c) _PrintHex16(uart3_transmit, c)
#define Uart3PrintHex32(c) _PrintHex32(uart3_transmit, c)
#define Uart3PrintString(s) _PrintString(uart3_transmit, s)
#define UART3PrintHex Uart3PrintHex
#define UART3PrintHex16 Uart3PrintHex16
#define UART3PrintHex32 Uart3PrintHex32
#define UART3PrintString Uart3PrintString
#endif /* USE_UART3 */
#ifdef USE_UART5
#define Uart5PrintHex(c) _PrintHex(uart5_transmit, c)
#define Uart5PrintHex16(c) _PrintHex16(uart5_transmit, c)
#define Uart5PrintHex32(c) _PrintHex32(uart5_transmit, c)
#define Uart5PrintString(s) _PrintString(uart5_transmit, s)
#define UART5PrintHex Uart5PrintHex
#define UART5PrintHex16 Uart5PrintHex16
#define UART5PrintHex32 Uart5PrintHex32
#define UART5PrintString Uart5PrintString
#endif /* USE_UART5 */
#define UsbSPrintHex(c) _PrintHex(VCOM_putchar, c)
#define UsbSPrintHex16(c) _PrintHex16(VCOM_putchar, c)
#define UsbSPrintString(s) _PrintString(VCOM_putchar, s)
sw/airborne/subsystems/imu/imu_aspirin.c
+9 -6
View File
@@ -26,9 +26,7 @@ static void send_i2c_msg_with_retry(struct i2c_transaction* t) {
void imu_impl_init(void) {
imu_aspirin.status = AspirinStatusUninit;
imu_aspirin.gyro_available = FALSE;
imu_aspirin.gyro_available_blaaa = FALSE;
imu_aspirin.mag_ready_for_read = FALSE;
imu_aspirin.mag_available = FALSE;
imu_aspirin.accel_available = FALSE;
@@ -37,6 +35,7 @@ void imu_impl_init(void) {
imu_aspirin.i2c_trans_gyro.slave_addr = ITG3200_ADDR;
imu_aspirin.i2c_trans_gyro.len_w = 1;
imu_aspirin.i2c_trans_gyro.len_r = 6;
imu_aspirin.i2c_trans_gyro.status = I2CTransFailed;
imu_aspirin_arch_init();
hmc5843_init();
@@ -51,10 +50,14 @@ void imu_periodic(void) {
configure_accel();
imu_aspirin_arch_int_enable();
imu_aspirin.status = AspirinStatusIdle;
}
else {
} else {
imu_aspirin.gyro_available_blaaa = TRUE;
imu_aspirin.time_since_last_reading++;
imu_aspirin.time_since_last_accel_reading++;
if (imu_aspirin.time_since_last_accel_reading > ASPIRIN_ACCEL_TIMEOUT) {
configure_accel();
imu_aspirin.time_since_last_accel_reading=0;
}
}
}
@@ -78,9 +81,9 @@ static void configure_gyro(void) {
t.buf[0] = ITG3200_REG_PWR_MGM;
t.buf[1] = 0x01;
send_i2c_msg_with_retry(&t);
/* enable interrupt on data ready, idle hight */
/* enable interrupt on data ready, idle high, latch until read any register */
t.buf[0] = ITG3200_REG_INT_CFG;
t.buf[1] = (0x01 | 0x01<<7);
t.buf[1] = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
send_i2c_msg_with_retry(&t);
}
sw/airborne/subsystems/imu/imu_aspirin.h
+56 -30
View File
@@ -63,19 +63,19 @@ struct ImuAspirin {
volatile enum AspirinStatus status;
struct i2c_transaction i2c_trans_gyro;
struct i2c_transaction i2c_trans_mag;
uint8_t gyro_eoc;
uint8_t gyro_available;
uint8_t gyro_available_blaaa;
uint8_t mag_available;
volatile uint8_t mag_ready_for_read;
volatile uint8_t accel_available;
volatile uint8_t accel_tx_buf[7];
volatile uint8_t accel_rx_buf[7];
uint32_t time_since_last_reading;
uint32_t time_since_last_accel_reading;
uint8_t mag_available;
uint8_t reading_gyro;
uint8_t gyro_available_blaaa;
};
extern struct ImuAspirin imu_aspirin;
#define ASPIRIN_GYRO_TIMEOUT 3
#define ASPIRIN_ACCEL_TIMEOUT 3
#include "peripherals/hmc5843.h"
#define foo_handler() {}
@@ -99,6 +99,7 @@ static inline void gyro_read_i2c(void)
{
imu_aspirin.i2c_trans_gyro.buf[0] = ITG3200_REG_GYRO_XOUT_H;
i2c_submit(&i2c2,&imu_aspirin.i2c_trans_gyro);
imu_aspirin.reading_gyro = 1;
}
static inline void gyro_copy_i2c(void)
@@ -117,45 +118,70 @@ static inline void accel_copy_spi(void)
VECT3_ASSIGN(imu.accel_unscaled, ax, ay, az);
}
static inline void imu_gyro_event(void (* _gyro_handler)(void))
{
}
static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
if (imu_aspirin.status == AspirinStatusUninit) return;
imu_aspirin_arch_int_disable();
ImuMagEvent(_mag_handler);
if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {
imu_aspirin.gyro_eoc = FALSE;
imu_aspirin.status = AspirinStatusIdle;
i2c2_er_irq_handler();
gyro_read_i2c();
imu_aspirin.time_since_last_reading = 0;
}
if (imu_aspirin.status == AspirinStatusReadingGyro && imu_aspirin.gyro_eoc &&
imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess && i2c_idle(&i2c2)) {
gyro_copy_i2c();
imu_aspirin.status = AspirinStatusIdle;
if (imu_aspirin.gyro_available_blaaa) {
imu_aspirin.gyro_available_blaaa = FALSE;
_gyro_handler();
}
}
if (imu_aspirin.gyro_eoc && i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {
imu_aspirin.time_since_last_reading = 0;
}
imu_aspirin.gyro_eoc = FALSE;
gyro_read_i2c();
}
if (imu_aspirin.accel_available) {
imu_aspirin.time_since_last_accel_reading = 0;
imu_aspirin.accel_available = FALSE;
accel_copy_spi();
_accel_handler();
}
imu_aspirin_arch_int_enable();
// Reset everything if we've been waiting too long
if (imu_aspirin.time_since_last_reading > ASPIRIN_GYRO_TIMEOUT) {
i2c2_er_irq_handler();
gyro_read_i2c();
imu_aspirin.time_since_last_reading = 0;
return;
}
// Try again later if transaction is in progress
if (imu_aspirin.i2c_trans_gyro.status == I2CTransPending || imu_aspirin.i2c_trans_gyro.status == I2CTransRunning)
{
return;
}
ImuMagEvent(_mag_handler);
// Try back later if things are not idle
if ((i2c2.status != I2CIdle) || !i2c_idle(&i2c2)) {
return;
}
if (imu_aspirin.reading_gyro) {
if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {
gyro_copy_i2c();
if (imu_aspirin.gyro_available_blaaa) {
imu_aspirin.gyro_available_blaaa = FALSE;
_gyro_handler();
}
}
imu_aspirin.reading_gyro = 0;
return;
}
// If we're not already waiting for read, and conversion is complete, schedule a read
if (!imu_aspirin.reading_gyro && imu_aspirin_eoc() && i2c2.status == I2CIdle && i2c_idle(&i2c2)) {
if (imu_aspirin.i2c_trans_gyro.status == I2CTransSuccess) {
imu_aspirin.time_since_last_reading = 0;
}
gyro_read_i2c();
return;
}
}
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do { \
imu_aspirin_event(_gyro_handler, _accel_handler, _mag_handler); \
} while(0);
} while(0);
#endif /* IMU_ASPIRIN_H */
sw/ground_segment/misc/Makefile
+11 -2
View File
@@ -1,7 +1,16 @@
UNAME = $(shell uname -s)
ifeq ("$(UNAME)","Darwin")
LIBRARYS = -L/opt/local/lib
else
LIBRARYS = -s
endif
all: davis2ivy
davis2ivy: davis2ivy.o
g++ -o davis2ivy davis2ivy.o -s -livy
g++ -o davis2ivy davis2ivy.o $(LIBRARYS) -livy
%.o : %.c
gcc -c -O2 -Wall $<
gcc -c -O2 -Wall -I /opt/local/include/ $<
sw/ground_segment/misc/davis2ivy.c
+6 -1
View File
@@ -55,6 +55,7 @@
#include <Ivy/ivy.h>
#include <Ivy/ivyloop.h>
#include <Ivy/timer.h>
#include <Ivy/version.h>
typedef enum { FALSE = 0, TRUE } BOOL;
@@ -263,7 +264,11 @@ int main(int argc, char **argv) {
// create timer
tid = TimerRepeatAfter (0, delay, handle_timer, 0);
IvyMainLoop();
#if IVYMINOR_VERSION == 8
IvyMainLoop (NULL,NULL);
#else
IvyMainLoop ();
#endif
return 0;
}