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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-10-26 00:35:38 +00:00
parent ca48c62b97
commit 065119d023
6 changed files with 174 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/Makefile
+36 -11
View File
@@ -93,37 +93,62 @@ ifneq ($(MAKECMDGOALS),clean)
-include .depend -include .depend
endif endif
AB=../airborne
AB_ARCH=$(AB)/sim
CC = gcc CC = gcc
CFLAGS=-g -O2 -Wall `pkg-config glib-2.0 --cflags` -I /usr/include/meschach -I../airborne -I../include -I../../var/BOOZ -I../../var/include -I../../conf/autopilot -I../airborne/sim CFLAGS= -g -Wall `pkg-config glib-2.0 --cflags` -I /usr/include/meschach -I$(AB) -I../include -I../../var/BOOZ -I../../var/include -I../../conf/autopilot -I$(AB_ARCH)
LDFLAGS=`pkg-config glib-2.0 --libs` -lm -lmeschach LDFLAGS=`pkg-config glib-2.0 --libs` -lm -lmeschach
BOOZ_SIM_SRCS = main_booz_sim.c booz_flight_model.c booz_flight_model_utils.c booz_flightgear.c
BOOZ_SIM_SRCS = main_booz_sim.c \
booz_flight_model.c \
booz_flight_model_utils.c \
booz_sensors_model.c \
booz_flightgear.c \
booz_joystick.c \
# booz_fake_main_imu.c \
CFLAGS += -DSITL CFLAGS += -DSITL
CFLAGS += -DCONTROLLER CFLAGS += -DCONTROLLER
CFLAGS += -DCONFIG=\"conf_booz.h\" CFLAGS += -DCONFIG=\"conf_booz.h\"
BOOZ_AB_SRCS += $(AB)/main_booz.c
BOOZ_AB_SRCS += $(AB)/sys_time.c
CFLAGS += -DRADIO_CONTROL CFLAGS += -DRADIO_CONTROL
BOOZ_AB_SRCS += ../airborne/radio_control.c BOOZ_AB_SRCS += $(AB)/radio_control.c \
CFLAGS += -DACTUATORS=\"servos_nil.h\" $(AB_ARCH)/ppm_hw.c
BOOZ_AB_SRCS += ../airborne/actuators.c
CFLAGS += -DACTUATORS=\"actuators_buss_twi_blmc_hw.h\"
BOOZ_AB_SRCS += $(AB)/actuators.c \
$(AB_ARCH)/actuators_buss_twi_blmc_hw.c \
$(AB)/i2c.c $(AB_ARCH)/i2c_hw.c
CFLAGS += -DDOWNLINK CFLAGS += -DDOWNLINK
BOOZ_AB_SRCS += ../airborne/booz_telemetry.c ../airborne/downlink.c BOOZ_AB_SRCS += $(AB)/booz_telemetry.c \
$(AB)/downlink.c
#CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DPPRZ_UART=SimUart -DDOWNLINK_DEVICE=SimUart -DSIM_UART #CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DPPRZ_UART=SimUart -DDOWNLINK_DEVICE=SimUart -DSIM_UART
#BOOZ_AB_SRCS += ../airborne/pprz_transport.c ../airborne/sim/sim_uart.c #BOOZ_AB_SRCS += ../airborne/pprz_transport.c ../airborne/sim/sim_uart.c
CFLAGS += -DDOWNLINK_TRANSPORT=IvyTransport CFLAGS += -DDOWNLINK_TRANSPORT=IvyTransport
BOOZ_AB_SRCS += ../airborne/sim/ivy_transport.c BOOZ_AB_SRCS += $(AB_ARCH)/ivy_transport.c
LDFLAGS += -lglibivy LDFLAGS += -lglibivy
#../airborne/link_imu.c #../airborne/link_imu.c
#CFLAGS += -DDATALINK=PPRZ #CFLAGS += -DDATALINK=PPRZ
#BOOZ_AB_SRCS += ../airborne/datalink.c #BOOZ_AB_SRCS += ../airborne/datalink.c
BOOZ_AB_SRCS += ../airborne/booz_estimator.c BOOZ_AB_SRCS += $(AB)/booz_estimator.c
BOOZ_AB_SRCS += ../airborne/booz_control.c BOOZ_AB_SRCS += $(AB)/booz_control.c
BOOZ_AB_SRCS += ../airborne/booz_autopilot.c BOOZ_AB_SRCS += $(AB)/booz_autopilot.c
BOOZ_AB_SRCS += ../airborne/commands.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_sim: $(BOOZ_SIM_SRCS) $(BOOZ_AB_SRCS) booz_sim: $(BOOZ_SIM_SRCS) $(BOOZ_AB_SRCS)
$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
sw/simulator/booz_joystick.c
+73
View File
@@ -0,0 +1,73 @@
#include "booz_joystick.h"
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <glib.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <linux/joystick.h>
double booz_joystick_value[JS_NB_AXIS];
const double booz_joystick_neutral[JS_NB_AXIS] = { 112., 113., 112., 112., 112., 112., 30.};
const double booz_joystick_max[JS_NB_AXIS] = { 1., 224., 224., 224., 224., 224., 223.};
const double booz_joystick_min[JS_NB_AXIS] = { 224., 1., 1., 1., 1., 1., 30.};
static gboolean on_data_received(GIOChannel *source, GIOCondition condition, gpointer data);
void booz_joystick_init(const char* device) {
int i;
for (i=0; i<JS_NB_AXIS; i++)
booz_joystick_value[i] = 0.;
int fd = open(device, O_RDONLY | O_NONBLOCK);
if (fd == -1) {
printf("opening joystick serial device %s : %s\n", device, strerror(errno));
return;
}
GIOChannel* channel = g_io_channel_unix_new(fd);
g_io_channel_set_encoding(channel, NULL, NULL);
g_io_add_watch (channel, G_IO_IN , on_data_received, NULL);
}
static gboolean on_data_received(GIOChannel *source, GIOCondition condition, gpointer data) {
struct js_event js;
gsize len;
GError *err = NULL;
g_io_channel_read_chars(source, (void*)(&js), sizeof(struct js_event), &len, &err);
if (js.type == JS_EVENT_AXIS) {
if (js.number < JS_NB_AXIS)
booz_joystick_value[js.number] = (js.value - booz_joystick_neutral[js.number]) /
(booz_joystick_max[js.number] - booz_joystick_min[js.number]);
}
return TRUE;
}
#if 0
switch (js.number) {
case JS_THROTTLE:
ppm_pulses[RADIO_THROTTLE] = 1223 + (js.value - 30) * (float)(2050-1223) / (float)(223 - 30);
break;
case JS_PITCH:
ppm_pulses[RADIO_PITCH] = 1498 + (js.value - 113) * (float)(2050-950) / (float)(224 - 1);
break;
case JS_ROLL:
ppm_pulses[RADIO_ROLL] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(1 - 224);
break;
case JS_YAW:
ppm_pulses[RADIO_YAW] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1);
break;
case JS_MODE:
ppm_pulses[RADIO_MODE] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1);
rc_values_contains_avg_channels = TRUE;
break;
}
#endif
sw/simulator/booz_joystick.h
+19
View File
@@ -0,0 +1,19 @@
#ifndef BOOZ_JOYSTICK_H
#define BOOZ_JOYSTICK_H
#define JS_ROLL 0
#define JS_PITCH 1
#define JS_MODE 2
#define JS_YAW 5
#define JS_THROTTLE 6
#define JS_NB_AXIS 7
extern void booz_joystick_init(const char* device);
extern double booz_joystick_value[JS_NB_AXIS];
#endif /* BOOZ_JOYSTICK_H */
sw/simulator/booz_sensors_model.c
+9
View File
@@ -0,0 +1,9 @@
#include "booz_sensors_model.h"
#include "booz_flight_model.h"
void booz_sensors_model_run(void) {
}
sw/simulator/booz_sensors_model.h
+8
View File
@@ -0,0 +1,8 @@
#ifndef BOOZ_SENSORS_MODEL_H
#define BOOZ_SENSORS_MODEL_H
#include "6dof.h"
extern void booz_sensors_model_run(void);
#endif /* BOOZ_SENSORS_MODEL_H */
sw/simulator/main_booz_sim.c
+29 -208
View File
@@ -3,16 +3,18 @@
#include <Ivy/ivy.h> #include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h> #include <Ivy/ivyglibloop.h>
#include <math.h>
#include "booz_flight_model.h" #include "booz_flight_model.h"
#include "booz_flightgear.h" #include "booz_sensors_model.h"
#include "booz_flightgear.h"
#include "booz_joystick.h"
#include "main_booz.h"
//char* fg_host = "127.0.0.1"; //char* fg_host = "127.0.0.1";
char* fg_host = "10.31.4.107"; char* fg_host = "10.31.4.107";
unsigned int fg_port = 5501; unsigned int fg_port = 5501;
char* joystick_dev = "/dev/input/js0";
/* 250Hz <-> 4ms */ /* 250Hz <-> 4ms */
#define TIMEOUT_PERIOD 4 #define TIMEOUT_PERIOD 4
@@ -25,9 +27,7 @@ double disp_time;
double foo_commands[] = {0., 0., 0., 0.}; double foo_commands[] = {0., 0., 0., 0.};
static gboolean timeout_callback(gpointer data); static gboolean timeout_callback(gpointer data);
static gboolean on_js_data_received(GIOChannel *source, GIOCondition condition, gpointer data);
static void joystick_init(void);
#ifdef SIM_UART #ifdef SIM_UART
static void sim_uart_init(void); static void sim_uart_init(void);
#endif #endif
@@ -36,18 +36,17 @@ static void ivy_transport_init(void);
static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *argv[]); static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *argv[]);
#endif #endif
static void airborne_init(void);
static void airborne_periodic_task(void);
static void airborne_event_task(void);
#include "booz_estimator.h" #include "booz_estimator.h"
#include "radio_control.h" #include "radio_control.h"
#include "actuators.h"
volatile bool_t ppm_valid; volatile bool_t ppm_valid;
#define RPM_OF_RAD_S(a) ((a)*60./M_PI) #define RPM_OF_RAD_S(a) ((a)*60./M_PI)
static gboolean timeout_callback(gpointer data) { static gboolean timeout_callback(gpointer data) {
booz_flight_model_run(DT, foo_commands); booz_flight_model_run(DT, foo_commands);
booz_sensors_model_run();
sim_time += DT; sim_time += DT;
if (sim_time >= disp_time) { if (sim_time >= disp_time) {
@@ -69,29 +68,32 @@ static gboolean timeout_callback(gpointer data) {
booz_estimator_psi = bfm.state->ve[BFMS_PSI]; booz_estimator_psi = bfm.state->ve[BFMS_PSI];
/* post a radio control event */ /* 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);
ppm_valid = TRUE; ppm_valid = TRUE;
airborne_event_task(); booz_main_event_task();
/* run periodic tak */
booz_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
airborne_periodic_task(); 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);
return TRUE; return TRUE;
} }
int main ( int argc, char** argv) { int main ( int argc, char** argv) {
sim_time = 0.; sim_time = 0.;
@@ -109,15 +111,14 @@ int main ( int argc, char** argv) {
ivy_transport_init(); ivy_transport_init();
#endif #endif
joystick_init(); booz_joystick_init(joystick_dev);
airborne_init();
booz_main_init();
GMainLoop *ml = g_main_loop_new(NULL, FALSE); GMainLoop *ml = g_main_loop_new(NULL, FALSE);
g_timeout_add(TIMEOUT_PERIOD, timeout_callback, NULL); g_timeout_add(TIMEOUT_PERIOD, timeout_callback, NULL);
g_main_loop_run(ml); g_main_loop_run(ml);
@@ -129,38 +130,6 @@ int main ( int argc, char** argv) {
// Helpers // Helpers
//////////////////// ////////////////////
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <linux/joystick.h>
static void joystick_init(void) {
const char* device = "/dev/input/js0";
int fd = open(device, O_RDONLY | O_NONBLOCK);
if (fd == -1) {
printf("opening joystick serial device %s : %s\n", device, strerror(errno));
return;
}
// if( ioctl( fd, JSIOCSCORR, corr ) != 0 )
// perror( "Unable to turn off deadband correction" );
GIOChannel* channel = g_io_channel_unix_new(fd);
g_io_channel_set_encoding(channel, NULL, NULL);
g_io_add_watch (channel, G_IO_IN , on_js_data_received, NULL);
}
#if defined DOWNLINK_TRANSPORT && DOWNLINK_TRANSPORT == IvyTransport #if defined DOWNLINK_TRANSPORT && DOWNLINK_TRANSPORT == IvyTransport
static void ivy_transport_init(void) { static void ivy_transport_init(void) {
@@ -215,152 +184,4 @@ static void sim_uart_init(void) {
//////////////////
// AIRBORNE CODE
//////////////////
#include "ppm.h"
#include "radio_control.h"
#include "actuators.h"
#include "commands.h"
#include "booz_telemetry.h"
//#include "datalink.h"
#include "booz_estimator.h"
#include "booz_control.h"
// FIXME grrrrr!
uint32_t link_imu_nb_err;
uint8_t link_imu_status;
int16_t trim_p = 0;
int16_t trim_q = 0;
int16_t trim_r = 0;
uint16_t ppm_pulses[PPM_NB_PULSES];
static void airborne_init(void) {
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.62 * (2050-1223);
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950);
ppm_pulses[RADIO_ROLL] = 1500 + 0. * (2050-950);
ppm_pulses[RADIO_YAW] = 1500 + 0. * (2050-950);
ppm_pulses[RADIO_MODE] = 1500 + 0.25 * (2050-950);
ppm_init();
radio_control_init();
booz_estimator_init();
booz_control_init();
}
static void airborne_periodic_task(void) {
#if 0
int foo = sim_time/10;
if (!(foo%2)) {
ppm_pulses[RADIO_YAW] = 1500 + 0.1 * (2050-950);
ppm_pulses[RADIO_PITCH] = 1498 + 0. * (2050-950);
}
else {
ppm_pulses[RADIO_YAW] = 1500 - 0. * (2050-950);
ppm_pulses[RADIO_PITCH] = 1498 + 0.1 * (2050-950);
}
#endif
booz_autopilot_periodic_task();
SetActuatorsFromCommands(commands);
static uint8_t _50hz = 0;
_50hz++;
if (_50hz > 5) _50hz = 0;
switch (_50hz) {
case 1:
radio_control_periodic_task();
break;
case 2:
booz_telemetry_periodic_task();
break;
case 3:
break;
case 4:
break;
}
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);
}
static void airborne_event_task(void) {
// DlEventCheckAndHandle();
if (ppm_valid) {
ppm_valid = FALSE;
radio_control_event_task();
booz_autopilot_mode = BOOZ_AP_MODE_ATTITUDE;
if (rc_values_contains_avg_channels) {
booz_autopilot_mode = BOOZ_AP_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
}
booz_autopilot_event_task();
}
}
#define JS_THROTTLE 6
#define JS_ROLL 0
#define JS_PITCH 1
#define JS_YAW 5
#define JS_MODE 2
static gboolean on_js_data_received(GIOChannel *source, GIOCondition condition, gpointer data) {
struct js_event js;
gsize len;
GError *err = NULL;
g_io_channel_read_chars(source, (void*)(&js), sizeof(struct js_event), &len, &err);
if (js.type == JS_EVENT_AXIS) {
//printf("%d %d\n", js.number, js.value);
switch (js.number) {
case JS_THROTTLE:
ppm_pulses[RADIO_THROTTLE] = 1223 + (js.value - 30) * (float)(2050-1223) / (float)(223 - 30);
break;
case JS_PITCH:
ppm_pulses[RADIO_PITCH] = 1498 + (js.value - 113) * (float)(2050-950) / (float)(224 - 1);
break;
case JS_ROLL:
ppm_pulses[RADIO_ROLL] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(1 - 224);
break;
case JS_YAW:
ppm_pulses[RADIO_YAW] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1);
break;
case JS_MODE:
ppm_pulses[RADIO_MODE] = 1500 + (js.value - 112) * (float)(2050-950) / (float)(224 - 1);
rc_values_contains_avg_channels = TRUE;
break;
}
}
return TRUE;
}