Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-26 01:23:44 +00:00
parent 065119d023
commit 39cab4ceb9
22 changed files with 229 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz.xml
+33 -33
View File
@@ -88,6 +88,39 @@ ctl.srcs += commands.c
ctl.srcs += booz_telemetry.c
#
# multitilt
#
mtt.ARCHDIR = $(ARCHI)
mtt.ARCH = arm7tdmi
mtt.TARGET = mtt
mtt.TARGETDIR = mtt
mtt.CFLAGS += -DIMU -DCONFIG=\"pprz_imu.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)'
mtt.srcs = main_mtt.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
mtt.CFLAGS += -DLED
mtt.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
mtt.srcs += $(SRC_ARCH)/uart_hw.c
mtt.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
mtt.srcs += downlink.c pprz_transport.c
mtt.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
mtt.srcs += $(SRC_ARCH)/adc_hw.c
mtt.srcs += $(SRC_ARCH)/max1167.c
mtt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
mtt.srcs += multitilt.c
mtt.CFLAGS += -DIMU
mtt.srcs += link_imu.c
#
# AHRS CPU
#
@@ -125,39 +158,6 @@ imu.CFLAGS += -DEKF_UPDATE_CONTINUOUS
imu.srcs += link_imu.c
#
# multitilt
#
mtt.ARCHDIR = $(ARCHI)
mtt.ARCH = arm7tdmi
mtt.TARGET = mtt
mtt.TARGETDIR = mtt
mtt.CFLAGS += -DIMU -DCONFIG=\"pprz_imu.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(4e-3)'
mtt.srcs = main_mtt.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
mtt.CFLAGS += -DLED
mtt.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
mtt.srcs += $(SRC_ARCH)/uart_hw.c
mtt.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
mtt.srcs += downlink.c pprz_transport.c
mtt.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
mtt.srcs += $(SRC_ARCH)/adc_hw.c
mtt.srcs += $(SRC_ARCH)/max1167.c
mtt.srcs += imu_v3.c
mtt.srcs += multitilt.c
mtt.CFLAGS += -DIMU
mtt.srcs += link_imu.c
</makefile>
</airframe>
sw/airborne/arm7/actuators_buss_twi_blmc_hw.c
+2 -2
View File
@@ -2,8 +2,8 @@
uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB];
volatile bool_t buss_twi_blmc_status;
volatile bool_t buss_twi_blmc_i2c_done;
volatile uint8_t buss_twi_blmc_nb_err;
volatile bool_t buss_twi_blmc_i2c_done;
volatile uint8_t buss_twi_blmc_idx;
const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = { 0x52, 0x54, 0x56, 0x58 };
@@ -13,6 +13,6 @@ void actuators_init ( void ) {
for (i=0; i<BUSS_TWI_BLMC_NB;i++)
buss_twi_blmc_motor_power[i] = 0;
buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;
buss_twi_blmc_i2c_done = TRUE;
buss_twi_blmc_nb_err = 0;
buss_twi_blmc_i2c_done = TRUE;
}
sw/airborne/arm7/actuators_buss_twi_blmc_hw.h
+1 -1
View File
@@ -28,8 +28,8 @@
#define BUSS_TWI_BLMC_NB 4
extern uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB];
extern volatile bool_t buss_twi_blmc_status;
extern volatile bool_t buss_twi_blmc_i2c_done;
extern volatile uint8_t buss_twi_blmc_nb_err;
extern volatile bool_t buss_twi_blmc_i2c_done;
extern volatile uint8_t buss_twi_blmc_idx;
extern const uint8_t buss_twi_blmc_addr[];
sw/airborne/arm7/imu_v3_hw.c
+52
View File
@@ -0,0 +1,52 @@
#include "imu_v3.h"
#include "max1167.h"
static void SPI1_ISR(void) __attribute__((naked));
#define SPI_SLAVE_NONE 0
#define SPI_SLAVE_MAX 1
uint8_t spi_cur_slave = SPI_SLAVE_NONE;
/* SSPCR0 settings */
#define SSP_DDS 0x07 << 0 /* data size : 8 bits */
#define SSP_FRF 0x00 << 4 /* frame format : SPI */
#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */
#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */
#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16 */
/* SSPCR1 settings */
#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */
#define SSP_MS 0x00 << 2 /* master slave mode : master */
#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */
void imu_v3_hw_init(void) {
max1167_init();
/* 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;
SSPCPSR = 0x20;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ
VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled
VICVectCntl7 = VIC_ENABLE | VIC_SPI1;
VICVectAddr7 = (uint32_t)SPI1_ISR; // address of the ISR
}
void SPI1_ISR(void) {
ISR_ENTRY();
Max1167OnSpiInt();
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
ISR_EXIT();
}
sw/airborne/arm7/imu_v3_hw.h
+9
View File
@@ -0,0 +1,9 @@
#ifndef IMU_V3_HW_H
#define IMU_V3_HW_H
extern void imu_v3_hw_init(void);
#endif /* IMU_V3_HW_H */
sw/airborne/booz_telemetry.h
+1 -1
View File
@@ -19,7 +19,7 @@
#include "downlink.h"
#define PERIODIC_SEND_BOOZ_STATUS() DOWNLINK_SEND_BOOZ_STATUS(&link_imu_nb_err, &link_imu_status, &rc_status, &booz_autopilot_mode, &buss_twi_blmc_idx, &cpu_time_sec, &buss_twi_blmc_nb_err)
#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);
sw/airborne/i2c.h
+2 -2
View File
@@ -3,9 +3,9 @@
#include "std.h"
#ifndef SITL
//#ifndef SITL
#include "i2c_hw.h"
#endif
//#endif
extern void i2c_init(void);
extern void i2c_receive(uint8_t slave_addr, uint8_t len, volatile bool_t* finished);
sw/airborne/imu_v3.c
+7 -2
View File
@@ -1,5 +1,7 @@
#include "imu_v3.h"
#include "imu_v3_hw.h"
#include CONFIG
#include "adc.h"
@@ -51,7 +53,10 @@ static bool_t imu_vs_buf_filled;
#define IMU_VS_ACCEL_RAW_VAR_MAX 7000.
void imu_init(void) {
void imu_v3_init(void) {
imu_v3_hw_init();
adc_buf_channel(ADC_CHANNEL_AX, &buf_ax, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(ADC_CHANNEL_AY, &buf_ay, DEFAULT_AV_NB_SAMPLE);
adc_buf_channel(ADC_CHANNEL_AZ, &buf_az, DEFAULT_AV_NB_SAMPLE);
@@ -76,7 +81,7 @@ void imu_init(void) {
}
void imu_detect_vehicle_still(void) {
void imu_v3_detect_vehicle_still(void) {
/* update the sliding average of sensors readings */
imu_vs_buf_head++;
sw/airborne/imu_v3.h
+4 -2
View File
@@ -4,6 +4,8 @@
#include "std.h"
#include "6dof.h"
#include "imu_v3_hw.h"
/* calibrated sensor readings */
extern float imu_accel[AXIS_NB]; /* accelerometers in arbitrary unit */
extern float imu_gyro[AXIS_NB]; /* gyros in rad/s */
@@ -17,8 +19,8 @@ extern uint16_t imu_accel_raw[AXIS_NB];
extern uint16_t imu_gyro_raw[AXIS_NB];
extern int16_t imu_mag_raw[AXIS_NB];
extern void imu_init(void);
extern void imu_detect_vehicle_still(void);
extern void imu_v3_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];
extern float imu_vs_gyro_unbiased[AXIS_NB]; /* unbiased gyros in rad/s */
sw/airborne/led.h
+1
View File
@@ -78,6 +78,7 @@ static inline void led_init ( void ) {
}
#else /* LED */
static inline void led_init ( void ) {}
#define LED_ON(i) {}
#define LED_OFF(i) {}
#define LED_TOGGLE(i) {}
sw/airborne/main_booz.c
+28 -10
View File
@@ -1,3 +1,5 @@
#include "main_booz.h"
#include "std.h"
#include "init_hw.h"
#include "interrupt_hw.h"
@@ -22,26 +24,32 @@
#include "datalink.h"
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void );
int16_t trim_p = 0;
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 ) {
main_init();
booz_main_init();
while(1) {
if (sys_time_periodic())
main_periodic_task();
main_event_task();
booz_main_periodic_task();
booz_main_event_task();
}
return 0;
}
#endif
STATIC_INLINE void booz_main_init( void ) {
static inline void main_init( void ) {
hw_init();
led_init();
sys_time_init();
@@ -53,23 +61,31 @@ static inline void main_init( void ) {
ppm_init();
radio_control_init();
#ifndef SITL
spi_init();
link_imu_init();
#endif
booz_estimator_init();
booz_control_init();
booz_autopilot_init();
//FIXME
#ifndef SITL
uart1_init_tx();
#endif
int_enable();
DOWNLINK_SEND_BOOT(&cpu_time_sec);
}
static inline void main_periodic_task( void ) {
STATIC_INLINE void booz_main_periodic_task( void ) {
// FIXME
#ifndef SITL
link_imu_periodic_task();
#endif
booz_autopilot_periodic_task();
@@ -80,7 +96,6 @@ static inline void main_periodic_task( void ) {
if (_50hz > 5) _50hz = 0;
switch (_50hz) {
case 0:
LED_TOGGLE(1);
break;
case 1:
radio_control_periodic_task();
@@ -99,11 +114,14 @@ static inline void main_periodic_task( void ) {
}
static inline void main_event_task( void ) {
STATIC_INLINE void booz_main_event_task( void ) {
// FIXME
#ifndef SITL
DlEventCheckAndHandle();
LinkImuEventCheckAndHandle();
#endif
if (ppm_valid) {
ppm_valid = FALSE;
sw/airborne/main_booz.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef MAIN_BOOZ_H
#define MAIN_BOOZ_H
#ifdef SITL
#define STATIC_INLINE
#else
#define STATIC_INLINE static inline
#endif
STATIC_INLINE void booz_main_init( void );
STATIC_INLINE void booz_main_periodic_task( void );
STATIC_INLINE void booz_main_event_task( void );
#endif /* MAIN_BOOZ_H */
sw/airborne/main_mtt.c
+3 -52
View File
@@ -8,7 +8,6 @@
#include "print.h"
#include "adc.h"
#include "max1167.h"
#include "imu_v3.h"
#include "multitilt.h"
@@ -32,14 +31,6 @@ static inline void main_event_task( void);
static inline void ahrs_task( void );
static void main_init_spi1( void );
static void SPI1_ISR(void) __attribute__((naked));
#define SPI_SLAVE_NONE 0
#define SPI_SLAVE_MAX 1
uint8_t spi_cur_slave = SPI_SLAVE_NONE;
int main( void ) {
main_init();
@@ -56,9 +47,8 @@ static inline void main_init( void ) {
sys_time_init();
uart1_init_tx();
adc_init();
imu_init();
main_init_spi1();
max1167_init();
imu_v3_init();
multitilt_init();
link_imu_init();
int_enable();
@@ -105,7 +95,7 @@ static inline void ahrs_task( void ) {
switch (mtt_status) {
case MT_STATUS_UNINIT : {
imu_detect_vehicle_still();
imu_v3_detect_vehicle_still();
#ifdef SEND_GYRO_RAW_AVG
DOWNLINK_SEND_IMU_GYRO_RAW_AVG(&imu_vs_gyro_raw_avg[AXIS_X], &imu_vs_gyro_raw_avg[AXIS_Y], &imu_vs_gyro_raw_avg[AXIS_Z], \
&imu_vs_gyro_raw_var[AXIS_X], &imu_vs_gyro_raw_var[AXIS_Y], &imu_vs_gyro_raw_var[AXIS_Z]);
@@ -144,42 +134,3 @@ static inline void ahrs_task( void ) {
}
}
/* SSPCR0 settings */
#define SSP_DDS 0x07 << 0 /* data size : 8 bits */
#define SSP_FRF 0x00 << 4 /* frame format : SPI */
#define SSP_CPOL 0x00 << 6 /* clock polarity : data captured on first clock transition */
#define SSP_CPHA 0x00 << 7 /* clock phase : SCK idles low */
#define SSP_SCR 0x0F << 8 /* serial clock rate : divide by 16 */
/* SSPCR1 settings */
#define SSP_LBM 0x00 << 0 /* loopback mode : disabled */
#define SSP_SSE 0x00 << 1 /* SSP enable : disabled */
#define SSP_MS 0x00 << 2 /* master slave mode : master */
#define SSP_SOD 0x00 << 3 /* slave output disable : don't care when master */
static void main_init_spi1( 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;
SSPCPSR = 0x20;
/* initialize interrupt vector */
VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ
VICIntEnable = VIC_BIT(VIC_SPI1); // SPI1 interrupt enabled
VICVectCntl7 = VIC_ENABLE | VIC_SPI1;
VICVectAddr7 = (uint32_t)SPI1_ISR; // address of the ISR
}
void SPI1_ISR(void) {
ISR_ENTRY();
Max1167OnSpiInt();
VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */
ISR_EXIT();
}
sw/airborne/sim/actuators_buss_twi_blmc_hw.c
+6
View File
@@ -0,0 +1,6 @@
#include "actuators.h"
volatile bool_t buss_twi_blmc_status;
volatile uint8_t buss_twi_blmc_nb_err;
void actuators_init ( void ) {}
sw/airborne/sim/actuators_buss_twi_blmc_hw.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef ACTUATORS_BUSS_TWI_BLMC_HW_H
#define ACTUATORS_BUSS_TWI_BLMC_HW_H
#include "std.h"
extern volatile bool_t buss_twi_blmc_status;
extern volatile uint8_t buss_twi_blmc_nb_err;
#define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s)
#define ChopServo(x,a,b) Chop(x, a, b)
#define Actuator(i) actuators[i]
#define ActuatorsCommit() {}
#endif /* ACTUATORS_BUSS_TWI_BLMC_HW_H */
sw/airborne/sim/adc_hw.c
+3
View File
@@ -0,0 +1,3 @@
#include "adc.h"
void adc_buf_channel(uint8_t adc_channel, struct adc_buf* s, uint8_t av_nb_sample) {}
sw/airborne/sim/i2c_hw.c
+4
View File
@@ -0,0 +1,4 @@
#include "i2c.h"
void i2c_hw_init ( void ) {}
sw/airborne/sim/i2c_hw.h
+9
View File
@@ -0,0 +1,9 @@
#ifndef I2C_HW_H
#define I2C_HW_H
#define I2cSendStart() {}
extern void i2c_hw_init(void);
#endif /* I2C_HW_H */
sw/airborne/sim/imu_v3_hw.c
+5
View File
@@ -0,0 +1,5 @@
#include "imu_v3.h"
void imu_v3_hw_init(void) {
}
sw/airborne/sim/imu_v3_hw.h
+7
View File
@@ -0,0 +1,7 @@
#ifndef IMU_V3_HW_H
#define IMU_V3_HW_H
extern void imu_v3_hw_init(void);
#endif /* IMU_V3_HW_H */
sw/simulator/Makefile
+1 -1
View File
@@ -147,7 +147,7 @@ BOOZ_AB_SRCS += $(AB)/booz_autopilot.c
BOOZ_AB_SRCS += $(AB)/commands.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
BOOZ_AB_SRCS += $(AB)/imu_v3.c $(AB_ARCH)/imu_v3_hw.c $(AB_ARCH)/adc_hw.c
booz_sim: $(BOOZ_SIM_SRCS) $(BOOZ_AB_SRCS)
sw/simulator/main_booz_sim.c
+23 -12
View File
@@ -26,7 +26,8 @@ double disp_time;
double foo_commands[] = {0., 0., 0., 0.};
static gboolean timeout_callback(gpointer data);
static gboolean booz_sim_periodic(gpointer data);
static inline void booz_sim_display(void);
#ifdef SIM_UART
static void sim_uart_init(void);
@@ -43,21 +44,13 @@ static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *arg
volatile bool_t ppm_valid;
#define RPM_OF_RAD_S(a) ((a)*60./M_PI)
static gboolean timeout_callback(gpointer data) {
static gboolean booz_sim_periodic(gpointer data) {
booz_flight_model_run(DT, foo_commands);
booz_sensors_model_run();
sim_time += DT;
if (sim_time >= disp_time) {
disp_time+= DT_DISPLAY;
booz_flightgear_send();
IvySendMsg("148 BOOZ_RPMS %f %f %f %f",
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_F]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_B]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_L]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_R]) );
}
booz_sim_display();
booz_estimator_p = bfm.state->ve[BFMS_P];
booz_estimator_q = bfm.state->ve[BFMS_Q];
@@ -118,7 +111,7 @@ int main ( int argc, char** argv) {
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
g_timeout_add(TIMEOUT_PERIOD, timeout_callback, NULL);
g_timeout_add(TIMEOUT_PERIOD, booz_sim_periodic, NULL);
g_main_loop_run(ml);
@@ -130,6 +123,24 @@ int main ( int argc, char** argv) {
// Helpers
////////////////////
static inline void booz_sim_display(void) {
if (sim_time >= disp_time) {
disp_time+= DT_DISPLAY;
booz_flightgear_send();
IvySendMsg("148 BOOZ_RPMS %f %f %f %f",
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_F]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_B]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_L]),
RPM_OF_RAD_S(bfm.state->ve[BFMS_OM_R]) );
}
}
#if defined DOWNLINK_TRANSPORT && DOWNLINK_TRANSPORT == IvyTransport
static void ivy_transport_init(void) {