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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-27 17:18:35 +00:00
parent 404d4b7ecc
commit 5332612725
14 changed files with 119 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/booz.xml
+20 -56
View File
@@ -50,7 +50,7 @@ ctl.TARGET = ctl
ctl.TARGETDIR = ctl
ctl.CFLAGS += -DCONTROLLER -DCONFIG=\"conf_booz.h\"
ctl.srcs = main_booz.c
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
@@ -90,73 +90,37 @@ ctl.srcs += booz_telemetry.c
#
# multitilt
# FILTER CPU
#
mtt.ARCHDIR = $(ARCHI)
mtt.ARCH = arm7tdmi
mtt.TARGET = mtt
mtt.TARGETDIR = mtt
flt.ARCHDIR = $(ARCHI)
flt.ARCH = arm7tdmi
flt.TARGET = flt
flt.TARGETDIR = flt
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
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
mtt.CFLAGS += -DLED
flt.CFLAGS += -DLED
mtt.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
mtt.srcs += $(SRC_ARCH)/uart_hw.c
flt.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B57600
flt.srcs += $(SRC_ARCH)/uart_hw.c
mtt.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
mtt.srcs += downlink.c pprz_transport.c booz_filter_telemetry.c
flt.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
flt.srcs += downlink.c pprz_transport.c booz_filter_telemetry.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
flt.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
flt.srcs += $(SRC_ARCH)/adc_hw.c
mtt.srcs += $(SRC_ARCH)/max1167.c
flt.srcs += $(SRC_ARCH)/max1167.c
mtt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
flt.srcs += imu_v3.c $(SRC_ARCH)/imu_v3_hw.c
mtt.srcs += multitilt.c
flt.srcs += multitilt.c
mtt.CFLAGS += -DIMU
mtt.srcs += link_imu.c
flt.CFLAGS += -DIMU
flt.srcs += link_imu.c
#
# AHRS CPU
#
imu.ARCHDIR = $(ARCHI)
imu.ARCH = arm7tdmi
imu.TARGET = imu
imu.TARGETDIR = imu
imu.CFLAGS += -DIMU -DCONFIG=\"pprz_imu.h\"
imu.srcs = main_imu.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
imu.CFLAGS += -DLED
imu.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B9600
imu.srcs += $(SRC_ARCH)/uart_hw.c
imu.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1
imu.srcs += downlink.c pprz_transport.c
imu.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4
imu.srcs += $(SRC_ARCH)/adc_hw.c
imu.srcs += $(SRC_ARCH)/max1167.c
imu.srcs += $(SRC_ARCH)/micromag.c
imu.srcs += imu_v3.c
//imu.srcs += ahrs_new.c
imu.srcs += ahrs_quat_fast_ekf.c
imu.CFLAGS += -DEKF_UPDATE_CONTINUOUS
//imu.srcs += multitilt.c
imu.srcs += link_imu.c
</makefile>
sw/airborne/arm7/actuators_buss_twi_blmc_hw.h
-4
View File
@@ -10,10 +10,6 @@
#define ActuatorsCommit() { \
if ( buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_IDLE) { \
buss_twi_blmc_idx = 0; \
/* buss_twi_blmc_motor_power[0] = 0; */ \
/* buss_twi_blmc_motor_power[1] = 0; */ \
/* buss_twi_blmc_motor_power[2] = 0; */ \
/*buss_twi_blmc_motor_power[3] = 0; */ \
buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY; \
ActuatorsBussTwiBlmcSend(); \
} \
sw/airborne/main_booz.c → sw/airborne/booz_controller_main.c
+7 -7
View File
@@ -1,4 +1,4 @@
#include "main_booz.h"
#include "booz_controller_main.h"
#include "std.h"
#include "init_hw.h"
@@ -38,17 +38,17 @@ uint8_t link_imu_status;
#ifndef SITL
int main( void ) {
booz_main_init();
booz_controller_main_init();
while(1) {
if (sys_time_periodic())
booz_main_periodic_task();
booz_main_event_task();
booz_controller_main_periodic_task();
booz_controller_main_event_task();
}
return 0;
}
#endif
STATIC_INLINE void booz_main_init( void ) {
STATIC_INLINE void booz_controller_main_init( void ) {
hw_init();
led_init();
@@ -80,7 +80,7 @@ STATIC_INLINE void booz_main_init( void ) {
DOWNLINK_SEND_BOOT(&cpu_time_sec);
}
STATIC_INLINE void booz_main_periodic_task( void ) {
STATIC_INLINE void booz_controller_main_periodic_task( void ) {
// FIXME
#ifndef SITL
@@ -114,7 +114,7 @@ STATIC_INLINE void booz_main_periodic_task( void ) {
}
STATIC_INLINE void booz_main_event_task( void ) {
STATIC_INLINE void booz_controller_main_event_task( void ) {
// FIXME
#ifndef SITL
sw/airborne/booz_controller_main.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef BOOZ_CONTROLLER_MAIN_H
#define BOOZ_CONTROLLER_MAIN_H
#ifdef SITL
#define STATIC_INLINE
#else
#define STATIC_INLINE static inline
#endif
STATIC_INLINE void booz_controller_main_init( void );
STATIC_INLINE void booz_controller_main_periodic_task( void );
STATIC_INLINE void booz_controller_main_event_task( void );
#endif /* BOOZ_CONTROLLER_MAIN_H */
sw/airborne/main_mtt.c → sw/airborne/booz_filter_main.c
+20 -13
View File
@@ -1,8 +1,9 @@
#include "booz_filter_main.h"
#include "std.h"
#include "sys_time.h"
#include "init_hw.h"
#include "interrupt_hw.h"
#include "sys_time.h"
#include "adc.h"
#include "imu_v3.h"
@@ -15,44 +16,49 @@
#include "link_imu.h"
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void);
static inline void on_imu_event( void );
#ifndef SITL
int main( void ) {
main_init();
booz_filter_main_init();
while (1) {
if (sys_time_periodic())
main_periodic_task();
main_event_task();
booz_filter_main_periodic_task();
booz_filter_main_event_task();
}
return 0;
}
#endif
STATIC_INLINE void booz_filter_main_init( void ) {
static inline void main_init( void ) {
hw_init();
sys_time_init();
//FIXME
#ifndef SITL
uart1_init_tx();
adc_init();
#endif
imu_v3_init();
multitilt_init();
//FIXME
#ifndef SITL
link_imu_init();
#endif
int_enable();
}
// static uint32_t t0, t1, diff;
static inline void main_event_task( void ) {
STATIC_INLINE void booz_filter_main_event_task( void ) {
ImuEventCheckAndHandle(on_imu_event);
}
static inline void main_periodic_task( void ) {
STATIC_INLINE void booz_filter_main_periodic_task( void ) {
ImuPeriodic();
static uint8_t _50hz = 0;
_50hz++;
@@ -82,10 +88,11 @@ static inline void on_imu_event( void ) {
multitilt_update(imu_accel);
// t1 = T0TC;
// diff = t1 - t0;
// DOWNLINK_SEND_TIME(&dif);
// DOWNLINK_SEND_TIME(&diff);
break;
}
#ifndef SITL
link_imu_send();
#endif
}
sw/airborne/booz_filter_main.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef BOOZ_FILTER_MAIN_H
#define BOOZ_FILTER_MAIN_H
#ifdef SITL
#define STATIC_INLINE
#else
#define STATIC_INLINE static inline
#endif
STATIC_INLINE void booz_filter_main_init( void );
STATIC_INLINE void booz_filter_main_periodic_task( void );
STATIC_INLINE void booz_filter_main_event_task( void );
#endif /* BOOZ_FILTER_MAIN_H */
sw/airborne/main_booz.h
-14
View File
@@ -1,14 +0,0 @@
#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_grz.c
-172
View File
@@ -1,172 +0,0 @@
#include "pprz_transport.h"
#include "datalink.h"
#include "uart.h"
#include "interrupt_hw.h"
#include "periodic.h"
#include "ap_downlink.h"
#include "flight_plan.h"
#include "control_grz.h"
#include "adc.h"
#include "radio_control.h"
#include "autopilot.h"
#include "gps.h"
#include "estimator.h"
#include "settings.h"
uint8_t pprz_mode = PPRZ_MODE_MANUAL;
uint8_t vertical_mode = 0;
uint8_t lateral_mode = 0;
uint8_t horizontal_mode = 0;
uint8_t rc_settings_mode = 0;
uint8_t mcu1_status = 0;
uint8_t ir_estim_mode = 0;
/** From nav.c */
int32_t nav_utm_east0;
int32_t nav_utm_north0;
struct adc_buf rangemeter_adc_buf;
uint16_t rangemeter; /* cm */
#define NB_RANGES 30
#define FLYING_MIN_CM 30
#define STILL_MAX_CM 18
#define SWITCH_PEDIOD_MIN 30 /* 60Hz */
bool_t sum_err_reset;
/** 60Hz */
void periodic_task_ap( void ) {
PORTD |= 1 << 5;
static uint8_t _10Hz = 0;
_10Hz++;
if (_10Hz>=6) _10Hz = 0;
if (!_10Hz) {
PeriodicSendAp();
if (sum_err_reset) {
sum_err_reset = FALSE;
ctl_grz_reset();
}
}
static uint16_t last_rangemeters[NB_RANGES];
static uint8_t last_idx;
static uint16_t sum_rangemeters = 0;
// ClearBit(ADCSRA, ADIE);
rangemeter = RangeCmOfAdc(rangemeter_adc_buf.sum/rangemeter_adc_buf.av_nb_sample);
// SetBit(ADCSRA, ADIE);
last_idx++;
if (last_idx >= NB_RANGES) last_idx = 0;
float last_avg = (float)sum_rangemeters / NB_RANGES;
sum_rangemeters += rangemeter - last_rangemeters[last_idx];
last_rangemeters[last_idx] = rangemeter;
float avg = (float)sum_rangemeters / NB_RANGES;
ctl_grz_z = avg / 100.; /* cm -> m */
ctl_grz_z_dot = ((avg - last_avg) * 61. / 100.);
/** Flying ? */
static uint8_t flying_cpt;
if (!flying) {
if (rangemeter > FLYING_MIN_CM) {
flying_cpt++;
if (flying_cpt > SWITCH_PEDIOD_MIN) {
flying = TRUE;
flying_cpt = 0;
}
} else
flying_cpt = 0;
} else if (rangemeter < STILL_MAX_CM) {
flying_cpt++;
if (flying_cpt > SWITCH_PEDIOD_MIN) {
flying = FALSE;
flying_cpt = 0;
}
} else
flying_cpt = 0;
pprz_mode = PPRZ_MODE_OF_PULSE(rc_values[RADIO_MODE], FIXME);
if (pprz_mode == PPRZ_MODE_AUTO2) {
ctl_grz_set_setpoints_auto2();
} else if (pprz_mode == PPRZ_MODE_AUTO1) {
ctl_grz_set_setpoints_auto1();
}
if (pprz_mode >= PPRZ_MODE_AUTO2) {
ctl_grz_z_dot_run();
/* if (!_10Hz) */
/* ctl_grz_horiz_speed_run(); */
}
if (pprz_mode >= PPRZ_MODE_AUTO1) {
// ctl_grz_alt_run();
// ctl_grz_z_dot_run();
ctl_grz_attitude_run();
}
PORTD &= ~ (1 << 5);
}
void event_task_ap( void ) {
/***** Datalink *******/
if (PprzBuffer()) {
ReadPprzBuffer();
if (pprz_msg_received) {
pprz_parse_payload();
pprz_msg_received = FALSE;
if (dl_msg_available) {
dl_parse_msg();
dl_msg_available = FALSE;
}
}
}
/* parse and use GPS messages */
if (GpsBuffer()) {
ReadGpsBuffer();
}
if (gps_msg_received) {
parse_gps_msg();
gps_msg_received = FALSE;
if (gps_pos_available) {
use_gps_pos();
gps_pos_available = FALSE;
}
}
}
#include <avr/io.h>
void init_ap( void ) {
OSCCAL=0xB1;
uart1_init_rx();
uart0_init_rx();
/** adc_init done in fbw */
adc_buf_channel(ADC_CHANNEL_RANGEMETER, &rangemeter_adc_buf, DEFAULT_AV_NB_SAMPLE);
gps_init ();
int_enable();
/** Debug */
SetBit(DDRD, 5);
}
sw/airborne/sim/actuators_buss_twi_blmc_hw.c
+2
View File
@@ -3,4 +3,6 @@
volatile bool_t buss_twi_blmc_status;
volatile uint8_t buss_twi_blmc_nb_err;
uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB];
void actuators_init ( void ) {}
sw/airborne/sim/actuators_buss_twi_blmc_hw.h
+5 -2
View File
@@ -7,8 +7,11 @@ 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 ChopServo(x,a,b) ((x)>(b)?(b):(x))
#define Actuator(i) buss_twi_blmc_motor_power[i]
#define ActuatorsCommit() {}
#define BUSS_TWI_BLMC_NB 4
extern uint8_t buss_twi_blmc_motor_power[BUSS_TWI_BLMC_NB];
#endif /* ACTUATORS_BUSS_TWI_BLMC_HW_H */
sw/airborne/sim/imu_v3_hw.c
+2 -2
View File
@@ -1,5 +1,5 @@
#include "imu_v3.h"
void imu_v3_hw_init(void) {
bool_t imu_data_available;
}
void imu_v3_hw_init(void) {}
sw/airborne/sim/imu_v3_hw.h
+17
View File
@@ -1,7 +1,24 @@
#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
+7 -2
View File
@@ -107,13 +107,12 @@ BOOZ_SIM_SRCS = main_booz_sim.c \
booz_sensors_model.c \
booz_flightgear.c \
booz_joystick.c \
# booz_fake_main_imu.c \
CFLAGS += -DSITL
CFLAGS += -DCONTROLLER
CFLAGS += -DCONFIG=\"conf_booz.h\"
BOOZ_AB_SRCS += $(AB)/main_booz.c
BOOZ_AB_SRCS += $(AB)/booz_controller_main.c
BOOZ_AB_SRCS += $(AB)/sys_time.c
@@ -146,9 +145,15 @@ BOOZ_AB_SRCS += $(AB)/booz_control.c
BOOZ_AB_SRCS += $(AB)/booz_autopilot.c
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)/booz_filter_telemetry.c
BOOZ_AB_SRCS += $(AB)/multitilt.c
booz_sim: $(BOOZ_SIM_SRCS) $(BOOZ_AB_SRCS)
$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
sw/simulator/main_booz_sim.c
+11 -5
View File
@@ -8,11 +8,14 @@
#include "booz_flightgear.h"
#include "booz_joystick.h"
#include "main_booz.h"
#include "booz_controller_main.h"
#include "booz_filter_main.h"
//char* fg_host = "127.0.0.1";
char* fg_host = "10.31.4.107";
//char* fg_host = "10.31.4.107";
char* fg_host = "192.168.1.191";
unsigned int fg_port = 5501;
char* joystick_dev = "/dev/input/js0";
@@ -67,9 +70,10 @@ static gboolean booz_sim_periodic(gpointer data) {
ppm_pulses[RADIO_YAW] = 1500 + booz_joystick_value[JS_YAW] * (2050-950);
ppm_pulses[RADIO_MODE] = 1500 + booz_joystick_value[JS_MODE] * (2050-950);
ppm_valid = TRUE;
booz_main_event_task();
booz_controller_main_event_task();
/* run periodic tak */
booz_main_periodic_task();
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);
@@ -106,7 +110,9 @@ int main ( int argc, char** argv) {
booz_joystick_init(joystick_dev);
booz_main_init();
booz_controller_main_init();
booz_filter_main_init();
GMainLoop *ml = g_main_loop_new(NULL, FALSE);