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 26 Packages Projects Releases Wiki Activity

sim fixed

Browse Source
This commit is contained in:
Pascal Brisset
2006-03-21 22:30:12 +00:00
parent 15c9119150
commit 5bb28ee90e
11 changed files with 45 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/twinstar1.xml
+2 -4
View File
@@ -104,13 +104,11 @@
<makefile>
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
</makefile>
</airframe>
conf/airframes/twinstar2.xml
+8 -5
View File
@@ -9,16 +9,17 @@
<servo name="AILERON_RIGHT" no="6" min="1000" neutral="1385" max="1833"/>
<servo name="ELEVATOR" no="7" min="1816" neutral="1586" max="1220"/>
<servo name="RUDDER" no="3" min="1270" neutral="1546" max="1850"/>
<servo name="CAM_ROLL" no="4" min="1000" neutral="1500" max="2000"/>
<servo name="CAM_PITCH" no="5" min="1000" neutral="1500" max="2000"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="MODE" failsafe_value="0"/>
<axis name="GAIN1" failsafe_value="0"/>
<axis name="CALIB" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="CAM_ROLL" failsafe_value="0"/>
<axis name="CAM_PITCH" failsafe_value="0"/>
</commands>
<command_laws>
@@ -97,9 +98,11 @@
<makefile>
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
</makefile>
</airframe>
sw/airborne/autopilot.h
+1 -1
View File
@@ -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);
}
sw/airborne/cam.c
+6 -6
View File
@@ -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);
sw/airborne/downlink.h
+3 -2
View File
@@ -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) {}
sw/airborne/main_ap.c
+10 -10
View File
@@ -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;
sw/airborne/nav.c
+4 -4
View File
@@ -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); \
sw/airborne/radio_control.c
+2 -2
View File
@@ -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;
sw/ground_segment/cockpit/Makefile
+1 -1
View File
@@ -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
sw/simulator/Makefile
+6 -1
View File
@@ -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
sw/simulator/sim_ap.c
+2 -2
View File
@@ -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);