From 5bb28ee90e7a65104e2f44089579746a1f71d072 Mon Sep 17 00:00:00 2001 From: Pascal Brisset Date: Tue, 21 Mar 2006 22:30:12 +0000 Subject: [PATCH] sim fixed --- conf/airframes/twinstar1.xml | 6 ++---- conf/airframes/twinstar2.xml | 13 ++++++++----- sw/airborne/autopilot.h | 2 +- sw/airborne/cam.c | 12 ++++++------ sw/airborne/downlink.h | 5 +++-- sw/airborne/main_ap.c | 20 ++++++++++---------- sw/airborne/nav.c | 8 ++++---- sw/airborne/radio_control.c | 4 ++-- sw/ground_segment/cockpit/Makefile | 2 +- sw/simulator/Makefile | 7 ++++++- sw/simulator/sim_ap.c | 4 ++-- 11 files changed, 45 insertions(+), 38 deletions(-) diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml index da1f44c46b..0c0434f691 100644 --- a/conf/airframes/twinstar1.xml +++ b/conf/airframes/twinstar1.xml @@ -104,13 +104,11 @@ include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile -ap.CFLAGS += -DUBX +ap.CFLAGS += -DGPS -DUBX -DINFRARED ap.CFLAGS += -Werror # ap.CFLAGS += -DSIMUL -ap.LOCAL_CFLAGS += -DDATALINK +ap.CFLAGS += -DDATALINK ap.EXTRA_SRCS += traffic_info.c datalink.c -ap.LOCAL_CFLAGS += -DWAVECARD -ap.EXTRA_SRCS += wavecard.c diff --git a/conf/airframes/twinstar2.xml b/conf/airframes/twinstar2.xml index 0f231a5897..ed9cff37b3 100644 --- a/conf/airframes/twinstar2.xml +++ b/conf/airframes/twinstar2.xml @@ -9,16 +9,17 @@ + + - - - - + + + @@ -97,9 +98,11 @@ include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile -ap.CFLAGS += -DUBX +ap.CFLAGS += -DGPS -DUBX -DINFRARED ap.CFLAGS += -DDATALINK ap.EXTRA_SRCS += traffic_info.c datalink.c ap.CFLAGS += -DMOBILE_CAM +ap.CFLAGS += -DWAVECARD +ap.EXTRA_SRCS += wavecard.c diff --git a/sw/airborne/autopilot.h b/sw/airborne/autopilot.h index c1bcd69f73..b992ebd8a0 100644 --- a/sw/airborne/autopilot.h +++ b/sw/airborne/autopilot.h @@ -113,7 +113,7 @@ void telecommand_task(void); #include "radio_control.h" static inline void autopilot_process_radio_control ( void ) { - pprz_mode = PPRZ_MODE_OF_PULSE(rc_values[COMMAND_MODE], 0); + pprz_mode = PPRZ_MODE_OF_PULSE(rc_values[RADIO_MODE], 0); } diff --git a/sw/airborne/cam.c b/sw/airborne/cam.c index c04f45adc1..310670fc16 100644 --- a/sw/airborne/cam.c +++ b/sw/airborne/cam.c @@ -60,18 +60,18 @@ float target_x, target_y, target_alt; void cam_manual( void ) { if (pprz_mode == PPRZ_MODE_AUTO2) { static pprz_t cam_roll, cam_pitch; - int16_t yaw = from_fbw.from_fbw.channels[COMMAND_YAW]; + int16_t yaw = from_fbw.from_fbw.channels[RADIO_YAW]; if (yaw > MIN_PPRZ_CAM || yaw < -MIN_PPRZ_CAM) { cam_roll += FLOAT_OF_PPRZ(yaw, 0, 300.); cam_roll = TRIM_PPRZ(cam_roll); } - int16_t pitch = from_fbw.from_fbw.channels[COMMAND_PITCH]; + int16_t pitch = from_fbw.from_fbw.channels[RADIO_PITCH]; if (pitch > MIN_PPRZ_CAM || pitch < -MIN_PPRZ_CAM) { cam_pitch += FLOAT_OF_PPRZ(pitch, 0, 300.); cam_pitch = TRIM_PPRZ(cam_pitch); } - from_ap.from_ap.channels[COMMAND_GAIN1] = cam_roll; - from_ap.from_ap.channels[COMMAND_CALIB] = cam_pitch; + from_ap.from_ap.channels[COMMAND_CAM_ROLL] = cam_roll; + from_ap.from_ap.channels[COMMAND_CAM_PITCH] = cam_pitch; } } @@ -102,13 +102,13 @@ void cam_target( void ) { #define MAX_DIST_TARGET 500. void cam_manual_target( void ) { - int16_t yaw = from_fbw.from_fbw.channels[COMMAND_YAW]; + int16_t yaw = from_fbw.from_fbw.channels[RADIO_YAW]; if (yaw > MIN_PPRZ_CAM || yaw < -MIN_PPRZ_CAM) { target_x += FLOAT_OF_PPRZ(yaw, 0, -20.); target_x = Min(target_x, MAX_DIST_TARGET + estimator_x); target_x = Max(target_x, -MAX_DIST_TARGET + estimator_x); } - int16_t pitch = from_fbw.from_fbw.channels[COMMAND_PITCH]; + int16_t pitch = from_fbw.from_fbw.channels[RADIO_PITCH]; if (pitch > MIN_PPRZ_CAM || pitch < -MIN_PPRZ_CAM) { target_y += FLOAT_OF_PPRZ(pitch, 0, -20.); target_y = Min(target_y, MAX_DIST_TARGET + estimator_y); diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h index 835c6553af..5e5ff6cde5 100644 --- a/sw/airborne/downlink.h +++ b/sw/airborne/downlink.h @@ -25,15 +25,16 @@ #ifndef DOWNLINK_H #define DOWNLINK_H -#include "modem.h" + #ifdef SITL #include "sitl_messages.h" #else +#include "modem.h" #include "messages.h" #endif -#ifdef DOWNLINK +#if DOWNLINK #define Downlink(x) x #else #define Downlink(x) {} diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c index 60b812e481..9e954b5fcf 100644 --- a/sw/airborne/main_ap.c +++ b/sw/airborne/main_ap.c @@ -107,7 +107,7 @@ static inline uint8_t pprz_mode_update( void ) { if ((pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER) || CheckEvent(rc_event_1)) { - ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(from_fbw.from_fbw.channels[COMMAND_MODE], from_fbw.from_fbw.status)); + ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(from_fbw.from_fbw.channels[RADIO_MODE], from_fbw.from_fbw.status)); } else return FALSE; } @@ -175,16 +175,16 @@ static inline uint8_t inflight_calib_mode_update ( void ) { static inline void events_update( void ) { static uint16_t event1_cpt = 0; - EventPos(event1_cpt, COMMAND_GAIN1, rc_event_1); + EventPos(event1_cpt, RADIO_GAIN1, rc_event_1); static uint16_t event2_cpt = 0; - EventNeg(event2_cpt, COMMAND_GAIN1, rc_event_2); + EventNeg(event2_cpt, RADIO_GAIN1, rc_event_2); } /** \brief Send back uncontrolled channels (actually only rudder) */ static inline void copy_from_to_fbw ( void ) { - from_ap.from_ap.channels[COMMAND_YAW] = from_fbw.from_fbw.channels[COMMAND_YAW]; + from_ap.from_ap.channels[COMMAND_YAW] = from_fbw.from_fbw.channels[RADIO_YAW]; } @@ -252,13 +252,13 @@ inline void telecommand_task( void ) { */ if (pprz_mode == PPRZ_MODE_AUTO1) { /** In Auto1 mode, roll is bounded between [-AUTO1_MAX_ROLL;AUTO1_MAX_ROLL] */ - desired_roll = FLOAT_OF_PPRZ(from_fbw.from_fbw.channels[COMMAND_ROLL], 0., -AUTO1_MAX_ROLL); + desired_roll = FLOAT_OF_PPRZ(from_fbw.from_fbw.channels[RADIO_ROLL], 0., -AUTO1_MAX_ROLL); /** In Auto1 mode, pitch is bounded between [-AUTO1_MAX_PITCH;AUTO1_MAX_PITCH] */ - desired_pitch = FLOAT_OF_PPRZ(from_fbw.from_fbw.channels[COMMAND_PITCH], 0., AUTO1_MAX_PITCH); + desired_pitch = FLOAT_OF_PPRZ(from_fbw.from_fbw.channels[RADIO_PITCH], 0., AUTO1_MAX_PITCH); } if (pprz_mode == PPRZ_MODE_MANUAL || pprz_mode == PPRZ_MODE_AUTO1) { - desired_gaz = from_fbw.from_fbw.channels[COMMAND_THROTTLE]; + desired_gaz = from_fbw.from_fbw.channels[RADIO_THROTTLE]; } /** else asynchronously set by climb_pid_run(); */ @@ -269,9 +269,9 @@ inline void telecommand_task( void ) { if (!estimator_flight_time) { #ifdef INFRARED - ground_calibrate(STICK_PUSHED(from_fbw.from_fbw.channels[COMMAND_ROLL])); + ground_calibrate(STICK_PUSHED(from_fbw.from_fbw.channels[RADIO_ROLL])); #endif - if (pprz_mode == PPRZ_MODE_AUTO2 && from_fbw.from_fbw.channels[COMMAND_THROTTLE] > GAZ_THRESHOLD_TAKEOFF) { + if (pprz_mode == PPRZ_MODE_AUTO2 && from_fbw.from_fbw.channels[RADIO_THROTTLE] > GAZ_THRESHOLD_TAKEOFF) { launch = TRUE; } } @@ -455,7 +455,7 @@ inline void periodic_task( void ) { from_ap.from_ap.channels[COMMAND_ROLL] = desired_aileron; from_ap.from_ap.channels[COMMAND_PITCH] = desired_elevator; -#ifdef MCU_SPI_LINK +#if defined MCU_SPI_LINK && !defined SITL link_fbw_send(); #endif break; diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c index b4663297a3..afc1de2881 100644 --- a/sw/airborne/nav.c +++ b/sw/airborne/nav.c @@ -65,7 +65,7 @@ int16_t circle_x, circle_y, circle_radius; int16_t segment_x_1, segment_y_1, segment_x_2, segment_y_2; uint8_t horizontal_mode; -#define RcRoll(travel) (from_fbw.channels[COMMAND_ROLL]* (float)travel /(float)MAX_PPRZ) +#define RcRoll(travel) (from_fbw.from_fbw.channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ) #define RcEvent1() CheckEvent(rc_event_1) #define RcEvent2() CheckEvent(rc_event_2) @@ -132,20 +132,20 @@ static float qdr; #define Goto3D(radius) { \ if (pprz_mode == PPRZ_MODE_AUTO2) { \ - int16_t yaw = from_fbw.from_fbw.channels[COMMAND_YAW]; \ + int16_t yaw = from_fbw.from_fbw.channels[RADIO_YAW]; \ if (yaw > MIN_DX || yaw < -MIN_DX) { \ carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \ carrot_x = Min(carrot_x, MAX_DIST_CARROT); \ carrot_x = Max(carrot_x, -MAX_DIST_CARROT); \ } \ - int16_t pitch = from_fbw.from_fbw.channels[COMMAND_PITCH]; \ + int16_t pitch = from_fbw.from_fbw.channels[RADIO_PITCH]; \ if (pitch > MIN_DX || pitch < -MIN_DX) { \ carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \ carrot_y = Min(carrot_y, MAX_DIST_CARROT); \ carrot_y = Max(carrot_y, -MAX_DIST_CARROT); \ } \ vertical_mode = VERTICAL_MODE_AUTO_ALT; \ - int16_t roll = from_fbw.from_fbw.channels[COMMAND_ROLL]; \ + int16_t roll = from_fbw.from_fbw.channels[RADIO_ROLL]; \ if (roll > MIN_DX || roll < -MIN_DX) { \ desired_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \ desired_altitude = Max(desired_altitude, MIN_HEIGHT_CARROT+GROUND_ALT); \ diff --git a/sw/airborne/radio_control.c b/sw/airborne/radio_control.c index 169b9c008f..63ceab10bb 100644 --- a/sw/airborne/radio_control.c +++ b/sw/airborne/radio_control.c @@ -1,8 +1,8 @@ #include "radio_control.h" -pprz_t rc_values[COMMANDS_NB]; +pprz_t rc_values[PPM_NB_PULSES]; uint8_t rc_status; -pprz_t avg_rc_values[COMMANDS_NB]; +pprz_t avg_rc_values[PPM_NB_PULSES]; uint8_t rc_values_contains_avg_channels = FALSE; uint8_t time_sime_last_ppm; diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile index 717ffde887..6bf8c6c14a 100644 --- a/sw/ground_segment/cockpit/Makefile +++ b/sw/ground_segment/cockpit/Makefile @@ -14,7 +14,7 @@ map2d : map2d.ml ../../lib/ocaml/lib-pprz.cma ../../lib/ocaml/xlib-pprz.cma $(OCAMLC) -custom $(INCLUDES) $(LIBS) threads.cma gtkThread.cmo gtkInit.cmo $< -o $@ map2d.opt : map2d.cmx - $(OCAMLOPT) $(INCLUDES) str.cmxa unix.cmxa xml-light.cmxa $(LIBS:.cma=.cmxa) threads.cmxa gtkInit.cmx $< -o $@ + $(OCAMLOPT) $(INCLUDES) str.cmxa unix.cmxa xml-light.cmxa $(LIBS:.cma=.cmxa) threads.cmxa gtkThread.cmx gtkInit.cmx $< -o $@ .SUFFIXES: .ml .mli .cmo .cmi .cmx diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index 525b3a4091..91d632c21f 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -27,6 +27,10 @@ include ../../conf/Makefile.local ACDIR= $(PAPARAZZI_HOME)/var/$(AIRCRAFT) OBJDIR= $(ACDIR)/sim +ifeq ($(MAKECMDGOALS),sim_sitl) +include $(ACDIR)/Makefile.ac +endif + SIMHML = stdlib.ml data.ml flightModel.ml gps.ml hitl.ml sim.ml SIMHCMO=$(SIMHML:%.ml=%.cmo) SIMSML = stdlib.ml data.ml flightModel.ml gps.ml sitl.ml sim.ml @@ -39,7 +43,8 @@ SIMSA=sims.cma OCAMLC = ocamlc OCAMLOPT=ocamlopt -p INCLUDES= -I +lablgtk2 -I ../lib/ocaml -OCAMLCC = gcc -Werror -O2 -I /usr/include/glib-2.0 -I /usr/lib/glib-2.0/include -DUBX -DMOBILE_CAM -DAP -DDOWNLINK -DSITL -DINFRARED -DGPS -I $(OBJDIR) -I $(ACDIR) +# OCAMLCC = gcc -Werror -O2 -I /usr/include/glib-2.0 -I /usr/lib/glib-2.0/include -DSITL -DUBX -DMOBILE_CAM -DAP -DDOWNLINK -DINFRARED -DGPS -I $(OBJDIR) -I $(ACDIR) +OCAMLCC = gcc -Werror -O2 -I /usr/include/glib-2.0 -I /usr/lib/glib-2.0/include -DSITL $(ap.CFLAGS) -I $(OBJDIR) -I $(ACDIR) AIRBORNE = ../airborne VARINCLUDE=$(PAPARAZZI_HOME)/var/include diff --git a/sw/simulator/sim_ap.c b/sw/simulator/sim_ap.c index b88c3540bd..a57d050b56 100644 --- a/sw/simulator/sim_ap.c +++ b/sw/simulator/sim_ap.c @@ -57,9 +57,9 @@ value set_really_lost(value on) { value sim_rc_task(value _unit) { NormalizePpm(); /** -> rc_values */ - /*** printf("update: %d : %f (%d)\n", Int_val(c), Double_val(v), rc_values[COMMAND_GAIN1]); ***/ + /*** printf("sim_rc_task ppm=%d rc_val=%d\n", ppm_pulses[RADIO_MODE], rc_values[RADIO_MODE]); ***/ int i; - for(i = 0; i < COMMANDS_NB; i++) + for(i = 0; i < RADIO_CTL_NB; i++) from_fbw.from_fbw.channels[i] = rc_values[i]; from_fbw.from_fbw.status = (radio_status << STATUS_RADIO_OK) | (radio_really_lost << RADIO_REALLY_LOST) | (1 << AVERAGED_CHANNELS_SENT);