Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +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-25 00:32:22 +00:00
parent 33ad88dea2
commit 45b5d516bf
4 changed files with 190 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/booz.xml
+3 -1
View File
@@ -6,5 +6,7 @@
<dl_setting var="booz_control_rate_pq_dgain" min="0" step="0.1" max="10" module="booz_control" shortname="pq_d"/>
<dl_setting var="booz_control_rate_r_pgain" min="-500" step="10" max="-10" module="booz_control" shortname="r_p"/>
<dl_setting var="booz_control_rate_r_dgain" min="0" step="0.1" max="10" module="booz_control" shortname="r_d"/>
</dl_settings>
<dl_setting var="booz_control_attitude_phi_theta_pgain" min="-20000" step="500" max="-500" module="booz_control" shortname="att_p"/>
<dl_setting var="booz_control_attitude_phi_theta_dgain" min="-20000" step="500" max="-500" module="booz_control" shortname="att_d"/>
</dl_settings>
</settings>
sw/airborne/main_coax.c
-93
View File
@@ -1,93 +0,0 @@
#include "std.h"
#include "init_hw.h"
#include "sys_time.h"
#include "led.h"
#include "interrupt_hw.h"
#include "uart.h"
#include "main_ap.h"
#include "airframe.h"
#include "messages.h"
#include "downlink.h"
#include "spi.h"
#include "baro_MS5534A.h"
#include "estimator.h"
float ground_alt;
#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
static bool_t dl_msg_available;
static inline void main_dl_parse_msg( void );
void init_ap( void ) {
/** init done in main_fbw */
spi_init();
baro_MS5534A_init();
int_enable();
baro_MS5534A_init();
}
void periodic_task_ap( void ) {
// LED_TOGGLE(1);
// DOWNLINK_SEND_TAKEOFF(&cpu_time_sec);
static uint8_t _20Hz = 0;
_20Hz++;
if (_20Hz>=3) _20Hz=0;
if (!_20Hz) {
baro_MS5534A_send();
}
}
void event_task_ap( void ) {
if (PprzBuffer()) {
ReadPprzBuffer();
if (pprz_msg_received) {
pprz_parse_payload();
pprz_msg_received = FALSE;
}
}
if (dl_msg_available) {
main_dl_parse_msg();
dl_msg_available = FALSE;
LED_TOGGLE(1);
}
if (spi_message_received) {
/* Got a message on SPI. */
spi_message_received = FALSE;
baro_MS5534A_event_task();
if (baro_MS5534A_available) {
baro_MS5534A_available = FALSE;
baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084;
if (alt_baro_enabled) {
EstimatorSetAlt(baro_MS5534A_z);
}
}
}
}
#define MSG_SIZE 128
uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned));
#include "settings.h"
#define IdOfMsg(x) (x[1])
static inline void main_dl_parse_msg(void) {
uint8_t msg_id = IdOfMsg(dl_buffer);
if (msg_id == DL_SETTING) {
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(&i, &var);
}
}
sw/simulator/Makefile
+13 -2
View File
@@ -105,10 +105,21 @@ CFLAGS += -DRADIO_CONTROL
BOOZ_AB_SRCS += ../airborne/radio_control.c
CFLAGS += -DACTUATORS=\"servos_nil.h\"
BOOZ_AB_SRCS += ../airborne/actuators.c
CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport
CFLAGS += -DDOWNLINK
BOOZ_AB_SRCS += ../airborne/booz_telemetry.c ../airborne/downlink.c
#CFLAGS += -DDOWNLINK_TRANSPORT=PprzTransport -DPPRZ_UART=SimUart -DDOWNLINK_DEVICE=SimUart -DSIM_UART
#BOOZ_AB_SRCS += ../airborne/pprz_transport.c ../airborne/sim/sim_uart.c
CFLAGS += -DDOWNLINK_TRANSPORT=IvyTransport
BOOZ_AB_SRCS += ../airborne/sim/ivy_transport.c
LDFLAGS += -lglibivy
BOOZ_AB_SRCS += ../airborne/booz_telemetry.c ../airborne/downlink.c ../airborne/sim/ivy_transport.c
#../airborne/link_imu.c
#CFLAGS += -DDATALINK=PPRZ
#BOOZ_AB_SRCS += ../airborne/datalink.c
BOOZ_AB_SRCS += ../airborne/booz_estimator.c
BOOZ_AB_SRCS += ../airborne/booz_control.c
BOOZ_AB_SRCS += ../airborne/booz_autopilot.c
sw/simulator/main_booz_sim.c
+174 -16
View File
@@ -23,8 +23,17 @@ double disp_time;
double foo_commands[] = {0., 0., 0., 0.};
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
static void sim_uart_init(void);
#endif
#if defined DOWNLINK_TRANSPORT && DOWNLINK_TRANSPORT == IvyTransport
static void ivy_transport_init(void);
static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *argv[]);
#endif
static void airborne_init(void);
static void airborne_periodic_task(void);
@@ -34,9 +43,6 @@ static void airborne_event_task(void);
static gboolean timeout_callback(gpointer data) {
booz_flight_model_run(DT, foo_commands);
sim_time += DT;
@@ -62,6 +68,18 @@ static gboolean timeout_callback(gpointer data) {
int main ( int argc, char** argv) {
sim_time = 0.;
@@ -71,6 +89,15 @@ int main ( int argc, char** argv) {
booz_flightgear_init(fg_host, fg_port);
#ifdef SIM_UART
sim_uart_init();
#endif
#if defined DOWNLINK_TRANSPORT && DOWNLINK_TRANSPORT == IvyTransport
ivy_transport_init();
#endif
joystick_init();
airborne_init();
@@ -79,8 +106,6 @@ int main ( int argc, char** argv) {
g_timeout_add(TIMEOUT_PERIOD, timeout_callback, NULL);
IvyInit ("BoozSim", "BoozSim READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
g_main_loop_run(ml);
@@ -88,11 +113,108 @@ int main ( int argc, char** argv) {
}
/////////////////////
// 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/js1";
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
static void ivy_transport_init(void) {
IvyInit ("BoozSim", "BoozSim READY", NULL, NULL, NULL, NULL);
IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
IvyStart("127.255.255.255");
}
#include "std.h"
#include "settings.h"
#include "booz_telemetry.h"
static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *argv[]){
uint8_t index = atoi(argv[2]);
float value = atof(argv[3]);
DlSetting(index, value);
DOWNLINK_SEND_DL_VALUE(&index, &value);
printf("setting %d %f\n", index, value);
}
#endif
#ifdef SIM_UART
#define AC_ID 148
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include "sim_uart.h"
static void sim_uart_init(void) {
char link_pipe_name[128];
sprintf(link_pipe_name, "/tmp/pprz_link_%d", AC_ID);
struct stat st;
if (stat(link_pipe_name, &st)) {
if (mkfifo(link_pipe_name, 0644) == -1) {
perror("make pipe");
exit (10);
}
}
if ( !(pipe_stream = fopen(link_pipe_name, "w")) ) {
perror("open pipe");
exit (10);
}
}
#endif /* SIM_UART */
//////////////////
// 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"
@@ -110,10 +232,8 @@ uint16_t ppm_pulses[PPM_NB_PULSES];
static void airborne_init(void) {
ppm_pulses[RADIO_THROTTLE] = 1223 + 0.615 * (2050-1223);
ppm_pulses[RADIO_ROLL] = 1500;
ppm_pulses[RADIO_PITCH] = 1498;
ppm_pulses[RADIO_YAW] = 1493;
ppm_init();
radio_control_init();
booz_estimator_init();
booz_control_init();
@@ -124,12 +244,6 @@ static void airborne_init(void) {
static void airborne_periodic_task(void) {
int foo = sim_time / 10;
if (!(foo%2))
ppm_pulses[RADIO_PITCH] = 1600;
else
ppm_pulses[RADIO_PITCH] = 1400;
booz_autopilot_periodic_task();
SetActuatorsFromCommands(commands);
@@ -160,6 +274,9 @@ static void airborne_periodic_task(void) {
static void airborne_event_task(void) {
// DlEventCheckAndHandle();
// if (ppm_valid) {
// ppm_valid = FALSE;
radio_control_event_task();
@@ -171,3 +288,44 @@ static void airborne_event_task(void) {
//}
}
#define JS_THROTTLE 6
#define JS_ROLL 0
#define JS_PITCH 1
#define JS_YAW 5
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;
}
}
return TRUE;
}