Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +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-28 03:26:44 +00:00
parent 5332612725
commit 48692f3c26
27 changed files with 547 additions and 837 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz.xml
+1 -85
View File
@@ -35,91 +35,7 @@
<makefile>
ARCHI=arm7
FLASH_MODE = IAP
#
# controller CPU
#
ctl.ARCHDIR = $(ARCHI)
ctl.ARCH = arm7tdmi
ctl.TARGET = ctl
ctl.TARGETDIR = ctl
ctl.CFLAGS += -DCONTROLLER -DCONFIG=\"conf_booz.h\"
ctl.srcs = booz_controller_main.c
ctl.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)' -DTIME_LED=1
ctl.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
ctl.CFLAGS += -DLED
ctl.srcs += $(SRC_ARCH)/armVIC.c
ctl.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
ctl.srcs += $(SRC_ARCH)/uart_hw.c
ctl.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
ctl.srcs += downlink.c pprz_transport.c
ctl.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart1
ctl.srcs += booz_datalink.c
ctl.CFLAGS += -DCONTROLLER -DUSE_SPI -DSPI_MASTER -DUSE_SPI_SLAVE0 -DSPI_SELECT_SLAVE0_PIN=20 -DSPI_SELECT_SLAVE0_PORT=0 -DSSPCPSR_VAL=0x0C
ctl.srcs += spi.c $(SRC_ARCH)/spi_hw.c link_imu.c
ctl.srcs += booz_estimator.c booz_control.c booz_autopilot.c
#ctl.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING
#ctl.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c
ctl.CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\" -DUSE_BUSS_TWI_BLMC
ctl.srcs += $(SRC_ARCH)/actuators_buss_twi_blmc_hw.c actuators.c
ctl.CFLAGS += -DI2C_SCLL=150 -DI2C_SCLH=150 -DI2C_VIC_SLOT=10
ctl.srcs += i2c.c $(SRC_ARCH)/i2c_hw.c
ctl.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA -DRC_LED=4
ctl.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
ctl.srcs += commands.c
ctl.srcs += booz_telemetry.c
#
# FILTER CPU
#
flt.ARCHDIR = $(ARCHI)
flt.ARCH = arm7tdmi
flt.TARGET = flt
flt.TARGETDIR = flt
flt.CFLAGS += -DIMU -DCONFIG=\"pprz_imu.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)'
flt.srcs = booz_filter_main.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
flt.CFLAGS += -DLED
flt.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
flt.srcs += $(SRC_ARCH)/uart_hw.c
flt.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
flt.srcs += downlink.c pprz_transport.c booz_filter_telemetry.c
flt.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
flt.srcs += $(SRC_ARCH)/adc_hw.c
flt.srcs += $(SRC_ARCH)/max1167.c
flt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
flt.srcs += multitilt.c
flt.CFLAGS += -DIMU
flt.srcs += link_imu.c
include $(PAPARAZZI_SRC)/conf/autopilot/conf_booz.makefile
</makefile>
conf/telemetry/booz.xml
+1
View File
@@ -26,6 +26,7 @@
<mode name="default">
<message name="IMU_GYRO" period=".017"/>
<message name="IMU_GYRO_LP" period=".017"/>
<message name="IMU_GYRO_RAW" period=".25"/>
</mode>
<mode name="raw_sensors">
<message name="IMU_GYRO_RAW" period=".017"/>
sw/airborne/arm7/booz_link_mcu_hw.c
+134
View File
@@ -0,0 +1,134 @@
#include "booz_link_mcu.h"
#ifdef BOOZ_FILTER_MCU
#include "armVIC.h"
volatile uint8_t spi0_data_available;
volatile uint8_t spi0_idx_buf;
uint8_t* spi0_buffer_output = (uint8_t*)&inter_mcu_state;
uint8_t* spi0_buffer_input = (uint8_t*)&booz_link_mcu_state_unused;
void SPI0_ISR(void) __attribute__((naked));
#define Spi0InitBuf() { \
spi0_idx_buf = 0; \
S0SPDR = spi0_buffer_output[0]; \
}
#define Spi0OneByte() { \
/* FIXME : do something usefull with the status register reading */ \
uint8_t foo __attribute__((unused)) = S0SPSR; \
spi0_buffer_input[spi0_idx_buf] = S0SPDR; \
spi0_idx_buf++; \
if (spi0_idx_buf < sizeof(inter_mcu_state)) { \
S0SPDR = spi0_buffer_output[spi0_idx_buf]; \
} \
else { \
spi0_data_available = TRUE; \
BoozLinkMcuSetUnavailable(); \
} \
}
/* */
#define S0SPCR_SPIE 7 /* SPI enable */
void booz_link_mcu_hw_init ( void ) {
/* init SPI0 */
/* setup pins for sck, miso, mosi, SSEL */
PINSEL0 |= 1<<8 | 1<<10| 1<<12 | 1<< 14;
/* setup P1_16 to P1_25 as GPIO */
PINSEL2 &= ~(1<<3);
/* P1_24 (DRDY) is output */
SetBit(IO1DIR, SPI0_DRDY);
/* DRDY idles high */
BoozLinkMcuSetUnavailable();
/* configure SPI : 8 bits CPOL=0 CPHA=0 MSB_first slave */
S0SPCR = _BV(3);
/* setup SPI clock rate */
S0SPCCR = 0x20;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(VIC_SPI0); // SPI0 selected as IRQ
VICIntEnable = VIC_BIT(VIC_SPI0); // SPI0 interrupt enabled
VICVectCntl1 = VIC_ENABLE | VIC_SPI0;
VICVectAddr1 = (uint32_t)SPI0_ISR; // address of the ISR
/* clear pending interrupt */
SetBit(S0SPINT, SPI0IF);
/* enable SPI interrupt */
SetBit(S0SPCR, S0SPCR_SPIE);
}
void SPI0_ISR(void) {
ISR_ENTRY();
Spi0OneByte();
/* clear the interrupt */
S0SPINT = _BV(SPI0IF);
/* clear this interrupt from the VIC */
VICVectAddr = 0x00000000;
ISR_EXIT();
}
#endif /* BOOZ_FILTER_MCU */
#ifdef BOOZ_CONTROLLER_MCU
/* IMU connected to SSP ( aka SPI1 ) */
#include "LPC21xx.h"
#include "interrupt_hw.h"
/* DRDY connected pin to P0.16 EINT0 */
#define LINK_IMU_DRDY_PINSEL PINSEL1
#define LINK_IMU_DRDY_PINSEL_VAL 0x01
#define LINK_IMU_DRDY_PINSEL_BIT 0
#define LINK_IMU_DRDY_EINT 0
static void EXTINT0_ISR(void) __attribute__((naked));
void booz_link_mcu_hw_init ( void ) {
/* configure DRDY pin */
LINK_IMU_DRDY_PINSEL |= LINK_IMU_DRDY_PINSEL_VAL << LINK_IMU_DRDY_PINSEL_BIT;
SetBit(EXTMODE, LINK_IMU_DRDY_EINT); /* EINT is edge trigered */
ClearBit(EXTPOLAR,LINK_IMU_DRDY_EINT); /* EINT is trigered on falling edge */
SetBit(EXTINT,LINK_IMU_DRDY_EINT); /* clear pending EINT */
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_EINT0 ); /* select EINT0 as IRQ source */
VICIntEnable = VIC_BIT( VIC_EINT0 ); /* enable it */
VICVectCntl9 = VIC_ENABLE | VIC_EINT0;
VICVectAddr9 = (uint32_t)EXTINT0_ISR; // address of the ISR
spi_buffer_input = (uint8_t*)&inter_mcu_state;
spi_buffer_output = (uint8_t*)&booz_link_mcu_state_unused;
spi_buffer_length = sizeof(inter_mcu_state);
}
void EXTINT0_ISR(void) {
ISR_ENTRY();
SpiSelectSlave0();
SpiStart();
/* clear EINT2 */
SetBit(EXTINT,LINK_IMU_DRDY_EINT); /* clear EINT0 */
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
ISR_EXIT();
}
#endif /* BOOZ_CONTROLLER_MCU */
sw/airborne/arm7/booz_link_mcu_hw.h
+33
View File
@@ -0,0 +1,33 @@
#ifndef BOOZ_LINK_MCU_HW_H
#define BOOZ_LINK_MCU_HW_H
#include "LPC21xx.h"
/*
wiring on IMU_V3
P0_4 SCK0
P0_5 MISO0
P0_6 MOSI0
P0_7 SSEL0
P1_24 DRDY
*/
#define SPI0_DRDY 24
extern volatile uint8_t spi0_data_available;
extern volatile uint8_t spi0_idx_buf;
extern uint8_t* spi0_buffer_output;
//extern uint8_t* spi0_buffer_input;
#define Spi0InitBuf() { \
spi0_idx_buf = 0; \
S0SPDR = spi0_buffer_output[0]; \
}
#define BoozLinkMcuSetUnavailable() { IO1SET = _BV(SPI0_DRDY); }
#define BoozLinkMcuSetAvailable() { IO1CLR = _BV(SPI0_DRDY); }
#endif /* BOOZ_LINK_MCU_HW_H */
sw/airborne/arm7/imu_v3_hw.c
-4
View File
@@ -4,9 +4,6 @@
static void SPI1_ISR(void) __attribute__((naked));
uint8_t spi_cur_slave;
/* SSPCR0 settings */
#define SSP_DDS 0x07 << 0 /* data size : 8 bits */
#define SSP_FRF 0x00 << 4 /* frame format : SPI */
@@ -21,7 +18,6 @@ uint8_t spi_cur_slave;
#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */
void imu_v3_hw_init(void) {
spi_cur_slave = SPI_SLAVE_NONE;
max1167_init();
sw/airborne/arm7/imu_v3_hw.h
-27
View File
@@ -1,32 +1,5 @@
#ifndef IMU_V3_HW_H
#define IMU_V3_HW_H
#include "max1167.h"
extern void imu_v3_hw_init(void);
#define SPI_SLAVE_NONE 0
#define SPI_SLAVE_MAX 1
extern uint8_t spi_cur_slave;
#define ImuEventCheckAndHandle(user_handler) { \
if (max1167_data_available) { \
max1167_data_available = FALSE; \
spi_cur_slave = SPI_SLAVE_NONE; \
ImuUpdateGyros(); \
ImuUpdateAccels(); \
user_handler(); \
} \
}
#define ImuPeriodic() { \
if (spi_cur_slave != SPI_SLAVE_NONE) { \
DOWNLINK_SEND_AHRS_OVERRUN(); \
} \
else { \
spi_cur_slave = SPI_SLAVE_MAX; \
max1167_read(); \
} \
}
#endif /* IMU_V3_HW_H */
sw/airborne/arm7/link_mcu_hw.h
+9 -9
View File
@@ -28,16 +28,16 @@
#ifndef LINK_MCU_HW_H
#define LINK_MCU_HW_H
#ifdef BOOZ_FILTER_MCU
#include "LPC21xx.h"
#define BoozLinkMcuSetUnavailable() { IO1SET = _BV(SPI0_DRDY); }
#define BoozLinkMcuSetAvailable() { IO1CLR = _BV(SPI0_DRDY); }
#endif /* BOOZ_FILTER_MCU */
#define CRC_INIT 0x0
#define CrcLow(x) ((x)&0xff)
#define CrcHigh(x) ((x)>>8)
static inline uint16_t CrcUpdate(uint16_t crc, uint8_t data) {
uint8_t a = ((uint8_t)CrcHigh(crc)) + data;
uint8_t b = ((uint8_t)CrcLow(crc)) + a;
crc = b | a << 8;
return crc;
}
#endif /* LINK_MCU_HW_H */
sw/airborne/booz_controller_main.c
+8 -17
View File
@@ -12,7 +12,8 @@
#include "radio_control.h"
#include "spi.h"
#include "link_imu.h"
#include "booz_link_mcu.h"
#include "booz_estimator.h"
#include "booz_control.h"
#include "booz_autopilot.h"
@@ -30,12 +31,6 @@ int16_t trim_q = 0;
int16_t trim_r = 0;
uint8_t vbat = 0;
//FIXME
#ifdef SITL
uint32_t link_imu_nb_err;
uint8_t link_imu_status;
#endif
#ifndef SITL
int main( void ) {
booz_controller_main_init();
@@ -61,10 +56,8 @@ STATIC_INLINE void booz_controller_main_init( void ) {
ppm_init();
radio_control_init();
#ifndef SITL
spi_init();
link_imu_init();
#endif
booz_link_mcu_init();
booz_estimator_init();
booz_control_init();
@@ -82,11 +75,9 @@ STATIC_INLINE void booz_controller_main_init( void ) {
STATIC_INLINE void booz_controller_main_periodic_task( void ) {
// FIXME
#ifndef SITL
link_imu_periodic_task();
#endif
/* check for timeout */
booz_link_mcu_periodic_task();
/* run control loops */
booz_autopilot_periodic_task();
SetActuatorsFromCommands(commands);
@@ -119,10 +110,10 @@ STATIC_INLINE void booz_controller_main_event_task( void ) {
// FIXME
#ifndef SITL
DlEventCheckAndHandle();
LinkImuEventCheckAndHandle();
#endif
BoozLinkMcuEventCheckAndHandle();
RadioControlEventCheckAndHandle(booz_autopilot_on_rc_event);
}
sw/airborne/booz_estimator.c
+17
View File
@@ -1,5 +1,8 @@
#include "booz_estimator.h"
#include "booz_inter_mcu.h"
float booz_estimator_uf_p;
float booz_estimator_uf_q;
float booz_estimator_uf_r;
@@ -27,3 +30,17 @@ void booz_estimator_init( void ) {
booz_estimator_psi = 0.;
}
void booz_estimator_read_inter_mcu_state( void ) {
booz_estimator_uf_p = inter_mcu_state.r_rates[AXIS_P] * M_PI/RATE_PI_S;
booz_estimator_uf_q = inter_mcu_state.r_rates[AXIS_Q] * M_PI/RATE_PI_S;
booz_estimator_uf_r = inter_mcu_state.r_rates[AXIS_R] * M_PI/RATE_PI_S;
booz_estimator_p = inter_mcu_state.f_rates[AXIS_P] * M_PI/RATE_PI_S;
booz_estimator_q = inter_mcu_state.f_rates[AXIS_Q] * M_PI/RATE_PI_S;
booz_estimator_r = inter_mcu_state.f_rates[AXIS_R] * M_PI/RATE_PI_S;
booz_estimator_phi = inter_mcu_state.f_eulers[AXIS_X] * M_PI/ANGLE_PI;
booz_estimator_theta = inter_mcu_state.f_eulers[AXIS_Y] * M_PI/ANGLE_PI;
booz_estimator_psi = inter_mcu_state.f_eulers[AXIS_Z] * M_PI/ANGLE_PI;
}
sw/airborne/booz_estimator.h
+1
View File
@@ -14,5 +14,6 @@ extern float booz_estimator_theta;
extern float booz_estimator_psi;
extern void booz_estimator_init( void );
extern void booz_estimator_read_inter_mcu_state( void );
#endif /* BOOZ_ESTIMATOR_H */
sw/airborne/booz_filter_main.c
+7 -9
View File
@@ -14,7 +14,7 @@
#include "downlink.h"
#include "booz_filter_telemetry.h"
#include "link_imu.h"
#include "booz_link_mcu.h"
static inline void on_imu_event( void );
@@ -43,22 +43,22 @@ STATIC_INLINE void booz_filter_main_init( void ) {
imu_v3_init();
multitilt_init();
//FIXME
#ifndef SITL
link_imu_init();
#endif
booz_link_mcu_init();
int_enable();
}
// static uint32_t t0, t1, diff;
STATIC_INLINE void booz_filter_main_event_task( void ) {
/* check if measurements are available */
ImuEventCheckAndHandle(on_imu_event);
}
STATIC_INLINE void booz_filter_main_periodic_task( void ) {
/* triger measurements */
ImuPeriodic();
static uint8_t _50hz = 0;
_50hz++;
@@ -92,7 +92,5 @@ static inline void on_imu_event( void ) {
break;
}
#ifndef SITL
link_imu_send();
#endif
booz_link_mcu_send();
}
sw/airborne/booz_link_mcu.c
+95
View File
@@ -0,0 +1,95 @@
#include "booz_link_mcu.h"
#include "std.h"
struct booz_inter_mcu_state booz_link_mcu_state_unused;
uint16_t booz_link_mcu_crc;
#define BOOZ_LINK_MCU_CRC_INIT 0x0
#define BOOZ_LINK_MCU_PAYLOAD_LENGTH (sizeof(struct booz_inter_mcu_state) - 2)
#define BoozLinkMcuCrcLow(x) (((x)&0xff))
#define BoozLinkMcuCrcHigh(x) (((x)>>8))
#define BoozLinkMcuComputeCRC() { \
uint8_t i; \
booz_link_mcu_crc = BOOZ_LINK_MCU_CRC_INIT; \
for(i = 0; i < BOOZ_LINK_MCU_PAYLOAD_LENGTH; i++) { \
uint8_t _byte = ((uint8_t*)&inter_mcu_state)[i]; \
uint8_t a = ((uint8_t)BoozLinkMcuCrcHigh(booz_link_mcu_crc)) + _byte; \
uint8_t b = ((uint8_t)BoozLinkMcuCrcLow(booz_link_mcu_crc)) + a; \
booz_link_mcu_crc = b | a << 8; \
} \
}
#ifdef BOOZ_FILTER_MCU /* FILTER LPC code */
#include "imu_v3.h"
#include "multitilt.h"
/* FIXME!!!! two function with same name in single MCU configuration */
/* by chance booz_link_mcu_hw_init does nothing in sim */
#ifndef SITL
void booz_link_mcu_init ( void ) {
booz_link_mcu_hw_init();
}
#endif
void booz_link_mcu_send ( void ) {
BoozLinkMcuSetUnavailable();
inter_mcu_fill_state();
BoozLinkMcuComputeCRC();
inter_mcu_state.crc = booz_link_mcu_crc;
Spi0InitBuf();
BoozLinkMcuSetAvailable();
}
#endif /* BOOZ_FILTER_MCU */
#ifdef BOOZ_CONTROLLER_MCU /* lpc controller board */
#include "spi.h"
#include "booz_estimator.h"
uint32_t booz_link_mcu_nb_err;
uint8_t booz_link_mcu_status;
uint32_t booz_link_mcu_timeout;
#define BOOZ_LINK_MCU_TIMEOUT 100
void booz_link_mcu_init ( void ) {
booz_link_mcu_hw_init();
booz_link_mcu_nb_err = 0;
booz_link_mcu_status = IMU_NO_LINK;
booz_link_mcu_timeout = BOOZ_LINK_MCU_TIMEOUT;
}
void booz_link_mcu_event_task( void ) {
BoozLinkMcuComputeCRC();
if (booz_link_mcu_crc == inter_mcu_state.crc) {
booz_link_mcu_timeout = 0;
booz_link_mcu_status = inter_mcu_state.status;
booz_estimator_read_inter_mcu_state();
}
else {
booz_link_mcu_nb_err++;
}
}
extern void booz_link_mcu_periodic_task( void ) {
if (booz_link_mcu_timeout < BOOZ_LINK_MCU_TIMEOUT)
booz_link_mcu_timeout++;
else
booz_link_mcu_status = IMU_NO_LINK;
}
#endif /* >BOOZ_CONTROLLER_MCU */
sw/airborne/booz_link_mcu.h
+53
View File
@@ -0,0 +1,53 @@
#ifndef BOOZ_LINK_LINK_MCU_H
#define BOOZ_LINK_LINK_MCU_H
#include <inttypes.h>
#include "6dof.h"
#include "booz_link_mcu_hw.h"
#include "booz_inter_mcu.h"
#include "spi.h"
extern void booz_link_mcu_init ( void );
extern void booz_link_mcu_hw_init ( void );
extern struct booz_inter_mcu_state booz_link_mcu_state_unused; /* single dir only */
#define CRC_INIT 0x0
#define CrcLow(x) ((x)&0xff)
#define CrcHigh(x) ((x)>>8)
static inline uint16_t BoozLinkMcuCrcUpdate(uint16_t crc, uint8_t data) {
uint8_t a = ((uint8_t)CrcHigh(crc)) + data;
uint8_t b = ((uint8_t)CrcLow(crc)) + a;
crc = b | a << 8;
return crc;
}
#ifdef BOOZ_FILTER_MCU
extern void booz_link_mcu_send( void );
#endif /* BOOZ_FILTER_MCU */
#ifdef BOOZ_CONTROLLER_MCU
extern uint32_t booz_link_mcu_nb_err;
extern uint8_t booz_link_mcu_status;
extern void booz_link_mcu_event_task( void );
extern void booz_link_mcu_periodic_task( void );
#define BoozLinkMcuEventCheckAndHandle() { \
if (spi_message_received) { \
spi_message_received = FALSE; \
booz_link_mcu_event_task(); \
} \
} \
#endif /* BOOZ_CONTROLLER_MCU */
#endif /* BOOZ_LINK_LINK_MCU_H */
sw/airborne/booz_telemetry.h
+32 -8
View File
@@ -8,7 +8,7 @@
#include "radio_control.h"
#include "actuators.h"
#include "link_imu.h"
#include "booz_link_mcu.h"
#include "booz_estimator.h"
#include "booz_autopilot.h"
#include "booz_control.h"
@@ -19,16 +19,40 @@
#include "downlink.h"
#define PERIODIC_SEND_BOOZ_STATUS() DOWNLINK_SEND_BOOZ_STATUS(&link_imu_nb_err, &link_imu_status, &rc_status, &booz_autopilot_mode, &booz_autopilot_mode, &cpu_time_sec, &buss_twi_blmc_nb_err)
#define PERIODIC_SEND_BOOZ_FD() DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, &booz_estimator_q, &booz_estimator_r,\
&booz_estimator_phi, &booz_estimator_theta, &booz_estimator_psi);
#define PERIODIC_SEND_BOOZ_DEBUG() DOWNLINK_SEND_BOOZ_DEBUG(&booz_control_p_sp, &booz_control_q_sp, &booz_control_r_sp, &booz_control_power_sp);
#define PERIODIC_SEND_BOOZ_STATUS() \
DOWNLINK_SEND_BOOZ_STATUS(&booz_link_mcu_nb_err, \
&booz_link_mcu_status, \
&rc_status, \
&booz_autopilot_mode, \
&booz_autopilot_mode, \
&cpu_time_sec, \
&buss_twi_blmc_nb_err)
#define PERIODIC_SEND_ACTUATORS() DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators);
#define PERIODIC_SEND_BOOZ_FD() \
DOWNLINK_SEND_BOOZ_FD(&booz_estimator_p, \
&booz_estimator_q, \
&booz_estimator_r, \
&booz_estimator_phi, \
&booz_estimator_theta, \
&booz_estimator_psi);
#define PERIODIC_SEND_BOOZ_RATE_LOOP() DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_p, &booz_control_p_sp, &booz_estimator_q, &booz_control_q_sp, &booz_estimator_r, &booz_control_r_sp );
#define PERIODIC_SEND_BOOZ_DEBUG() \
DOWNLINK_SEND_BOOZ_DEBUG(&booz_control_p_sp, \
&booz_control_q_sp, \
&booz_control_r_sp, \
&booz_control_power_sp);
#define PERIODIC_SEND_BOOZ_ATT_LOOP() DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_phi_sp, &booz_estimator_theta, &booz_control_theta_sp);
#define PERIODIC_SEND_ACTUATORS() \
DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators);
#define PERIODIC_SEND_BOOZ_RATE_LOOP() \
DOWNLINK_SEND_BOOZ_RATE_LOOP(&booz_estimator_p, &booz_control_p_sp, \
&booz_estimator_q, &booz_control_q_sp, \
&booz_estimator_r, &booz_control_r_sp );
#define PERIODIC_SEND_BOOZ_ATT_LOOP() \
DOWNLINK_SEND_BOOZ_ATT_LOOP(&booz_estimator_phi, &booz_control_phi_sp, \
&booz_estimator_theta, &booz_control_theta_sp);
#define PERIODIC_SEND_BOOZ_UF_RATES() \
DOWNLINK_SEND_BOOZ_UF_RATES(&booz_estimator_uf_p, \
sw/airborne/control_grz.c
-320
View File
@@ -1,320 +0,0 @@
/*
* Paparazzi $Id$
*
* Copyright (C) 2006 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#include "std.h"
#include "airframe.h"
#include "control_grz.h"
#include "link_imu.h"
#include "paparazzi.h"
#include "radio_control.h"
#include "commands.h"
#include "estimator.h"
#include "fbw_downlink.h"
#define NORMALIZE(n, lower, uper) { \
while ( n > uper) { n -= (uper - lower);} \
while ( n < lower) { n += (uper - lower);} \
}
float trim_roll = -450;
float trim_yaw = -250;
float trim_pitch = 0;
float max_roll_dot_sp = (1.5/MAX_PPRZ);
float max_pitch_dot_sp = (1.5/MAX_PPRZ);
float max_yaw_dot_sp = (1.5/MAX_PPRZ);
float max_roll_sp = (0.4/MAX_PPRZ);
float max_pitch_sp = (0.4/MAX_PPRZ);
/* yaw is incremented acording to stick position */
#define UPDATE_SP_DT 0.016384
float max_yaw_sp = (0.8/MAX_PPRZ*UPDATE_SP_DT);
float max_v_sp = (6./MAX_PPRZ);
static float throttle_setpoint = 0.;
float max_z_dot_sp = (5./MAX_PPRZ);
float ctl_grz_z_dot_pgain = -2000;
float ctl_grz_z_dot_igain = 3.;
float ctl_grz_z_dot_dgain = 8.;
float ctl_grz_z_dot = 0;
float ctl_grz_z_dot_setpoint = 0.;
float ctl_grz_z_dot_sum_err = 0.;
float ctl_grz_throttle_level = 6700.;
float max_z_sp = (4./MAX_PPRZ);
float ctl_grz_z_pgain = -1.2;
float ctl_grz_z_igain = 0.;
float ctl_grz_z_dgain = 5.;
float ctl_grz_z = 0;
float ctl_grz_z_setpoint = 0.;
float ctl_grz_z_sum_err = 0.;
bool_t flying = FALSE;
#define MIN_Z_DOT -1.
#define MAX_Z_DOT 1.
struct pid roll_pid;
struct pid pitch_pid;
struct pid yaw_pid;
struct pid roll_dot_pid;
struct pid pitch_dot_pid;
struct pid yaw_dot_pid;
struct pid vn_pid;
struct pid ve_pid;
void ctl_grz_init( void ) {
roll_dot_pid.p_gain = 4700.;
roll_dot_pid.i_gain = 1.;
roll_dot_pid.d_gain = 1.875;
pitch_dot_pid.p_gain = -4700.;
pitch_dot_pid.i_gain = 1.;
pitch_dot_pid.d_gain = 1.875;
yaw_dot_pid.p_gain = -4700.;
yaw_dot_pid.i_gain = 1.;
yaw_dot_pid.d_gain = 2.;
roll_pid.p_gain = -5;
roll_pid.i_gain = 0.;
roll_pid.d_gain = 1.;
roll_pid.saturation = 3.;
pitch_pid.p_gain = -5;
pitch_pid.i_gain = 0.;
pitch_pid.d_gain = 1.;
pitch_pid.saturation = 3.;
yaw_pid.p_gain = -1.;
yaw_pid.i_gain = 0.;
yaw_pid.d_gain = 0.;
yaw_pid.saturation = 2.;
yaw_pid.set_point = 0.; // RadOfDeg(-20);
vn_pid.p_gain = -0.1;
vn_pid.i_gain = 0.;
vn_pid.d_gain = 0.;
vn_pid.saturation = .2;
ve_pid.p_gain = -0.2;
ve_pid.i_gain = 1.;
ve_pid.d_gain = 1.;
ve_pid.saturation = .2;
}
/** FIXME: should be done by SetCommandsFromRC(commands); */
void ctl_grz_set_setpoints_rate( void ) {
roll_dot_pid.set_point = - commands[COMMAND_ROLL_DOT] * max_roll_dot_sp;
pitch_dot_pid.set_point = commands[COMMAND_PITCH_DOT] * max_pitch_dot_sp;
yaw_dot_pid.set_point = - commands[COMMAND_YAW_DOT] * max_yaw_dot_sp;
throttle_setpoint = commands[COMMAND_Z];
}
#define YAW_GAP ((int16_t)(0.05*MAX_PPRZ))
void ctl_grz_set_setpoints_auto1( void ) {
roll_pid.set_point = - rc_values[RADIO_ROLL] * max_roll_sp;
pitch_pid.set_point = rc_values[RADIO_PITCH] * max_pitch_sp;
#if 0
// yaw_dot_pid.set_point = - rc_values[RADIO_YAW] * max_yaw_dot_sp;
/* yaw stick increments yaw setpoint */
if (rc_values[RADIO_YAW] > YAW_GAP || rc_values[RADIO_YAW] < -YAW_GAP) {
yaw_pid.set_point += - rc_values[RADIO_YAW] * max_yaw_dot_sp;
NORMALIZE(yaw_pid.set_point, -M_PI, M_PI);
}
#endif
// ctl_grz_z_dot_setpoint = (rc_values[RADIO_THROTTLE] - MAX_PPRZ/2) * max_z_dot_sp;
throttle_setpoint = rc_values[RADIO_THROTTLE];
// ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.5;
}
void ctl_grz_set_setpoints_auto2( void ) {
roll_pid.set_point = - rc_values[RADIO_ROLL] * max_roll_sp;
pitch_pid.set_point = rc_values[RADIO_PITCH] * max_pitch_sp;
/* float vright = - rc_values[RADIO_ROLL] * max_v_sp; */
/* float vfront = - rc_values[RADIO_PITCH] * max_v_sp; */
/* float alpha = M_PI/2. - yaw_pid.measure; */
/* float cos_alpha = cos(alpha); */
/* float sin_alpha = sin(alpha); */
/* ve_pid.set_point = sin_alpha*vright + cos_alpha*vfront; */
/* vn_pid.set_point = - cos_alpha*vright + sin_alpha*vfront; */
ctl_grz_z_dot_setpoint = (rc_values[RADIO_THROTTLE] - MAX_PPRZ/2) * max_z_dot_sp;
// ctl_grz_z_setpoint = rc_values[RADIO_THROTTLE] * max_z_sp - 0.5;
// throttle_setpoint = rc_values[RADIO_THROTTLE];
}
void ctl_grz_set_measures( void ) {
roll_dot_pid.measure = link_imu_state.rates[AXIS_X];
pitch_dot_pid.measure = link_imu_state.rates[AXIS_Y];
yaw_dot_pid.measure = link_imu_state.rates[AXIS_Z];
roll_pid.measure = link_imu_state.eulers[AXIS_X];
pitch_pid.measure = link_imu_state.eulers[AXIS_Y];
yaw_pid.measure = link_imu_state.eulers[AXIS_Z];
ve_pid.measure = estimator_hspeed_mod*sin(estimator_hspeed_dir);
vn_pid.measure = estimator_hspeed_mod*cos(estimator_hspeed_dir);
}
void ctl_grz_rate_run ( void ) {
float err = roll_dot_pid.measure - roll_dot_pid.set_point;
float d_err = err - roll_dot_pid.last_err;
roll_dot_pid.last_err = err;
if (flying) {
roll_dot_pid.sum_err += err/100;
Bound(roll_dot_pid.sum_err, -0.2, 0.2);
} else
roll_dot_pid.sum_err = 0.;
commands[COMMAND_ROLL] = TRIM_PPRZ((int16_t)((err + d_err * roll_dot_pid.d_gain + roll_dot_pid.sum_err * roll_dot_pid.i_gain) * roll_dot_pid.p_gain));
err = pitch_dot_pid.measure - pitch_dot_pid.set_point;
d_err = err - pitch_dot_pid.last_err;
pitch_dot_pid.last_err = err;
if (flying) {
pitch_dot_pid.sum_err += err/100;
Bound(pitch_dot_pid.sum_err, -0.2, 0.2);
} else
pitch_dot_pid.sum_err = 0;
commands[COMMAND_PITCH] = TRIM_PPRZ((int16_t)((err + d_err * pitch_dot_pid.d_gain + pitch_dot_pid.sum_err * pitch_dot_pid.i_gain) * pitch_dot_pid.p_gain));
err = yaw_dot_pid.measure - yaw_dot_pid.set_point;
d_err = err - yaw_dot_pid.last_err;
yaw_dot_pid.last_err = err;
if (flying) {
yaw_dot_pid.sum_err += err/100;
Bound(yaw_dot_pid.sum_err, -0.2, 0.2);
} else
yaw_dot_pid.sum_err = 0;
commands[COMMAND_YAW] = TRIM_PPRZ((int16_t)((err + d_err * yaw_dot_pid.d_gain + yaw_dot_pid.sum_err * yaw_dot_pid.i_gain) * yaw_dot_pid.p_gain));
commands[COMMAND_THROTTLE] = throttle_setpoint;
}
void ctl_grz_attitude_run ( void ) {
float err = roll_pid.measure - roll_pid.set_point;
float d_err = err - roll_pid.last_err;
roll_pid.last_err = err;
roll_dot_pid.set_point = (err + d_err * roll_pid.d_gain) * roll_pid.p_gain;
// Bound(roll_dot_pid.set_point, roll_pid.saturation, -roll_pid.saturation);
err = pitch_pid.measure - pitch_pid.set_point;
d_err = err - pitch_pid.last_err;
pitch_pid.last_err = err;
pitch_dot_pid.set_point = (err + d_err * pitch_pid.d_gain) * pitch_pid.p_gain;
// Bound(pitch_dot_pid.set_point, pitch_pid.saturation, -pitch_pid.saturation);
err = yaw_pid.measure - yaw_pid.set_point;
NORMALIZE(err, -M_PI, M_PI);
d_err = err - yaw_pid.last_err;
yaw_pid.last_err = err;
yaw_dot_pid.set_point = (err + d_err * yaw_pid.d_gain) * yaw_pid.p_gain;
// Bound(yaw_dot_pid.set_point, yaw_pid.saturation, -yaw_pid.saturation);
}
void ctl_grz_alt_run( void ) {
float err = ctl_grz_z - ctl_grz_z_setpoint;
static float z_last_err;
float d_err = err - z_last_err;
z_last_err = err;
// ctl_grz_z_sum_err += err;
if (ctl_grz_z_setpoint < 0.1)
ctl_grz_z_dot_setpoint = -10;
else
ctl_grz_z_dot_setpoint = Chop(((err + d_err * ctl_grz_z_dgain + ctl_grz_z_sum_err * ctl_grz_z_igain) * ctl_grz_z_pgain), MIN_Z_DOT, MAX_Z_DOT);
}
void ctl_grz_z_dot_run( void ) {
float err = ctl_grz_z_dot - ctl_grz_z_dot_setpoint;
static float z_dot_last_err;
float d_err = err - z_dot_last_err;
z_dot_last_err = err;
if (flying) {
ctl_grz_z_dot_sum_err += err/600;
Bound(ctl_grz_z_dot_sum_err, -1., 1.);
} else
ctl_grz_z_dot_sum_err = 0;
throttle_setpoint = TRIM_PPRZ(ctl_grz_throttle_level + (int16_t)((err + d_err * ctl_grz_z_dot_dgain + ctl_grz_z_dot_sum_err * ctl_grz_z_dot_igain) * ctl_grz_z_dot_pgain));
}
float north_angle_set_point;
float east_angle_set_point;
void ctl_grz_horiz_speed_run ( void ) {
float err = ve_pid.measure - ve_pid.set_point;
float d_err = err - ve_pid.last_err;
ve_pid.last_err = err;
ve_pid.sum_err += err/100;
east_angle_set_point = (err + d_err * ve_pid.d_gain + ve_pid.sum_err * ve_pid.i_gain) * ve_pid.p_gain;
Bound(east_angle_set_point, -ve_pid.saturation, ve_pid.saturation);
Bound(ve_pid.sum_err, -3, 3);
err = vn_pid.measure - vn_pid.set_point;
d_err = err - vn_pid.last_err;
vn_pid.last_err = err;
vn_pid.sum_err += err/100;
north_angle_set_point = - ((err + d_err * ve_pid.d_gain + vn_pid.sum_err * ve_pid.i_gain) * ve_pid.p_gain);
Bound(north_angle_set_point, -vn_pid.saturation, vn_pid.saturation);
Bound(vn_pid.sum_err, -3, 3);
float alpha = (M_PI/2. - yaw_pid.measure);
float cos_alpha = cos(alpha);
float sin_alpha = sin(alpha);
roll_pid.set_point = sin_alpha*east_angle_set_point + cos_alpha*north_angle_set_point;
pitch_pid.set_point = - cos_alpha*east_angle_set_point + sin_alpha*north_angle_set_point;
}
void ctl_grz_reset( void ) {
ctl_grz_z_dot_sum_err = 0;
ve_pid.sum_err = 0;
vn_pid.sum_err = 0;
roll_dot_pid.sum_err = 0;
pitch_dot_pid.sum_err = 0;
yaw_dot_pid.sum_err = 0;
}
sw/airborne/imu_v3.h
+19
View File
@@ -4,6 +4,7 @@
#include "std.h"
#include "6dof.h"
#include "max1167.h"
#include "imu_v3_hw.h"
/* calibrated sensor readings */
@@ -22,6 +23,7 @@ extern uint16_t imu_gyro_raw[AXIS_NB];
extern int16_t imu_mag_raw[AXIS_NB];
extern void imu_v3_init(void);
extern void imu_v3_hw_init(void);
extern void imu_v3_detect_vehicle_still(void);
extern bool_t imu_vehicle_still;
extern float imu_vs_gyro_initial_bias[AXIS_NB];
@@ -132,6 +134,23 @@ extern struct adc_buf buf_bat;
}
#define ImuEventCheckAndHandle(user_handler) { \
if (max1167_status == STA_MAX1167_DATA_AVAILABLE) { \
max1167_status = STA_MAX1167_IDLE; \
ImuUpdateGyros(); \
ImuUpdateAccels(); \
user_handler(); \
} \
}
#define ImuPeriodic() { \
if (max1167_status != STA_MAX1167_IDLE) { \
DOWNLINK_SEND_AHRS_OVERRUN(); \
} \
else { \
max1167_read(); \
} \
}
#endif /* IMU_V3_H */
sw/airborne/link_imu.c
-232
View File
@@ -1,232 +0,0 @@
#include "link_imu.h"
#include "std.h"
struct imu_state link_imu_state;
struct imu_state link_imu_state_foo;
uint16_t link_imu_crc;
#define LINK_IMU_CRC_INIT 0x0
#define LINK_IMU_PAYLOAD_LENGTH (sizeof(struct imu_state) - 2)
#define LinkImuCrcLow(x) ((x)&0xff)
#define LinkImuCrcHigh(x) ((x)>>8)
#define LinkImuComputeCRC() { \
uint8_t i; \
link_imu_crc = LINK_IMU_CRC_INIT; \
for(i = 0; i < LINK_IMU_PAYLOAD_LENGTH; i++) { \
uint8_t _byte = ((uint8_t*)&link_imu_state)[i]; \
uint8_t a = ((uint8_t)LinkImuCrcHigh(link_imu_crc)) + _byte; \
uint8_t b = ((uint8_t)LinkImuCrcLow(link_imu_crc)) + a; \
link_imu_crc = b | a << 8; \
} \
}
#ifdef IMU /* IMU LPC code */
#include "imu_v3.h"
#include "multitilt.h"
#include "LPC21xx.h"
#include "armVIC.h"
volatile uint8_t spi0_data_available;
volatile uint8_t spi0_idx_buf;
uint8_t* spi0_buffer_output = (uint8_t*)&link_imu_state;
uint8_t* spi0_buffer_input = (uint8_t*)&link_imu_state_foo;
#define LinkImuSetUnavailable() { IO1SET = _BV(SPI0_DRDY); }
#define LinkImuSetAvailable() { IO1CLR = _BV(SPI0_DRDY); }
void SPI0_ISR(void) __attribute__((naked));
#define Spi0InitBuf() { \
spi0_idx_buf = 0; \
S0SPDR = spi0_buffer_output[0]; \
}
#define Spi0OneByte() { \
/* FIXME : do something usefull with the status register reading */ \
uint8_t foo __attribute__((unused)) = S0SPSR; \
spi0_buffer_input[spi0_idx_buf] = S0SPDR; \
spi0_idx_buf++; \
if (spi0_idx_buf < sizeof(link_imu_state)) { \
S0SPDR = spi0_buffer_output[spi0_idx_buf]; \
} \
else { \
spi0_data_available = TRUE; \
LinkImuSetUnavailable(); \
} \
}
/*
wiring on IMU_V3
P0_4 SCK0
P0_5 MISO0
P0_6 MOSI0
P0_7 SSEL0
P1_24 DRDY
*/
#define SPI0_DRDY 24
/* */
#define S0SPCR_SPIE 7 /* SPI enable */
void link_imu_init ( void ) {
/* init SPI0 */
/* setup pins for sck, miso, mosi, SSEL */
PINSEL0 |= 1<<8 | 1<<10| 1<<12 | 1<< 14;
/* setup P1_16 to P1_25 as GPIO */
PINSEL2 &= ~(1<<3);
/* P1_24 (DRDY) is output */
SetBit(IO1DIR, SPI0_DRDY);
/* DRDY idles high */
LinkImuSetUnavailable();
/* configure SPI : 8 bits CPOL=0 CPHA=0 MSB_first slave */
S0SPCR = _BV(3);
/* setup SPI clock rate */
S0SPCCR = 0x20;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(VIC_SPI0); // SPI0 selected as IRQ
VICIntEnable = VIC_BIT(VIC_SPI0); // SPI0 interrupt enabled
VICVectCntl1 = VIC_ENABLE | VIC_SPI0;
VICVectAddr1 = (uint32_t)SPI0_ISR; // address of the ISR
/* clear pending interrupt */
SetBit(S0SPINT, SPI0IF);
/* enable SPI interrupt */
SetBit(S0SPCR, S0SPCR_SPIE);
}
void link_imu_send ( void ) {
LinkImuSetUnavailable();
link_imu_state.r_rates[AXIS_P] = imu_vs_gyro_unbiased[AXIS_P] * RATE_PI_S/M_PI;
link_imu_state.r_rates[AXIS_Q] = imu_vs_gyro_unbiased[AXIS_Q] * RATE_PI_S/M_PI;
link_imu_state.r_rates[AXIS_R] = imu_vs_gyro_unbiased[AXIS_R] * RATE_PI_S/M_PI;
link_imu_state.f_rates[AXIS_P] = mtt_p * RATE_PI_S/M_PI;
link_imu_state.f_rates[AXIS_Q] = mtt_q * RATE_PI_S/M_PI;
link_imu_state.f_rates[AXIS_R] = mtt_r * RATE_PI_S/M_PI;
link_imu_state.f_eulers[AXIS_X] = mtt_phi * ANGLE_PI/M_PI;
link_imu_state.f_eulers[AXIS_Y] = mtt_theta* ANGLE_PI/M_PI;
link_imu_state.f_eulers[AXIS_Z] = mtt_psi * ANGLE_PI/M_PI;
link_imu_state.status = IMU_RUNNING;
LinkImuComputeCRC();
link_imu_state.crc = link_imu_crc;
Spi0InitBuf();
LinkImuSetAvailable();
}
void SPI0_ISR(void) {
ISR_ENTRY();
Spi0OneByte();
/* clear the interrupt */
S0SPINT = _BV(SPI0IF);
/* clear this interrupt from the VIC */
VICVectAddr = 0x00000000;
ISR_EXIT();
}
#endif /* IMU */
#ifdef CONTROLLER /* lpc controller board */
/* IMU connected to SSP ( aka SPI1 ) */
#include "LPC21xx.h"
#include "interrupt_hw.h"
#include "spi.h"
#include "booz_estimator.h"
uint32_t link_imu_nb_err;
uint8_t link_imu_status;
uint32_t link_imu_timeout;
#define LINK_IMU_TIMEOUT 100
/* DRDY connected pin to P0.16 EINT0 */
#define LINK_IMU_DRDY_PINSEL PINSEL1
#define LINK_IMU_DRDY_PINSEL_VAL 0x01
#define LINK_IMU_DRDY_PINSEL_BIT 0
#define LINK_IMU_DRDY_EINT 0
static void EXTINT0_ISR(void) __attribute__((naked));
void link_imu_init ( void ) {
/* configure DRDY pin */
LINK_IMU_DRDY_PINSEL |= LINK_IMU_DRDY_PINSEL_VAL << LINK_IMU_DRDY_PINSEL_BIT;
SetBit(EXTMODE, LINK_IMU_DRDY_EINT); /* EINT is edge trigered */
ClearBit(EXTPOLAR,LINK_IMU_DRDY_EINT); /* EINT is trigered on falling edge */
SetBit(EXTINT,LINK_IMU_DRDY_EINT); /* clear pending EINT */
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT( VIC_EINT0 ); /* select EINT0 as IRQ source */
VICIntEnable = VIC_BIT( VIC_EINT0 ); /* enable it */
VICVectCntl9 = VIC_ENABLE | VIC_EINT0;
VICVectAddr9 = (uint32_t)EXTINT0_ISR; // address of the ISR
spi_buffer_input = (uint8_t*)&link_imu_state;
spi_buffer_output = (uint8_t*)&link_imu_state_foo;
spi_buffer_length = sizeof(link_imu_state);
link_imu_nb_err = 0;
link_imu_status = IMU_NO_LINK;
link_imu_timeout = LINK_IMU_TIMEOUT;
}
void link_imu_event_task( void ) {
LinkImuComputeCRC();
if (link_imu_crc == link_imu_state.crc) {
link_imu_timeout = 0;
link_imu_status = link_imu_state.status;
booz_estimator_uf_p = link_imu_state.r_rates[AXIS_P] * M_PI/RATE_PI_S;
booz_estimator_uf_q = link_imu_state.r_rates[AXIS_Q] * M_PI/RATE_PI_S;
booz_estimator_uf_r = link_imu_state.r_rates[AXIS_R] * M_PI/RATE_PI_S;
booz_estimator_p = link_imu_state.f_rates[AXIS_P] * M_PI/RATE_PI_S;
booz_estimator_q = link_imu_state.f_rates[AXIS_Q] * M_PI/RATE_PI_S;
booz_estimator_r = link_imu_state.f_rates[AXIS_R] * M_PI/RATE_PI_S;
booz_estimator_phi = link_imu_state.f_eulers[AXIS_X] * M_PI/ANGLE_PI;
booz_estimator_theta = link_imu_state.f_eulers[AXIS_Y] * M_PI/ANGLE_PI;
booz_estimator_psi = link_imu_state.f_eulers[AXIS_Z] * M_PI/ANGLE_PI;
}
else {
link_imu_nb_err++;
}
}
extern void link_imu_periodic_task( void ) {
if (link_imu_timeout < LINK_IMU_TIMEOUT)
link_imu_timeout++;
else
link_imu_status = IMU_NO_LINK;
}
void EXTINT0_ISR(void) {
ISR_ENTRY();
SpiSelectSlave0();
SpiStart();
/* clear EINT2 */
SetBit(EXTINT,LINK_IMU_DRDY_EINT); /* clear EINT0 */
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
ISR_EXIT();
}
#endif /* CONTROLLER */
sw/airborne/link_imu.h
-56
View File
@@ -1,56 +0,0 @@
#ifndef LINK_IMU_H
#define LINK_IMU_H
#include <inttypes.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 0
#define IMU_RUNNING 1
#define IMU_CRASHED 2
#define IMU_NO_LINK 3
struct imu_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 imu_state link_imu_state;
extern void link_imu_init ( void );
#ifdef IMU
extern void link_imu_send( void );
#endif /* IMU */
#ifdef CONTROLLER
extern uint32_t link_imu_nb_err;
extern uint8_t link_imu_status;
extern void link_imu_event_task( void );
extern void link_imu_periodic_task( void );
#define LinkImuEventCheckAndHandle() { \
if (spi_message_received) { \
spi_message_received = FALSE; \
link_imu_event_task(); \
} \
} \
#endif /* CONTROLLER */
#endif /* LINK_IMU_H */
sw/airborne/main_fbw.c
-14
View File
@@ -54,8 +54,6 @@
#include "link_mcu.h"
#endif
#include "link_imu.h"
#ifdef ADC
struct adc_buf vsupply_adc_buf;
#endif
@@ -99,10 +97,6 @@ void init_fbw( void ) {
spi_init();
link_mcu_restart();
#endif
#ifdef LINK_IMU
spi_init();
link_imu_init();
#endif
fbw_mode = FBW_MODE_FAILSAFE;
@@ -169,14 +163,6 @@ void event_task_fbw( void) {
#endif /* MCU_SPI_LINK */
#endif /* INTER_MCU */
#ifdef LINK_IMU
if (spi_message_received) {
/* Got a message on SPI. */
spi_message_received = FALSE;
link_imu_event_task();
EstimatorSetAtt(link_imu_state.eulers[AXIS_X], link_imu_state.eulers[AXIS_Z], link_imu_state.eulers[AXIS_Y]);
}
#endif /* LINK_IMU */
}
sw/airborne/multitilt.h
+2 -2
View File
@@ -4,8 +4,8 @@
#include "std.h"
#define MT_STATUS_UNINIT 0
#define MT_STATUS_RUNNING 2
#define MT_STATUS_CRASHED 3
#define MT_STATUS_RUNNING 1
#define MT_STATUS_CRASHED 2
extern uint8_t mtt_status;
sw/airborne/sim/booz_link_mcu_hw.c
+3
View File
@@ -0,0 +1,3 @@
#include "booz_link_mcu.h"
void booz_link_mcu_hw_init ( void ) {}
sw/airborne/sim/booz_link_mcu_hw.h
+12
View File
@@ -0,0 +1,12 @@
#ifndef BOOZ_LINK_MCU_HW_H
#define BOOZ_LINK_MCU_HW_H
/* FIXME: not sure what that does here !!!! */
#define Spi0InitBuf() {}
#define BoozLinkMcuSetUnavailable() { }
#define BoozLinkMcuSetAvailable() { spi_message_received = TRUE; }
#endif /* BOOZ_LINK_MCU_HW_H */
sw/airborne/sim/imu_v3_hw.h
-20
View File
@@ -1,24 +1,4 @@
#ifndef IMU_V3_HW_H
#define IMU_V3_HW_H
#include "std.h"
//#include "booz_sensors_model.h"
extern void imu_v3_hw_init(void);
extern bool_t imu_data_available;
#define ImuEventCheckAndHandle(user_handler) { \
if (imu_data_available) { \
imu_data_available = FALSE; \
/* ImuUpdateGyros(); */ \
/* ImuUpdateAccels();*/ \
user_handler(); \
} \
}
#define ImuPeriodic() { }
#endif /* IMU_V3_HW_H */
sw/simulator/Makefile
+14 -2
View File
@@ -109,7 +109,7 @@ BOOZ_SIM_SRCS = main_booz_sim.c \
booz_joystick.c \
CFLAGS += -DSITL
CFLAGS += -DCONTROLLER
CFLAGS += -DBOOZ_CONTROLLER_MCU
CFLAGS += -DCONFIG=\"conf_booz.h\"
BOOZ_AB_SRCS += $(AB)/booz_controller_main.c
@@ -136,10 +136,16 @@ CFLAGS += -DDOWNLINK_TRANSPORT=IvyTransport
BOOZ_AB_SRCS += $(AB_ARCH)/ivy_transport.c
LDFLAGS += -lglibivy
BOOZ_AB_SRCS += $(AB)/booz_inter_mcu.c
CFLAGS += -DUSE_SPI
BOOZ_AB_SRCS += $(AB)/booz_link_mcu.c $(AB_ARCH)/booz_link_mcu_hw.c
BOOZ_AB_SRCS += $(AB)/spi.c $(AB_ARCH)/spi_hw.c
#../airborne/link_imu.c
#CFLAGS += -DDATALINK=PPRZ
#BOOZ_AB_SRCS += ../airborne/datalink.c
CFLAGS += -DBOOZ_FILTER_MCU
BOOZ_AB_SRCS += $(AB)/booz_estimator.c
BOOZ_AB_SRCS += $(AB)/booz_control.c
BOOZ_AB_SRCS += $(AB)/booz_autopilot.c
@@ -148,10 +154,16 @@ BOOZ_AB_SRCS += $(AB)/commands.c
BOOZ_AB_SRCS += $(AB)/booz_filter_main.c
CFLAGS += -DADC_CHANNEL_AX=1 -DADC_CHANNEL_AY=2 -DADC_CHANNEL_AZ=3 -DADC_CHANNEL_BAT=4
BOOZ_AB_SRCS += $(AB)/imu_v3.c $(AB_ARCH)/imu_v3_hw.c $(AB_ARCH)/adc_hw.c
BOOZ_AB_SRCS += $(AB_ARCH)/adc_hw.c
BOOZ_AB_SRCS += $(AB)/booz_filter_telemetry.c
BOOZ_AB_SRCS += $(AB)/max1167.c $(AB_ARCH)/max1167_hw.c
BOOZ_AB_SRCS += $(AB)/imu_v3.c $(AB_ARCH)/imu_v3_hw.c
BOOZ_AB_SRCS += $(AB)/multitilt.c
sw/simulator/booz_sensors_model.c
+44 -1
View File
@@ -1,9 +1,52 @@
#include "booz_sensors_model.h"
#include <math.h>
#include "booz_flight_model.h"
void booz_sensors_model_run(void) {
struct BoozSensorsModel bsm;
void booz_sensors_model_init(void) {
bsm.accel = v_get(AXIS_NB);
bsm.accel->ve[AXIS_X] = 0.;
bsm.accel->ve[AXIS_Y] = 0.;
bsm.accel->ve[AXIS_Z] = 0.;
bsm.gyro = v_get(AXIS_NB);
bsm.gyro->ve[AXIS_P] = 0.;
bsm.gyro->ve[AXIS_Q] = 0.;
bsm.gyro->ve[AXIS_R] = 0.;
bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB);
m_zero(bsm.gyro_sensitivity);
bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = 1./-0.0002202;
bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = 1./-0.0002150;
bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = 1./ 0.0002104;
bsm.gyro_neutral = v_get(AXIS_NB);
bsm.gyro_neutral->ve[AXIS_P] = 40885;
bsm.gyro_neutral->ve[AXIS_Q] = 40910;
bsm.gyro_neutral->ve[AXIS_R] = 39552;
}
void booz_sensors_model_run(void) {
/* extract rotational speed from flight model state */
static VEC *rate_body = VNULL;
rate_body = v_resize(rate_body, AXIS_NB);
rate_body->ve[AXIS_P] = bfm.state->ve[BFMS_P];
rate_body->ve[AXIS_Q] = bfm.state->ve[BFMS_Q];
rate_body->ve[AXIS_R] = bfm.state->ve[BFMS_R];
/* compute sensor readings */
bsm.gyro = mv_mlt(bsm.gyro_sensitivity, rate_body, bsm.gyro);
bsm.gyro = v_add(bsm.gyro, bsm.gyro_neutral, bsm.gyro);
bsm.gyro->ve[AXIS_P] = round( bsm.gyro->ve[AXIS_P]);
bsm.gyro->ve[AXIS_Q] = round( bsm.gyro->ve[AXIS_Q]);
bsm.gyro->ve[AXIS_R] = round( bsm.gyro->ve[AXIS_R]);
}
sw/simulator/booz_sensors_model.h
+11
View File
@@ -2,7 +2,18 @@
#define BOOZ_SENSORS_MODEL_H
#include "6dof.h"
#include <matrix.h>
extern void booz_sensors_model_init(void);
extern void booz_sensors_model_run(void);
struct BoozSensorsModel {
VEC* accel;
VEC* gyro;
MAT* gyro_sensitivity;
VEC* gyro_neutral;
};
extern struct BoozSensorsModel bsm;
#endif /* BOOZ_SENSORS_MODEL_H */
sw/simulator/main_booz_sim.c
+51 -31
View File
@@ -27,11 +27,14 @@ double sim_time;
#define DT_DISPLAY 0.04
double disp_time;
double foo_commands[] = {0., 0., 0., 0.};
double booz_sim_actuators_values[] = {0., 0., 0., 0.};
static gboolean booz_sim_periodic(gpointer data);
static inline void booz_sim_display(void);
static void booz_sim_set_ppm_from_joystick( void );
static void booz_sim_read_actuators( void );
#ifdef SIM_UART
static void sim_uart_init(void);
#endif
@@ -44,49 +47,42 @@ static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *arg
#include "booz_estimator.h"
#include "radio_control.h"
#include "actuators.h"
#include "imu_v3.h"
#include "booz_inter_mcu.h"
volatile bool_t ppm_valid;
#define RPM_OF_RAD_S(a) ((a)*60./M_PI)
static gboolean booz_sim_periodic(gpointer data) {
/* read actuators positions */
booz_sim_read_actuators();
booz_flight_model_run(DT, foo_commands);
booz_flight_model_run(DT, booz_sim_actuators_values);
booz_sensors_model_run();
sim_time += DT;
booz_sim_display();
booz_estimator_p = bfm.state->ve[BFMS_P];
booz_estimator_q = bfm.state->ve[BFMS_Q];
booz_estimator_r = bfm.state->ve[BFMS_R];
booz_estimator_phi = bfm.state->ve[BFMS_PHI];
booz_estimator_theta = bfm.state->ve[BFMS_THETA];
booz_estimator_psi = bfm.state->ve[BFMS_PSI];
/* call the filter periodic task to read sensors */
/* the sim implementation of max1167_read ( called by ImuPeriodic() ) */
/* will post a ImuEvent */
booz_filter_main_periodic_task();
/* process the ImuEvent */
/* it will run the filter and the inter-process communication which */
/* will post a BoozLinkMcuEvent in the Controller process */
booz_filter_main_event_task();
/* process the BoozLinkMcuEvent */
/* this will update the controller stimator */
booz_controller_main_event_task();
/* post a radio control event */
ppm_pulses[RADIO_THROTTLE] = 1223 + booz_joystick_value[JS_THROTTLE] * (2050-1223);
ppm_pulses[RADIO_PITCH] = 1498 + booz_joystick_value[JS_PITCH] * (2050-950);
ppm_pulses[RADIO_ROLL] = 1500 + booz_joystick_value[JS_ROLL] * (2050-950);
ppm_pulses[RADIO_YAW] = 1500 + booz_joystick_value[JS_YAW] * (2050-950);
ppm_pulses[RADIO_MODE] = 1500 + booz_joystick_value[JS_MODE] * (2050-950);
booz_sim_set_ppm_from_joystick();
ppm_valid = TRUE;
/* and let the controller process it */
booz_controller_main_event_task();
/* run periodic tak */
/* call the controller periodic task to run control loops */
booz_controller_main_periodic_task();
booz_filter_main_periodic_task();
/* get actuators values */
#if 0
foo_commands[SERVO_MOTOR_BACK] = (actuators[SERVO_MOTOR_BACK] - 1200)/(double)(1850-1200);
foo_commands[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 1200)/(double)(1850-1200);
foo_commands[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 1200)/(double)(1850-1200);
foo_commands[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT] - 1200)/(double)(1850-1200);
#endif
foo_commands[SERVO_MOTOR_BACK] = (actuators[SERVO_MOTOR_BACK] - 0)/(double)(255);
foo_commands[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 0)/(double)(255);
foo_commands[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 0)/(double)(255);
foo_commands[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT] - 0)/(double)(255);
booz_sim_display();
return TRUE;
}
@@ -98,6 +94,8 @@ int main ( int argc, char** argv) {
booz_flight_model_init();
booz_sensors_model_init();
booz_flightgear_init(fg_host, fg_port);
#ifdef SIM_UART
@@ -129,7 +127,7 @@ int main ( int argc, char** argv) {
// Helpers
////////////////////
#define RPM_OF_RAD_S(a) ((a)*60./M_PI)
static inline void booz_sim_display(void) {
if (sim_time >= disp_time) {
disp_time+= DT_DISPLAY;
@@ -144,8 +142,30 @@ static inline void booz_sim_display(void) {
static void booz_sim_set_ppm_from_joystick( void ) {
ppm_pulses[RADIO_THROTTLE] = 1223 + booz_joystick_value[JS_THROTTLE] * (2050-1223);
ppm_pulses[RADIO_PITCH] = 1498 + booz_joystick_value[JS_PITCH] * (2050-950);
ppm_pulses[RADIO_ROLL] = 1500 + booz_joystick_value[JS_ROLL] * (2050-950);
ppm_pulses[RADIO_YAW] = 1500 + booz_joystick_value[JS_YAW] * (2050-950);
ppm_pulses[RADIO_MODE] = 1500 + booz_joystick_value[JS_MODE] * (2050-950);
}
static void booz_sim_read_actuators( void ) {
#if 0
booz_sim_actuators_values[SERVO_MOTOR_BACK] = (actuators[SERVO_MOTOR_BACK] - 1200)/(double)(1850-1200);
booz_sim_actuators_values[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 1200)/(double)(1850-1200);
booz_sim_actuators_values[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 1200)/(double)(1850-1200);
booz_sim_actuators_values[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT] - 1200)/(double)(1850-1200);
#endif
booz_sim_actuators_values[SERVO_MOTOR_BACK] = (actuators[SERVO_MOTOR_BACK] - 0)/(double)(255);
booz_sim_actuators_values[SERVO_MOTOR_FRONT] = (actuators[SERVO_MOTOR_FRONT] - 0)/(double)(255);
booz_sim_actuators_values[SERVO_MOTOR_RIGHT] = (actuators[SERVO_MOTOR_RIGHT] - 0)/(double)(255);
booz_sim_actuators_values[SERVO_MOTOR_LEFT] = (actuators[SERVO_MOTOR_LEFT] - 0)/(double)(255);
}
#if defined DOWNLINK_TRANSPORT && DOWNLINK_TRANSPORT == IvyTransport