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

Merge branch 'master' of github.com:paparazzi/paparazzi

Browse Source
This commit is contained in:
Christophe De Wagter
2011-04-20 14:54:46 +02:00
parent 596ed48c7f ca9322cf2c
commit 45071b9735
44 changed files with 3326 additions and 368 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+7 -2
View File
@@ -78,6 +78,10 @@
/sw/ground_segment/tmtc/server
/sw/ground_segment/tmtc/diadec
# /sw/ground_segment/joystick
/sw/ground_segment/joystick/input2ivy
/sw/ground_segment/joystick/test_stick
# /sw/lib/ocaml/
/sw/lib/ocaml/gtk_papget_editor.ml
/sw/lib/ocaml/gtk_papget_text_editor.ml
@@ -109,8 +113,9 @@
/sw/tools/fp_parser.ml
/sw/tools/wiki_gen/wiki_gen
# /sw/ground_segment/joystick
/sw/ground_segment/joystick/test_stick
# /sw/ground_segment/misc
/sw/ground_segment/misc/davis2ivy
# /sw/airborne/arch/lpc21/test/bootloader
/sw/airborne/arch/lpc21/test/bootloader/bl.dmp
Makefile
+6 -2
View File
@@ -43,6 +43,7 @@ AIRBORNE=sw/airborne
COCKPIT=sw/ground_segment/cockpit
TMTC=sw/ground_segment/tmtc
MULTIMON=sw/ground_segment/multimon
MISC=sw/ground_segment/misc
LOGALIZER=sw/logalizer
SIMULATOR=sw/simulator
MAKE=make PAPARAZZI_SRC=$(PAPARAZZI_SRC) PAPARAZZI_HOME=$(PAPARAZZI_HOME)
@@ -69,7 +70,7 @@ OCAMLRUN=$(shell which ocamlrun)
all: commands static conf
static : lib center tools cockpit multimon tmtc logalizer lpc21iap sim_static static_h usb_lib
static : lib center tools cockpit multimon tmtc misc logalizer lpc21iap sim_static static_h usb_lib
conf: conf/conf.xml conf/control_panel.xml
@@ -98,13 +99,16 @@ cockpit: lib
tmtc: lib cockpit
cd $(TMTC); $(MAKE) all
misc:
cd $(MISC); $(MAKE) all
multimon:
cd $(MULTIMON); $(MAKE)
static_h: $(MESSAGES_H) $(MESSAGES2_H) $(UBX_PROTOCOL_H) $(XSENS_PROTOCOL_H) $(DL_PROTOCOL_H) $(DL_PROTOCOL2_H)
usb_lib:
@[ -d sw/airborne/arch/lpc21/lpcusb ] && ((test -x $(ARMGCC) && (cd sw/airborne/arch/lpc21/lpcusb; $(MAKE))) || echo "Not building usb_lib: ARMGCC=$(ARMGCC) not found") || echo "Not building usb_lib: sw/airborne/arch/lpc21/lpcusb directory missing"
@[ -d sw/airborne/arch/lpc21/lpcusb ] && ((test -x "$(ARMGCC)" && (cd sw/airborne/arch/lpc21/lpcusb; $(MAKE))) || echo "Not building usb_lib: ARMGCC=$(ARMGCC) not found") || echo "Not building usb_lib: sw/airborne/arch/lpc21/lpcusb directory missing"
$(MESSAGES_H) : $(MESSAGES_XML) $(CONF_XML) tools
$(Q)test -d $(STATICINCLUDE) || mkdir -p $(STATICINCLUDE)
conf/airframes/LAAS/mmlaas_N1.xml
+11 -6
View File
@@ -24,8 +24,8 @@
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="attitude" type="infrared">
<configure name="ADC_IR1" value="ADC_0"/>
<configure name="ADC_IR2" value="ADC_1"/>
<configure name="ADC_IR1" value="ADC_1"/>
<configure name="ADC_IR2" value="ADC_0"/>
<configure name="ADC_IR_TOP" value="ADC_2"/>
</subsystem>
<subsystem name="gps" type="ublox_lea4p"/>
@@ -40,9 +40,14 @@
<modules>
<load name="formation_flight.xml"/>
<load name="tcas.xml"/>
<!-- <load name="tcas.xml"/> -->
<!--load name="potential.xml"/-->
</modules>
<load name="cartography.xml"/>
</modules>
<!-- commands section -->
<servos>
@@ -95,7 +100,7 @@
<define name="PITCH_NEUTRAL_DEFAULT" value="8.6" unit="deg"/>
<define name="HORIZ_SENSOR_TILTED" value="1"/>
<define name="IR2_SIGN" value="-1"/>
<define name="IR2_SIGN" value="1"/>
<define name="TOP_SIGN" value="1"/>
<!--linear name="RollOfIrs" arity="2" coeff1="0.7" coeff2="0.7"/>
@@ -107,7 +112,7 @@
<define name="ADC_TOP_NEUTRAL" value="515"/>
<define name="LATERAL_CORRECTION" value="1."/>
<define name="LONGITUDINAL_CORRECTION" value="1."/>
<define name="LONGITUDINAL_CORRECTION" value="-1."/>
<define name="VERTICAL_CORRECTION" value="1.5"/>
</section>
conf/airframes/LAAS/mmlaas_N1_carto_cam.xml
+271
View File
@@ -0,0 +1,271 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="MiniMag Laas N1 Tiny 0.99">
<!-- commands section -->
<servos>
<servo name="THROTTLE" no="0" min="1200" neutral="1200" max="2000"/>
<!-- Aileron gauche (blanc) oppose de l'aileron droit (jaune) -->
<!-- max = valeur aileron vers le bas-->
<servo name="AILERON_LEFT" no="3" min="1900" neutral="1450" max="1100"/>
<servo name="AILERON_RIGHT" no="1" min="1900" neutral="1520" max="1100"/>
<servo name="RUDDER" no="4" min="1850" neutral="1500" max="1150"/>
<servo name="ELEVATOR" no="5" min="2000" neutral="1568" max="1150"/>
</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="HATCH" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="YAW" value="@YAW"/>
</rc_commands>
<section name="MIXER">
<define name="AILERON_DIFF" value="0.66"/>
<define name="COMBI_SWITCH" value="1.0"/>
</section>
<command_laws>
<set servo="THROTTLE" value="@THROTTLE"/>
<set servo="ELEVATOR" value="@PITCH"/>
<set servo="RUDDER" value="@YAW + @ROLL*COMBI_SWITCH"/>
<let var="roll" value="@ROLL"/>
<set servo="AILERON_LEFT" value="($roll > 0 ? 1 : AILERON_DIFF) * $roll"/>
<set servo="AILERON_RIGHT" value="($roll > 0 ? AILERON_DIFF : 1) * $roll"/>
</command_laws>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.80"/>
<define name="MAX_PITCH" value="0.80"/>
</section>
<section name="adc" prefix="ADC_CHANNEL_">
<define name="IR1" value="ADC_0"/>
<define name="IR2" value="ADC_1"/>
<define name="IR_TOP" value="ADC_2"/>
<define name="IR_NB_SAMPLES" value="16"/>
</section>
<section name="INFRARED" prefix="IR_">
<define name="ROLL_NEUTRAL_DEFAULT" value="-2.6" unit="deg"/>
<!-- <define name="PITCH_NEUTRAL_DEFAULT" value="8.6" unit="deg"/>-->
<define name="PITCH_NEUTRAL_DEFAULT" value="10.6" unit="deg"/>
<linear name="RollOfIrs" arity="2" coeff1="0.7" coeff2="0.7"/>
<linear name="PitchOfIrs" arity="2" coeff1="-0.7" coeff2="0.7"/>
<linear name="TopOfIr" arity="1" coeff1="1.5"/>
<define name="ADC_IR1_NEUTRAL" value="507"/>
<define name="ADC_IR2_NEUTRAL" value="509"/>
<define name="ADC_TOP_NEUTRAL" value="515"/>
<define name="CORRECTION_UP" value="1."/>
<define name="CORRECTION_DOWN" value="1."/>
<define name="CORRECTION_LEFT" value="1."/>
<define name="CORRECTION_RIGHT" value="1."/>
<define name="LATERAL_CORRECTION" value="1."/>
<define name="LONGITUDINAL_CORRECTION" value="1."/>
<define name="VERTICAL_CORRECTION" value="1."/>
</section>
<section name="BAT">
<define name="MILLIAMP_PER_PERCENT" value="0.86"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
</section>
<section name="MISC">
<define name="NOMINAL_AIRSPEED" value="11." unit="m/s"/>
<define name="CARROT" value="3." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_RATE" value="60" unit="Hz"/>
<!-- <define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\rATBD6\rATWR\r\""/> -->
<define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/>
<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="TRIGGER_DELAY" value="1."/>
<define name="DEFAULT_CIRCLE_RADIUS" value="60."/>
</section>
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
<!-- outer loop proportional gain -->
<define name="ALTITUDE_PGAIN" value="-0.15"/>
<!-- outer loop saturation -->
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.55"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.30"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.90"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
<define name="AUTO_THROTTLE_DASH_TRIM" value="-2000"/>
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="-0.05"/>
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
<!-- auto pitch inner loop -->
<define name="AUTO_PITCH_PGAIN" value="-0.065"/>
<define name="AUTO_PITCH_IGAIN" value="0.15"/>
<define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
<define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
<define name="THROTTLE_SLEW" value="0.05"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="-1."/>
<define name="ROLL_MAX_SETPOINT" value="0.6" unit="radians"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
<define name="ROLL_PGAIN" value="10000."/>
<define name="AILERON_OF_THROTTLE" value="0.0"/>
<define name="PITCH_PGAIN" value="-12000."/>
<define name="PITCH_DGAIN" value="1.5"/>
<define name="ELEVATOR_OF_ROLL" value="1250"/>
</section>
<section name="NAV">
<define name="NAV_PITCH" value="0."/>
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
<define name="NAV_GROUND_SPEED_PGAIN" value="-0.015"/>
<define name="NAV_FOLLOW_PGAIN" value="-0.05"/>
</section>
<section name="FORMATION" prefix="FORM_">
<define name="CARROT" value="3." unit="s"/> <!-- carrot distance for followers -->
<define name="POS_PGAIN" value="0.02"/> <!-- coef on position error -->
<define name="SPEED_PGAIN" value="0.4"/> <!-- coef on speed error -->
<define name="COURSE_PGAIN" value="0.8"/> <!-- coef on course error (override course pgain for followers) -->
<define name="ALTITUDE_PGAIN" value="0.1"/> <!-- coef on altitude error -->
<define name="PROX" value="60." unit="m"/> <!-- proximity distance -->
<define name="MODE" value="0"/> <!-- mode 0 = global, 1 = local -->
</section>
<section name="TCAS" prefix="TCAS_">
<define name="TAU_TA" value="10." unit="s"/> <!-- traffic advisory -->
<define name="TAU_RA" value="6." unit="s"/> <!-- resolution advisory -->
<define name="ALIM" value="15." unit="m"/> <!-- altitude limitation -->
<define name="DT_MAX" value="2000" unit="ms"/> <!-- lost comm or timeout -->
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<define name="CLIMB_THROTTLE" value="0.8"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
<define name="DESCENT_NAV_RATIO" value="1.0"/>
</section>
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
<define name="HOME_RADIUS" value="100" unit="m"/>
</section>
<section name="DATALINK" prefix="DATALINK_">
<define name="DEVICE_TYPE" value="XBEE"/>
<define name="DEVICE_ADDRESS" value="...."/>
</section>
<section name="SIMU">
<define name="YAW_RESPONSE_FACTOR" value="0.5"/>
</section>
<makefile>
include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile
FLASH_MODE=IAP
ap.CFLAGS += -DFBW -DAP -DCONFIG=\"tiny_0_99.h\" -DLED -DTIME_LED=1
ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c
ap.srcs += commands.c
ap.CFLAGS += -DACTUATORS=\"servos_4015_MAT_hw.h\" -DSERVOS_4015_MAT
ap.srcs += $(SRC_ARCH)/servos_4015_MAT_hw.c actuators.c
ap.CFLAGS += -DRADIO_CONTROL -DRADIO_CONTROL_TYPE=RC_FUTABA
ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c
ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=XBeeTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DXBEE_UART=Uart0 -DDATALINK=XBEE -DUART0_BAUD=B57600
ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c datalink.c xbee.c
ap.CFLAGS += -DINTER_MCU
ap.srcs += inter_mcu.c
ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2
ap.srcs += $(SRC_ARCH)/adc_hw.c
ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400
ap.srcs += gps_ubx.c gps.c latlong.c
ap.CFLAGS += -DINFRARED -DALT_KALMAN
ap.srcs += infrared.c estimator.c
ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c nav_survey_rectangle.c nav_line.c
# Hack to use the same tuning file than slayer1
#�ap.CFLAGS += -DUSE_GPIO
# ap.srcs += $(SRC_ARCH)/gpio.c
# formation
ap.CFLAGS += -DFORMATION -DTRAFFIC_INFO
ap.srcs += traffic_info.c formation.c snav.c
# external module
ap.srcs += external.c
# tcas
ap.CFLAGS += -DTCAS
ap.srcs += tcas.c
# Harware In The Loop
#ap.CFLAGS += -DHITL
# Losange Cartography with Onboard Camera
ap.CFLAGS += -DUSE_ONBOARD_CAMERA
ap.srcs +=nav_survey_losange_carto.c
#################################################################################################
# Config for SITL simulation
include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN -DFORMATION -DTRAFFIC_INFO -DTCAS
sim.srcs += nav_survey_rectangle.c nav_line.c external.c traffic_info.c formation.c snav.c tcas.c
# Losange Cartography with Onboard Camera
sim.CFLAGS += -DUSE_ONBOARD_CAMERA
sim.srcs += nav_survey_losange_carto.c
#pour les printf de debug
sim.CFLAGS += -DDEBUG_PRINTF
</makefile>
</airframe>
conf/airframes/Poine/booz2_a6.xml → conf/airframes/Poine/h_hex.xml
View File
conf/airframes/esden/calib/aspirin_012.xml
+32
View File
@@ -0,0 +1,32 @@
<section name="IMU" prefix="IMU_">
<define name="GYRO_P_NEUTRAL" value="93"/>
<define name="GYRO_Q_NEUTRAL" value="131"/>
<define name="GYRO_R_NEUTRAL" value="-25"/>
<define name="GYRO_P_SENS" value="5.0" integer="16"/>
<define name="GYRO_Q_SENS" value="5.0" integer="16"/>
<define name="GYRO_R_SENS" value="5.0" integer="16"/>
<define name="GYRO_PQ_SENS" value="0.0" integer="16"/>
<define name="GYRO_PR_SENS" value="0.0" integer="16"/>
<define name="GYRO_QR_SENS" value="0.0" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="4"/>
<define name="ACCEL_Y_NEUTRAL" value="-14"/>
<define name="ACCEL_Z_NEUTRAL" value="-5"/>
<define name="ACCEL_X_SENS" value="39" integer="16"/>
<define name="ACCEL_Y_SENS" value="39" integer="16"/>
<define name="ACCEL_Z_SENS" value="39" integer="16"/>
<define name="ACCEL_XY_SENS" value="0.0" integer="16"/>
<define name="ACCEL_XZ_SENS" value="0.0" integer="16"/>
<define name="ACCEL_YZ_SENS" value="0.0" integer="16"/>
<define name="MAG_X_NEUTRAL" value="-21"/>
<define name="MAG_Y_NEUTRAL" value="10"/>
<define name="MAG_Z_NEUTRAL" value="12"/>
<define name="MAG_X_SENS" value="3.17378921476" integer="16"/>
<define name="MAG_Y_SENS" value="3.14663275967" integer="16"/>
<define name="MAG_Z_SENS" value="3.26531022727" integer="16"/>
<define name="MAG_XY_SENS" value="0.0" integer="16"/>
<define name="MAG_XZ_SENS" value="0.0" integer="16"/>
<define name="MAG_YZ_SENS" value="0.0" integer="16"/>
</section>
conf/airframes/esden/lisa_m_pwm.xml
+5 -5
View File
@@ -158,7 +158,7 @@
</section>
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="10." integer="16"/>
<define name="BARO_SENS" value="3.3" integer="16"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
@@ -167,14 +167,14 @@
<define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
<define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
<define name="MAX_SUM_ERR" value="2000000"/>
<define name="HOVER_KP" value="-500"/>
<define name="HOVER_KD" value="-200"/>
<define name="HOVER_KI" value="-100"/>
<define name="HOVER_KP" value="-150"/>
<define name="HOVER_KD" value="-80"/>
<define name="HOVER_KI" value="-20"/>
<!-- 1.5m/s for full stick : BOOZ_SPEED_I_OF_F(1.5) / (MAX_PPRZ/2) -->
<define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
<define name="INV_M" value ="0.2"/>
<define name="INV_M" value ="0.21"/>
</section>
conf/airframes/esden/lisa_pwm_aspirin.xml
+33 -33
View File
@@ -38,7 +38,7 @@
<section name="SUPERVISION" prefix="SUPERVISION_">
<define name="MIN_MOTOR" value="1090"/>
<define name="MIN_MOTOR" value="1150"/>
<define name="MAX_MOTOR" value="2000"/>
<define name="TRIM_A" value="0"/>
<define name="TRIM_E" value="0"/>
@@ -51,40 +51,13 @@
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
</section>
<include href="conf/airframes/esden/calib/aspirin_012.xml"/>
<section name="IMU" prefix="IMU_">
<define name="GYRO_P_NEUTRAL" value="27"/>
<define name="GYRO_Q_NEUTRAL" value="-10"/>
<define name="GYRO_R_NEUTRAL" value="20"/>
<define name="GYRO_P_SENS" value="5.0" integer="16"/>
<define name="GYRO_Q_SENS" value="5.0" integer="16"/>
<define name="GYRO_R_SENS" value="5.0" integer="16"/>
<define name="GYRO_PQ_SENS" value="0.0" integer="16"/>
<define name="GYRO_PR_SENS" value="0.0" integer="16"/>
<define name="GYRO_QR_SENS" value="0.0" integer="16"/>
<define name="ACCEL_X_NEUTRAL" value="4"/>
<define name="ACCEL_Y_NEUTRAL" value="-14"/>
<define name="ACCEL_Z_NEUTRAL" value="-5"/>
<define name="ACCEL_X_SENS" value="39" integer="16"/>
<define name="ACCEL_Y_SENS" value="39" integer="16"/>
<define name="ACCEL_Z_SENS" value="39" integer="16"/>
<define name="ACCEL_XY_SENS" value="0.0" integer="16"/>
<define name="ACCEL_XZ_SENS" value="0.0" integer="16"/>
<define name="ACCEL_YZ_SENS" value="0.0" integer="16"/>
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
<define name="MAG_X_SENS" value="1." integer="16"/>
<define name="MAG_Y_SENS" value="1." integer="16"/>
<define name="MAG_Z_SENS" value="1." integer="16"/>
<define name="MAG_XY_SENS" value="0.0" integer="16"/>
<define name="MAG_XZ_SENS" value="0.0" integer="16"/>
<define name="MAG_YZ_SENS" value="0.0" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="RadOfDeg( 0. )"/>
<define name="BODY_TO_IMU_THETA" value="RadOfDeg( 0. )"/>
<define name="BODY_TO_IMU_PSI" value="RadOfDeg( 45. )"/>
<define name="BODY_TO_IMU_PSI" value="RadOfDeg( 225. )"/>
</section>
@@ -137,6 +110,7 @@
<define name="REF_MAX_R" value="RadOfDeg(180.)"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
<!-- Old settings with exponential throttle motor controllers
<!-- feedback -->
<define name="PHI_PGAIN" value="-250"/>
<define name="PHI_DGAIN" value="-200"/>
@@ -154,7 +128,33 @@
<define name="PHI_DDGAIN" value=" 200"/>
<define name="THETA_DDGAIN" value=" 200"/>
<define name="PSI_DDGAIN" value=" 200"/>
-->
<!-- feedback -->
<define name="PHI_PGAIN" value="-1900"/>
<define name="PHI_DGAIN" value="-380"/>
<define name="PHI_IGAIN" value="-200"/>
<define name="THETA_PGAIN" value="-1900"/>
<define name="THETA_DGAIN" value="-380"/>
<define name="THETA_IGAIN" value="-200"/>
<define name="PSI_PGAIN" value="-2000"/>
<define name="PSI_DGAIN" value="-400"/>
<define name="PSI_IGAIN" value="-10"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value=" 300"/>
<define name="THETA_DDGAIN" value=" 300"/>
<define name="PSI_DDGAIN" value=" 300"/>
</section>
<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_FREQUENCY" value="512"/>
<define name="H_X" value=" 0.51562740288882"/>
<define name="H_Y" value="-0.05707735220832"/>
<define name="H_Z" value=" 0.85490967783446"/>
</section>
<section name="INS" prefix="INS_">
@@ -178,7 +178,6 @@
<define name="INV_M" value ="0.15"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="-100"/>
<define name="DGAIN" value="-100"/>
@@ -224,7 +223,8 @@
<subsystem name="gps" type="ublox">
<configure name="GPS_BAUD" value="B57600"/>
</subsystem>
<subsystem name="ahrs" type="cmpl"/>
<!--subsystem name="ahrs" type="cmpl"/-->
<subsystem name="ahrs" type="ic"/>
</firmware>
<firmware name="lisa_l_test_progs">
conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
+1 -1
View File
@@ -15,5 +15,5 @@ ifeq ($(NORADIO), False)
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control/rc_datalink.c
# arch only with sim target for compatibility (empty functions)
sim.srcs += $(SRC_ARCH)/radio_control/rc_datalink.c
sim.srcs += $(SRC_ARCH)/subsystems/radio_control/rc_datalink.c
endif
conf/control_panel.xml.example
+5
View File
@@ -63,6 +63,11 @@
</program>
<program name="Http Server" command="sw/ground_segment/tmtc/boa"/>
<program name="Weather Station" command="sw/ground_segment/misc/davis2ivy">
<arg flag="-b" variable="ivy_bus"/>
<arg flag="-d" constant="/dev/ttyUSB1"/>
</program>
</section>
conf/flight_plans/basic_booz.xml → conf/flight_plans/rotorcraft_basic.xml
View File
conf/flight_plans/versatile_carto_fixe_muret.xml
+160
View File
@@ -0,0 +1,160 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="240" ground_alt="190" lat0="43.4622" lon0="1.2729" max_dist_from_home="1200" name="Versatile" qfu="270" security_height="60">
<!--<flight_plan alt="240" ground_alt="190" lat0="43.224622" lon0="1.2729" max_dist_from_home="900" name="Versatile" qfu="270" security_height="25"> -->
<header>
#include "datalink.h"
/*
#ifndef VARDECLARED
#define VARDECLARED
float varsweepsize=110;
#endif
*/
</header>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint name="STDBY" x="20" y="80"/>
<waypoint name="1" x="44.8" y="102.2"/>
<waypoint name="2" x="-63.5" y="122.9"/>
<waypoint name="MOB" x="-11.5" y="-21.2"/>
<waypoint name="S1" x="-177.6" y="11.6"/>
<waypoint name="S2" x="173.9" y="-41.3"/>
<waypoint name="S3" x="-60.6" y="310.9"/>
<waypoint name="S4" x="-204.7" y="17.0"/>
<waypoint alt="30" name="AF" x="200" y="-10"/>
<waypoint alt="0" name="TD" x="80.0" y="20.0"/>
<waypoint name="BASELEG" x="26.9" y="-23.0"/>
<waypoint name="_1" x="-100" y="0"/>
<waypoint name="_2" x="-100" y="200"/>
<waypoint name="_3" x="100" y="200"/>
<waypoint name="_4" x="100" y="0"/>
<waypoint name="CLIMB" x="-122.5" y="35.4"/>
<waypoint name="SP1" x="-57.2" y="9.8"/>
<waypoint name="SP2" x="54.2" y="-41.7"/>
<waypoint name="SP3" x="-15.7" y="88.6"/>
<waypoint name="SP4" x="97.5" y="37.8"/>
</waypoints>
<!-- <sectors>
<sector name="PolySectorS">
<corner name="S1"/>
<corner name="S2"/>
<corner name="S3"/>
<corner name="S4"/>
</sector>
</sectors>-->
<exceptions/>
<blocks>
<block name="Wait GPS">
<set value="1" var="kill_throttle"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Holding point">
<set value="1" var="kill_throttle"/>
<set value="NAV_MODE_ROLL" var="nav_mode"/>
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png">
<exception cond="estimator_z > ground_alt+25" deroute="Standby"/>
<set value="0" var="kill_throttle"/>
<go wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="IncRailNumber" strip_button="Standby" strip_icon="home.png">
<call fun="nav_survey_Inc_railnumberSinceBoot()"/>
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="Snapshoot" strip_button="Standby" strip_icon="home.png">
<call fun="nav_survey_Snapshoot()"/>
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="SnapshootContinu" strip_button="Standby" strip_icon="home.png">
<call fun="nav_survey_Snapshoot_Continu()"/>
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="StopSnapshoot" strip_button="Standby" strip_icon="home.png">
<call fun="nav_survey_StopSnapshoot()"/>
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="Oval 1-2" strip_button="Oval (wp 1-2)" strip_icon="oval.png">
<oval p1="1" p2="2" radius="nav_radius"/>
</block>
<block name="MOB" strip_button="Turn around here" strip_icon="mob.png">
<call fun="NavSetWaypointHere(WP_MOB)"/>
<circle radius="100" wp="MOB"/>
</block>
<block name="init_display_carto">
<call fun="nav_survey_computefourth_corner(WP_S1,WP_S2,WP_S3,WP_S4)"/>
</block>
<block name="Monter">
<call fun="NavSetWaypointHere(WP_MOB)"/>
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
<circle alt="ground_alt+105" radius="nav_radius" wp="MOB"/>
<exception cond="estimator_z > ground_alt+100" deroute="Carto survey S1-S2-S3"/>
</block>
<block name="Carto survey S1-S2-S3" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
<call fun="nav_survey_losange_carto_init(WP_S1,WP_S2,WP_S3, 50,150)"/>
<call fun="nav_survey_losange_carto()"/>
</block>
<block name="Carto survey S1-S3-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
<call fun="nav_survey_losange_carto_init(WP_S1,WP_S3,WP_S2, 0,150)"/>
<call fun="nav_survey_losange_carto()"/>
</block>
<block name="Fin Carto" strip_button="Standby" strip_icon="home.png">
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="init_display_carto Fin">
<call fun="nav_survey_computefourth_corner(WP_SP1,WP_SP2,WP_SP3,WP_SP4)"/>
</block>
<block name="Descendre">
<call fun="NavSetWaypointHere(WP_MOB)"/>
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
<circle alt="ground_alt+35" radius="nav_radius" wp="MOB"/>
<exception cond="ground_alt+40 > estimator_z " deroute="Carto survey SP1-SP2-SP3 fin"/>
</block>
<block name="Carto survey SP1-SP2-SP3 fin" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
<call fun="nav_survey_losange_carto_init(WP_SP1,WP_SP2,WP_SP3, 20,150)"/>
<call fun="nav_survey_losange_carto()"/>
</block>
<block name="Carto survey SP1-SP3-SP2 fin " strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
<call fun="nav_survey_losange_carto_init(WP_SP1,WP_SP3,WP_SP2, 0,150)"/>
<call fun="nav_survey_losange_carto()"/>
</block>
<block name="Fin Carto fin " strip_button="Standby" strip_icon="home.png">
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Survey S1-S2" strip_button="Survey (wp S1-S2)" strip_icon="survey.png">
<survey_rectangle grid="150" wp1="S1" wp2="S2"/>
</block>
</blocks>
</flight_plan>
sw/ground_segment/joystick/attack3_booz_nav.xml → conf/joystick/attack3_booz_nav.xml
+16 -2
View File
@@ -11,8 +11,17 @@
<button index="5" name="button6"/>
<button index="6" name="button7"/>
<button index="7" name="button8"/>
<button index="8" name="button9"/>
<button index="9" name="button10"/>
<button index="10" name="button11"/>
</input>
<variables>
<var name="mode" default="0"/>
<set var="mode" value="0" on_event="button10"/>
<set var="mode" value="1" on_event="button11"/>
</variables>
<messages period="0.1">
<!--message class="datalink" name="BOOZ2_FMS_COMMAND">
<field name="h_mode" value="IndexOfEnum(NAV)"/>
@@ -34,12 +43,17 @@
<field name="index" value="IndexOfSetting(booz2_cam_tilt_pwm)"/>
<field name="value" value="Scale(throttle, 1000, 2000)"/>
</message-->
<message class="ground" name="DL_SETTING" on_event="button11 || button10">
<field name="index" value="IndexOfSetting(kill_throttle)"/>
<field name="value" value="mode"/>
</message>
<message class="ground" name="JUMP_TO_BLOCK" on_event="shoot && button7">
<field name="block_id" value="IndexOfBlock('land here')"/>
</message>
<message class="ground" name="JUMP_TO_BLOCK" on_event="shoot && button8">
<!--message class="ground" name="JUMP_TO_BLOCK" on_event="shoot && button8">
<field name="block_id" value="IndexOfBlock('Joystick')"/>
</message>
</message-->
</messages>
</joystick>
conf/joystick/aurora_skate_controller.xml
+144
View File
@@ -0,0 +1,144 @@
<!-- Custom Aurora Skate Controller
Joystick has four axes:
axis 0: X on right stick
axis 1: Y on right stick
axis 2: Y on left stick
axis 3: X on left stick
It has 9 buttons.
b0 - right side/left bumper
b1 - right side/right bumper
b2 - right side/down bumper
b3 - right side/up bumper
b7 - yellow
b8 - red
b9 - green
b10 - left side/left bumper
b11 - left side/right bumper
-->
<joystick>
<input>
<axis index="0" name="rx" limit="1.00" exponent="0.7" trim="0"/>
<axis index="1" name="ry" limit="1.00" exponent="0.7" trim="0"/>
<axis index="2" name="ly" limit="1.00" exponent="0.0" trim="0"/>
<axis index="3" name="lx" limit="1.00" exponent="0.7" trim="0"/>
<button index="0" name="b0"/>
<button index="1" name="b1"/>
<button index="2" name="b2"/>
<button index="3" name="b3"/>
<button index="4" name="b4"/>
<button index="5" name="b5"/>
<button index="6" name="b6"/>
<button index="7" name="b7"/>
<button index="8" name="b8"/>
<button index="9" name="b9"/>
<button index="10" name="b10"/>
<button index="11" name="b11"/>
</input>
<messages period="0.0333333333">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="joystick_id" value="JoystickID()"/>
<field name="throttle" value="Scale(ly,-10,127)"/>
<field name="roll" value="rx"/>
<field name="yaw" value="lx"/>
<field name="pitch" value="0-ry"/>
</message>
<!-- trim commands -->
<message class="ground" name="DL_SETTING_MOD" on_event="b1">
<field name="index" value="IndexOfSetting('rc_roll_trim')"/>
<field name="mod" value="10"/>
</message>
<message class="ground" name="DL_SETTING_MOD" on_event="b0">
<field name="index" value="IndexOfSetting('rc_roll_trim')"/>
<field name="mod" value="0-10"/>
</message>
<message class="ground" name="DL_SETTING_MOD" on_event="b3">
<field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
<field name="mod" value="10"/>
</message>
<message class="ground" name="DL_SETTING_MOD" on_event="b2">
<field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
<field name="mod" value="0-10"/>
</message>
<message class="ground" name="DL_SETTING_MOD" on_event="b11">
<field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
<field name="mod" value="10"/>
</message>
<message class="ground" name="DL_SETTING_MOD" on_event="b10">
<field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
<field name="mod" value="0-10"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b10 && b11">
<field name="index" value="IndexOfSetting('rc_roll_trim')"/>
<field name="value" value="0"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b10 && b11">
<field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
<field name="value" value="0"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b10 && b11">
<field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
<field name="value" value="0-10"/>
</message>
<!--
<message class="ground" name="lx" on_event="b8"/>
<message class="trim_plus" name="ly" on_event="b10"/>
<message class="trim_minus" name="ly" on_event="b11"/>
<message class="trim_plus" name="rx" on_event="b2"/>
<message class="trim_minus" name="rx" on_event="b0"/>
<message class="trim_minus" name="ry" on_event="b3"/>
<message class="trim_plus" name="ry" on_event="b1"/>
<message class="trim_save" name="" on_event="b5"/>
-->
<!-- Camera switch. Currently uses same buttons as yaw trim -->
<message class="ground" name="DL_SETTING" on_event="b1">
<field name="index" value="IndexOfSetting('cam_switch')"/>
<field name="value" value="1"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b0">
<field name="index" value="IndexOfSetting('cam_switch')"/>
<field name="value" value="0"/>
</message>
<!-- Red Button --> <!-- AUTO2 mode -->
<message class="ground" name="DL_SETTING" on_event="b8">
<field name="index" value="IndexOfSetting('pprz_mode')"/>
<field name="value" value="2"/>
</message>
<!-- Yellow Button --> <!-- AUTO1 mode -->
<message class="ground" name="DL_SETTING" on_event="b7">
<field name="index" value="IndexOfSetting('pprz_mode')"/>
<field name="value" value="1"/>
</message>
<!-- Green Button --> <!-- MANUAL mode -->
<message class="ground" name="DL_SETTING" on_event="b9">
<field name="index" value="IndexOfSetting('pprz_mode')"/>
<field name="value" value="0"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b7 || b8 || b9">
<field name="index" value="IndexOfSetting('rc_dl_active_joystick')"/>
<field name="value" value="JoystickID()"/>
</message>
</messages>
</joystick>
conf/joystick/cockpit_sx.xml
+22
View File
@@ -0,0 +1,22 @@
<joystick>
<input>
<axis index="0" name="roll"/>
<axis index="1" name="pitch"/>
<axis index="2" name="yaw"/>
<axis index="3" name="spoiler"/>
<axis index="4" name="throttle"/>
<button index="3" name="mode"/>
</input>
<messages period="0.1">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="mode" value="mode + 1"/> <!-- only AUTO1 and AUTO2 available -->
<field name="throttle" value="Fit(throttle,0-100,100,0,127)"/>
<field name="roll" value="roll"/>
<field name="pitch" value="pitch"/>
<field name="yaw" value="yaw"/>
</message>
</messages>
</joystick>
conf/joystick/ikarus_usb.xml
+20
View File
@@ -0,0 +1,20 @@
<joystick>
<input>
<axis index="0" name="roll"/>
<axis index="1" name="pitch"/>
<axis index="2" name="throttle"/>
<axis index="4" name="mode"/>
</input>
<messages period="0.1">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="mode" value="PprzMode(0-mode)"/>
<field name="throttle" value="127-throttle"/>
<field name="roll" value="roll"/>
<field name="pitch" value="pitch"/>
<field name="yaw" value="0"/>
</message>
</messages>
</joystick>
conf/joystick/logitech_dual_action.xml
+87
View File
@@ -0,0 +1,87 @@
<!-- Logitech dual action controller profile
Joystick has six axes:
axis 1: X on left stick
axis 2: Y on left stick
axis 3: X on right stick
axis 4: Y on right stick
axis 5: X on D pad
axis 6: Y on D pad
It has 12 buttons, 10 of which are labeled on the joystick
Buttons 11 and 12 are not to be found.
The "mode" button swaps the axes on the left stick and the d pad.
-->
<joystick>
<input>
<axis index="0" name="lx" limit="1.00" exponent="0.7" trim="0"/>
<axis index="1" name="ly" limit="1.00" exponent="0.7" trim="0"/>
<axis index="2" name="rx" limit="1.00" exponent="0.7" trim="0"/>
<axis index="3" name="ry" limit="1.00" exponent="0.7" trim="0"/>
<axis index="4" name="dpadx"/>
<axis index="5" name="dpady"/>
<button index="0" name="b0"/>
<button index="1" name="b1"/>
<button index="2" name="b2"/>
<button index="3" name="b3"/>
<button index="4" name="b4"/>
<button index="5" name="b5"/>
<button index="6" name="b6"/>
<button index="7" name="b7"/>
<button index="8" name="b8"/>
<button index="9" name="b9"/>
<button index="10" name="b10"/>
<button index="11" name="b11"/>
</input>
<messages period="0.017">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="throttle" value="Scale(0-ly,0,255)"/>
<field name="roll" value="rx"/>
<field name="yaw" value="lx"/>
<field name="pitch" value="ry"/>
</message>
<!-- trim commands -->
<!-- not needed
<message class="trim_plus" name="lx" on_event="b9"/>
<message class="trim_minus" name="lx" on_event="b8"/>
<message class="trim_plus" name="ly" on_event="b10"/>
<message class="trim_minus" name="ly" on_event="b11"/>
<message class="trim_plus" name="rx" on_event="b2"/>
<message class="trim_minus" name="rx" on_event="b0"/>
<message class="trim_minus" name="ry" on_event="b3"/>
<message class="trim_plus" name="ry" on_event="b1"/>
<message class="trim_save" name="" on_event="b5"/>
-->
<!-- Button 5 top left bumper) --> <!-- AUTO1 mode -->
<message class="ground" name="DL_SETTING" on_event="b4">
<field name="index" value="5"/>
<field name="value" value="1"/>
</message>
<!-- Button 7 bottom left bumper --> <!-- AUTO2 mode -->
<message class="ground" name="DL_SETTING" on_event="b6">
<field name="index" value="5"/>
<field name="value" value="2"/>
</message>
<!-- Buttons 6+8 (right pad) --> <!-- KILL mode -->
<message class="ground" name="DL_SETTING" on_event="b5">
<field name="index" value="5"/>
<field name="value" value="5"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b7">
<field name="index" value="5"/>
<field name="value" value="5"/>
</message>
</messages>
</joystick>
conf/joystick/logitech_f710.xml
+153
View File
@@ -0,0 +1,153 @@
<!-- Logitech F710
This config is for Xinput mode. Make sure slider switch on back of controller is on X (not D)
Also make sure controller not in sports mode (mode light should be off)
Joystick has eight axes:
axis 0: X on left stick
axis 1: Y on left stick
axis 2: Left trigger
axis 3: X on right stick
axis 4: Y on right stick
axis 5: Right trigger
axis 6: X on D-Pad (not analog)
axis 7: Y on D-Pad (not analog)
It has 8 buttons.
b_green - green button (A)
b_red - red button (B)
b_blue - blue button (X)
b_yellow - yellow button (Y)
b_ltb - left trigger button
b_rtb - right trigger button
b_start - start button
b_back - back button
-->
<joystick>
<input>
<axis index="0" name="lx" limit="1.00" exponent="0.7" trim="0"/>
<axis index="1" name="ly" limit="1.00" exponent="0.0" trim="0"/>
<axis index="2" name="lt" limit="1.00" exponent="0.0" trim="0"/>
<axis index="3" name="rx" limit="1.00" exponent="0.7" trim="0"/>
<axis index="4" name="ry" limit="1.00" exponent="0.7" trim="0"/>
<axis index="5" name="rt" limit="1.00" exponent="0.0" trim="0"/>
<axis index="6" name="dx" limit="1.00" exponent="0.0" trim="0"/>
<axis index="7" name="dy" limit="1.00" exponent="0.0" trim="0"/>
<button index="0" name="b_green"/>
<button index="1" name="b_red"/>
<button index="2" name="b_blue"/>
<button index="3" name="b_yellow"/>
<button index="4" name="b_ltb"/>
<button index="5" name="b_rtb"/>
<button index="6" name="b_start"/>
<button index="7" name="b7"/>
<button index="8" name="b8"/>
<button index="9" name="b9"/>
<button index="10" name="b_back"/>
</input>
<messages period="0.0333333333">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="joystick_id" value="JoystickID()"/>
<field name="throttle" value="Bound(0-ly,0,127)"/>
<field name="roll" value="rx"/>
<field name="yaw" value="lx"/>
<field name="pitch" value="ry"/>
</message>
<!-- trim commands -->
<message class="ground" name="DL_SETTING_MOD" on_event="dx > 100">
<field name="index" value="IndexOfSetting('rc_roll_trim')"/>
<field name="mod" value="10"/>
</message>
<message class="ground" name="DL_SETTING_MOD" on_event="0-dx > 100" >
<field name="index" value="IndexOfSetting('rc_roll_trim')"/>
<field name="mod" value="0-10"/>
</message>
<message class="ground" name="DL_SETTING_MOD" on_event="dy > 100">
<field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
<field name="mod" value="10"/>
</message>
<message class="ground" name="DL_SETTING_MOD" on_event="0-dy > 100">
<field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
<field name="mod" value="0-10"/>
</message>
<message class="ground" name="DL_SETTING_MOD" on_event="b_rtb">
<field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
<field name="mod" value="10"/>
</message>
<message class="ground" name="DL_SETTING_MOD" on_event="b_ltb">
<field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
<field name="mod" value="0-10"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b_ltb && b_rtb">
<field name="index" value="IndexOfSetting('rc_roll_trim')"/>
<field name="value" value="0"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b_ltb && b_rtb">
<field name="index" value="IndexOfSetting('rc_pitch_trim')"/>
<field name="value" value="0"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b_ltb && b_rtb">
<field name="index" value="IndexOfSetting('rc_yaw_trim')"/>
<field name="value" value="0-10"/>
</message>
<!--
<message class="ground" name="lx" on_event="b8"/>
<message class="trim_plus" name="ly" on_event="b10"/>
<message class="trim_minus" name="ly" on_event="b11"/>
<message class="trim_plus" name="rx" on_event="b2"/>
<message class="trim_minus" name="rx" on_event="b0"/>
<message class="trim_minus" name="ry" on_event="b3"/>
<message class="trim_plus" name="ry" on_event="b1"/>
<message class="trim_save" name="" on_event="b5"/>
-->
<!-- Camera switch. Currently uses same buttons as yaw trim -->
<message class="ground" name="DL_SETTING" on_event="b_start">
<field name="index" value="IndexOfSetting('cam_switch')"/>
<field name="value" value="1"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b_back">
<field name="index" value="IndexOfSetting('cam_switch')"/>
<field name="value" value="0"/>
</message>
<!-- Red Button --> <!-- AUTO2 mode -->
<message class="ground" name="DL_SETTING" on_event="b_red">
<field name="index" value="IndexOfSetting('pprz_mode')"/>
<field name="value" value="2"/>
</message>
<!-- Yellow Button --> <!-- AUTO1 mode -->
<message class="ground" name="DL_SETTING" on_event="b_yellow">
<field name="index" value="IndexOfSetting('pprz_mode')"/>
<field name="value" value="1"/>
</message>
<!-- Green Button --> <!-- MANUAL mode -->
<message class="ground" name="DL_SETTING" on_event="b_green">
<field name="index" value="IndexOfSetting('pprz_mode')"/>
<field name="value" value="0"/>
</message>
<message class="ground" name="DL_SETTING" on_event="b_green || b_yellow || b_red">
<field name="index" value="IndexOfSetting('rc_dl_active_joystick')"/>
<field name="value" value="JoystickID()"/>
</message>
</messages>
</joystick>
sw/ground_segment/joystick/ngs_set_actuators.xml → conf/joystick/ngs_set_actuators.xml
View File
sw/ground_segment/joystick/xbox_booz_nav.xml → conf/joystick/xbox_booz_nav.xml
View File
conf/messages.xml
+17 -3
View File
@@ -451,7 +451,7 @@
<field name="mode" type="uint8" unit="byte_mask"/>
<field name="gps_nb_err" type="uint8"/>
</message>
<message name="H_CTL_A" id="60">
<field name="roll_sum_err" type="float"/>
<field name="ref_roll_angle" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
@@ -1332,7 +1332,7 @@
<field name="bq" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="br" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
</message>
<!--179 is free -->
@@ -1603,7 +1603,12 @@
<!--216 is free -->
<!--217 is free -->
<!--218 is free -->
<!--219 is free -->
<message name="WEATHER" id="219">
<field name="p_amb" type="float" unit="Pa" alt_unit="mBar" alt_unit_coef="0.01"/>
<field name="t_amb" type="float" unit="deg C"/>
<field name="windspeed" type="float" unit="m/s"/>
<field name="wind_from" type="float" unit="deg"/>
</message>
<message name="IMU_TURNTABLE" id="220">
<field name="omega" type="float" unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578"/>
@@ -1949,6 +1954,15 @@
<field name="pitch" type="int8"/>
</message>
<message name="RC_4CH" id="52" link="broadcasted">
<field name="ac_id" type="uint8"/>
<field name="mode" type="uint8"/>
<field name="throttle" type="uint8"/>
<field name="roll" type="int8"/>
<field name="pitch" type="int8"/>
<field name="yaw" type="int8"/>
</message>
<message name="KITE_COMMAND" id="96">
<field name="POWER" type="uint16"/>
<field name="TURN" type="uint16"/>
conf/modules/cartography.xml
+22
View File
@@ -0,0 +1,22 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="cartography">
<header>
<file name="cartography.h"/>
</header>
<init fun="init_carto()"/>
<periodic fun="periodic_1Hz_carto()" freq="1." start="start_carto()" stop="stop_carto()" autorun="TRUE"/>
<periodic fun="periodic_10Hz_carto()" period="0.1" start="start_carto()" stop="stop_carto()" autorun="FALSE"/>
<makefile>
<raw>
#Exemple of RAW makefile part
</raw>
<define name="DEMO_MODULE_LED" value="2"/>
<file name="cartography.c"/>
</makefile>
<makefile target="sim">
<define name="SOME_FLAG"/>
<configure name="SOME_DEFINE" value="bla"/>
</makefile>
</module>
conf/modules/ins_arduimu_basic.xml
+1 -1
View File
@@ -6,7 +6,7 @@
</header>
<init fun="ArduIMU_init()"/>
<periodic fun="ArduIMU_periodic()" freq="60"/>
<periodic fun="ArduIMU_periodicGPS()" freq="8"/>
<periodic fun="ArduIMU_periodicGPS()" freq="4"/>
<event fun="ArduIMU_event()"/>
<makefile target="ap">
<file name="ins_arduimu_basic.c"/>
conf/settings/tuning_ctl_new.xml
+12 -55
View File
@@ -4,67 +4,27 @@
<settings>
<dl_settings>
<dl_settings NAME="flight params">
<dl_setting MAX="1000" MIN="0" STEP="10" VAR="flight_altitude" shortname="altitude"/>
<dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_east"/>
<dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_north"/>
</dl_settings>
<dl_settings NAME="mode">
<dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode" module="autopilot"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled" shortname="alt_kalman" module="estimator"/>
<dl_setting MAX="0" MIN="0" STEP="1" VAR="estimator_flight_time" shortname="flight time"/>
<dl_setting MAX="1000" MIN="0" STEP="1" VAR="stage_time"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="kill_throttle"/>
<dl_setting MAX="2" MIN="0" STEP="1" VAR="telemetry_mode_Ap_DefaultChannel" shortname="tele_AP" module="downlink"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="telemetry_mode_Fbw_DefaultChannel" shortname="tele_FBW" module="downlink"/>
<dl_setting MAX="2" MIN="0" STEP="1" VAR="gps_reset" module="gps_ubx" handler="Reset" shortname="GPS reset"/>
<dl_setting MAX="200" MIN="-200" STEP="10" VAR="nav_radius" module="subsystems/nav" handler="SetNavRadius">
<strip_button icon="circle-right.png" name="Circle right" value="1" group="circle"/>
<strip_button icon="circle-left.png" name="Circle left" value="-1" group="circle"/>
<key_press key="greater" value="1"/>
<key_press key="less" value="-1"/>
</dl_setting>
</dl_settings>
<dl_settings NAME="control">
<dl_settings NAME="infrared">
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="infrared.roll_neutral" shortname="roll_neutral" param="IR_ROLL_NEUTRAL_DEFAULT" unit="rad" module="subsystems/sensors/infrared" />
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="infrared.pitch_neutral" shortname="pitch_neutral" param="IR_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.lateral_correction" shortname="lat_corr" param="IR_LATERAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0." STEP="0.1" VAR="infrared.longitudinal_correction" shortname="log_corr" param="IR_LONGITUDINAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.vertical_correction" shortname="vert_corr" param="IR_VERTICAL_CORRECTION"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_left" shortname="corr_left" param="IR_CORRECTION_LEFT"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_right" shortname="corr_right" param="IR_CORRECTION_RIGHT"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_up" shortname="corr_up" param="IR_CORRECTION_UP"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.1" VAR="infrared.correction_down" shortname="corr_down" param="IR_CORRECTION_DOWN"/>
</dl_settings>
<dl_settings NAME="attitude">
<dl_setting MAX="1" MIN="0" STEP="1" var="use_airspeed_ratio" values="FALSE|TRUE"/>
<dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="fw_h_ctl"/>
<dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
<dl_setting MAX="0" MIN="-15000" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_igain" shortname="roll_igain" module="fw_h_ctl_a" handler="SetRollIGain"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_igain" shortname="roll_igain" param="H_CTL_ROLL_IGAIN" handler="SetRollIGain"/>
<dl_setting MAX="0" MIN="-55000" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain" param="H_CTL_PITCH_PGAIN"/>
<dl_setting MAX="0" MIN="-55000" STEP="250" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_igain" shortname="pitch_igain" module="fw_h_ctl_a" handler="SetPitchIGain"/>
<dl_setting MAX=".3" MIN="0." STEP="0.001" VAR="h_ctl_pitch_of_roll" shortname="pitch_of_roll" param="H_CTL_PITCH_OF_ROLL" module="fw_h_ctl_a"/>
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_igain" shortname="pitch_igain" param="H_CTL_PITCH_IGAIN" handler="SetPitchIGain"/>
<dl_setting MAX=".3" MIN="0." STEP="0.001" VAR="h_ctl_pitch_of_roll" shortname="pitch_of_roll" param="H_CTL_PITCH_OF_ROLL"/>
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle" module="stabilization/stabilization_adaptive"/>
<dl_setting MAX="60" MIN="0" STEP="1" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg" alt_unit_coef="0.0174533"/>
<dl_setting MAX="1" MIN="0" STEP="1" var="use_airspeed_ratio" values="FALSE|TRUE"/>
</dl_settings>
<dl_settings NAME="feedforward">
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffa" module="fw_h_ctl_a" shortname="roll_Kffa"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffd" module="fw_h_ctl_a" shortname="roll_Kffd"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffa" module="fw_h_ctl_a" shortname="pitch_Kffa"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffd" module="fw_h_ctl_a" shortname="pitch_Kffd"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffa" shortname="roll_Kffa" param="H_CTL_ROLL_KFFA"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kffd" shortname="roll_Kffd" param="H_CTL_ROLL_KFFD"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffa" shortname="pitch_Kffa" param="H_CTL_PITCH_KFFA"/>
<dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_Kffd" shortname="pitch_Kffd" param="H_CTL_PITCH_KFFD"/>
</dl_settings>
<dl_settings name="alt">
@@ -77,11 +37,11 @@
<strip_button name="AS" value="1" group="speed_mode"/>
<strip_button name="GS" value="2" group="speed_mode"/>
</dl_setting>
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle" module="fw_v_ctl" handler="SetCruiseThrottle" param="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE"/>
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle" module="guidance/guidance_v" handler="SetCruiseThrottle" param="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE"/>
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_pgain" shortname="pitch_p" param="V_CTL_AUTO_PITCH_PGAIN"/>
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_igain" shortname="pitch_i" param="V_CTL_AUTO_PITCH_IGAIN"/>
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_dgain" shortname="pitch_d" param="V_CTL_AUTO_PITCH_DGAIN" module="fw_v_ctl_n"/>
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_pitch_dgain" shortname="pitch_d" param="V_CTL_AUTO_PITCH_DGAIN" module="guidance/guidance_v_n"/>
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_p" param="V_CTL_AUTO_THROTTLE_PGAIN"/>
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_igain" shortname="throttle_i" param="V_CTL_AUTO_THROTTLE_IGAIN"/>
@@ -114,13 +74,10 @@
<dl_setting MAX="2" MIN="1" STEP="1" VAR="nav_mode"/>
<dl_setting MAX="5" MIN="-5" STEP="0.5" VAR="nav_climb"/>
<dl_setting MAX="20" MIN="-20" STEP="1" VAR="fp_pitch"/>
<dl_setting MAX="1" MIN="0" STEP="0.1" VAR="fp_throttle"/>
<dl_setting MAX="50" MIN="-50" STEP="5" VAR="nav_shift" module="subsystems/nav" handler="IncreaseShift" shortname="inc. shift"/>
<dl_setting MAX="50" MIN="5" STEP="0.5" VAR="nav_ground_speed_setpoint" shortname="ground speed"/>
<dl_setting MAX="0." MIN="-0.2" STEP="0.01" VAR="nav_ground_speed_pgain" shortname="ground speed pgain"/>
<dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_survey_shift"/>
<dl_setting MAX="0.3" MIN="0" STEP="0.01" VAR="cte_igain"/>
<dl_setting MAX="20" MIN="0" STEP="1" VAR="cte_max"/>
</dl_settings>
</dl_settings>
</dl_settings>
sw/airborne/arch/stm32/mcu_arch.c
+7
View File
@@ -87,5 +87,12 @@ void mcu_arch_init(void) {
/* Set the Vector Table base location at 0x08000000 */
NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0);
#ifdef STM32_FORCE_ALL_CLOCK_ON
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD |
RCC_APB2Periph_GPIOE | RCC_APB2Periph_AFIO, ENABLE);
#endif
}
sw/airborne/arch/stm32/mcu_periph/i2c_arch.new.c
+616
View File
File diff suppressed because it is too large Load Diff
sw/airborne/firmwares/fixedwing/datalink.c
+10 -1
View File
@@ -185,11 +185,20 @@ void dl_parse_msg(void) {
#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
LED_TOGGLE(3);
parse_rc_datalink(
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
} else
if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
LED_TOGGLE(3);
parse_rc_4ch_datalink(
DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
} else
#endif // RC_DATALINK
{ /* Last else */
/* Parse modules datalink */
sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+2 -2
View File
@@ -198,7 +198,7 @@ static inline void v_ctl_set_pitch ( void ) {
if (v_ctl_auto_pitch_igain < 0.) {
v_ctl_auto_pitch_sum_err += err*(1./60.);
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / (-v_ctl_auto_pitch_igain));
}
// PI loop + feedforward ctl
@@ -223,7 +223,7 @@ static inline void v_ctl_set_throttle( void ) {
if (v_ctl_auto_throttle_igain < 0.) {
v_ctl_auto_throttle_sum_err += err*(1./60.);
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain);
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / (-v_ctl_auto_throttle_igain));
}
// PID loop + feedforward ctl
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde
+4 -24
View File
@@ -31,29 +31,9 @@ float read_adc(int select)
float temp;
if (SENSOR_SIGN[select]<0){
temp = (AN_OFFSET[select]-AN[select]);
if (abs(temp)>900) {
#if PRINT_DEBUG != 0
Serial.print("!!!ADC:1,VAL:");
Serial.print (temp);
Serial.println("***");
#endif
#if PERFORMANCE_REPORTING == 1
adc_constraints++;
#endif
}
return constrain(temp,-900,900); //Throw out nonsensical values
} else {
temp = (AN[select]-AN_OFFSET[select]);
if (abs(temp)>900) {
#if PRINT_DEBUG != 0
Serial.print("!!!ADC:2,VAL:");
Serial.print (temp);
Serial.println("***");
#endif
#if PERFORMANCE_REPORTING == 1
adc_constraints++;
#endif
}
return constrain(temp,-900,900);
}
}
@@ -61,8 +41,8 @@ float read_adc(int select)
//Activating the ADC interrupts.
void Analog_Init(void)
{
ADCSRA|=(1<<ADIE)|(1<<ADEN);
ADCSRA|= (1<<ADSC);
ADCSRA|=(1<<ADIE)|(1<<ADEN);
ADCSRA|= (1<<ADSC);
}
//
@@ -80,8 +60,8 @@ ISR(ADC_vect)
high = ADCH;
if(analog_count[MuxSel]<63) {
analog_buffer[MuxSel] += (high << 8) | low; // cumulate analog values
analog_count[MuxSel]++;
analog_buffer[MuxSel] += (high << 8) | low; // cumulate analog values
analog_count[MuxSel]++;
}
MuxSel++;
MuxSel &= 0x07; //if(MuxSel >=8) MuxSel=0;
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde
+4 -66
View File
@@ -21,27 +21,13 @@ void Normalize(void)
renorm= .5 * (3-renorm); //eq.21
} else if (renorm < 100.0f && renorm > 0.01f) {
renorm= 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
#if PRINT_DEBUG != 0
Serial.print("???SQT:1,RNM:");
Serial.print (renorm);
Serial.print (",ERR:");
Serial.print (error);
Serial.println("***");
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
#if PRINT_DEBUG != 0
Serial.print("???PRB:1,RNM:");
Serial.print (renorm);
Serial.print (",ERR:");
Serial.print (error);
Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
@@ -53,25 +39,11 @@ void Normalize(void)
renorm= 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
#if PRINT_DEBUG != 0
Serial.print("???SQT:2,RNM:");
Serial.print (renorm);
Serial.print (",ERR:");
Serial.print (error);
Serial.println("***");
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
#if PRINT_DEBUG != 0
Serial.print("???PRB:2,RNM:");
Serial.print (renorm);
Serial.print (",ERR:");
Serial.print (error);
Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
@@ -83,23 +55,11 @@ void Normalize(void)
renorm= 1. / sqrt(renorm);
#if PERFORMANCE_REPORTING == 1
renorm_sqrt_count++;
#endif
#if PRINT_DEBUG != 0
Serial.print("???SQT:3,RNM:");
Serial.print (renorm);
Serial.print (",ERR:");
Serial.print (error);
Serial.println("***");
#endif
} else {
problem = TRUE;
#if PERFORMANCE_REPORTING == 1
renorm_blowup_count++;
#endif
#if PRINT_DEBUG != 0
Serial.print("???PRB:3,RNM:");
Serial.print (renorm);
Serial.println("***");
#endif
}
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
@@ -170,23 +130,18 @@ void Drift_correction(void)
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
if (Integrator_magnitude > ToRad(300)) {
Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
#if PRINT_DEBUG != 0
Serial.print("!!!INT:1,MAG:");
Serial.print (ToDeg(Integrator_magnitude));
Serial.println("***");
#endif
}
}
/**************************************************/
void Accel_adjust(void)
{
Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
}
/**************************************************/
/**************************************************/
void Matrix_update(void)
{
Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
@@ -202,7 +157,6 @@ void Matrix_update(void)
Accel_adjust(); //Remove centrifugal acceleration.
#if OUTPUTMODE==1
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
@@ -212,17 +166,6 @@ void Matrix_update(void)
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#endif
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
@@ -235,16 +178,11 @@ void Matrix_update(void)
}
}
/**************************************************/
void Euler_angles(void)
{
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
roll = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
yaw = 0;
#else
pitch = -asin(DCM_Matrix[2][0]);
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
#endif
}
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde
+19 -16
View File
@@ -1,24 +1,28 @@
//*****I2C Output************************************************************
void requestEvent(){
#if PRINT_DEBUG != 0
Serial.println("Sending IMU Data");
#endif
void fill_I2C_message() {
// Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ
// Float Number is multipited with 10000 and converted to an Integer, for sending via I2C.
// Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib)
I2C_Message_ar[0] = int(roll*(1<<12));
I2C_Message_ar[1] = int(pitch*(1<<12));
I2C_Message_ar[2] = int(yaw*(1<<12));
I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12));
I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12));
I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12));
I2C_Message_ar[3] = int(Omega_Vector[0]*(1<<12));
I2C_Message_ar[4] = int(Omega_Vector[1]*(1<<12));
I2C_Message_ar[5] = int(Omega_Vector[2]*(1<<12));
I2C_Message_ar[6] = int((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10));
}
void requestEvent(){
#if PRINT_DEBUG != 0
Serial.println("Sending IMU Data");
#endif
fill_I2C_message();
byte* pointer;
pointer = (byte*) &I2C_Message_ar;
Wire.send(pointer, 18);
@@ -51,6 +55,7 @@ void receiveEvent(int howMany){
void printdata(void){
#if PRINT_I2C_MSG == 1
fill_I2C_message();
Serial.print("Time ");
Serial.print(millis());
Serial.print("; Roll ");
@@ -89,8 +94,7 @@ void printdata(void){
Serial.print (",AN4:");
Serial.print(read_adc(4));
Serial.print (",AN5:");
Serial.print(read_adc(5));
Serial.print (",");
Serial.println(read_adc(5));
#endif
#if PRINT_DCM == 1
@@ -111,8 +115,7 @@ void printdata(void){
Serial.print (",EX7:");
Serial.print(convert_to_dec(DCM_Matrix[2][1]));
Serial.print (",EX8:");
Serial.print(convert_to_dec(DCM_Matrix[2][2]));
Serial.print (",");
Serial.println(convert_to_dec(DCM_Matrix[2][2]));
#endif
#if PRINT_EULER == 1
@@ -123,8 +126,7 @@ void printdata(void){
Serial.print(",YAW:");
Serial.print(ToDeg(yaw));
Serial.print(",IMUH:");
Serial.print((imu_health>>8)&0xff);
Serial.print (",");
Serial.println((imu_health>>8)&0xff);
#endif
#if PRINT_GPS == 1
@@ -141,6 +143,7 @@ void printdata(void){
gps_messages_sent++;
#endif
}
Serial.println("");
#endif
}
@@ -174,7 +177,7 @@ void printPerfData(long time)
Serial.print(gps_messages_sent,DEC);
Serial.print(",imu:");
Serial.print((imu_health>>8),DEC);
Serial.print(",***");
Serial.println(",***");
// Reset counters
mainLoop_count = 0;
sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde
+9 -10
View File
@@ -30,9 +30,6 @@
/*For debugging propurses*/
#define PRINT_DEBUG 0 //Will print Debug messages
//OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data
#define OUTPUTMODE 1
#define PRINT_I2C_MSG 0 //Will print the I2C output buffer
#define PRINT_DCM 0 //Will print the whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
@@ -49,7 +46,7 @@ int I2C_Message_ar[9];
// *** NOTE! To use ArduIMU with ArduPilot you must select binary output messages (change to 1 here)
#define PRINT_BINARY 0 //Will print binary message and suppress ASCII messages (above)
#define PRINT_BINARY 1 //Will print binary message and suppress ASCII messages (above)
// *** NOTE! Performance reporting is only supported for Ublox. Set to 0 for others
#define PERFORMANCE_REPORTING 1 //Will include performance reports in the binary output ~ 1/2 min
@@ -70,10 +67,9 @@ int I2C_Message_ar[9];
#define ToDeg(x) (x*57.2957795131) // *180/pi
// LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, 3.22mV/ADC step => 1.03
// Tested values : 0.96,0.96,0.94
#define Gyro_Gain_X 0.92 //X axis Gyro gain
#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
#define Gyro_Gain_X 1.0 //X axis Gyro gain
#define Gyro_Gain_Y 1.0 //Y axis Gyro gain
#define Gyro_Gain_Z 1.0 //Z axis Gyro gain
#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
@@ -84,7 +80,7 @@ int I2C_Message_ar[9];
//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
#define Ki_YAW 0.00005
#define ADC_WARM_CYCLES 75
#define ADC_WARM_CYCLES 100
#define FALSE 0
#define TRUE 1
@@ -268,7 +264,10 @@ void loop() //Main Loop
{
timeNow = millis();
if((timeNow-timer)>=20) // Main loop runs at 50Hz
// 20 -> Main loop runs at 50Hz
// 5 -> Main loop runs at 200Hz
// Max measured duty time around 3ms
if((timeNow-timer)>=5)
{
timer_old = timer;
timer = timeNow;
sw/airborne/modules/cartography/cartography.c
+745
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/cartography/cartography.h
+52
View File
@@ -0,0 +1,52 @@
/** Automatic survey of an oriented rectangle (defined by three points) */
#ifndef CARTOGRAPHY_H
#define CARTOGRAPHY_H
void init_carto(void);
void periodic_1Hz_carto(void);
void periodic_10Hz_carto(void);
void start_carto(void);
void stop_carto(void);
/*
typedef enum {NS, WE} survey_orientation_t;
*/
#ifdef USE_ONBOARD_CAMERA
extern bool_t CAMERA_SNAPSHOT_REQUIERED;
extern uint16_t camera_snapshot_image_number;
#endif
extern float distrailinteractif; //pour exporter la variable et pouvoir la changer depuis settings
///////////////////////////////////////////////////////////////////////////////////////////////
extern bool_t nav_survey_Inc_railnumberSinceBoot(void);
extern bool_t nav_survey_Snapshoot(void);
bool_t nav_survey_Snapshoot_Continu(void);
extern bool_t nav_survey_StopSnapshoot(void);
extern bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4 );
extern bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrail, float distplus);
extern bool_t nav_survey_losange_carto(void); // !!!! important il faut mettre void en parametres d'entrée, sinon le compilo dit: attention : function declaration isn»t a prototype
//(uint8_t wp1, uint8_t wp2, uint8_t wp3);
/*
#define NavSurveylosange_cartoInit(_wp1, _wp2, _grid, _distplus) nav_survey_losange_init(_wp1, _wp2, _wp3, _grid, _distplus)
#define NavSurveylosange_carto nav_survey_losange
*/
#endif
sw/airborne/modules/ins/ins_arduimu_basic.c
+1 -1
View File
@@ -132,7 +132,7 @@ void ArduIMU_event( void ) {
estimator_p = arduimu_rates.p;
ardu_ins_trans.status = I2CTransDone;
RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
//RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
}
sw/airborne/subsystems/radio_control/rc_datalink.c
+20 -3
View File
@@ -21,7 +21,7 @@
* Boston, MA 02111-1307, USA.
*/
#include "radio_control/rc_datalink.h"
#include "subsystems/radio_control/rc_datalink.h"
#include "subsystems/radio_control.h"
int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
@@ -33,11 +33,11 @@ void radio_control_impl_init(void) {
}
void parse_rc_datalink( uint8_t throttle_mode,
void parse_rc_3ch_datalink( uint8_t throttle_mode,
int8_t roll,
int8_t pitch)
{
uint8_t throttle = throttle_mode & 0xFC;
uint8_t throttle = ((throttle_mode & 0xFC)>>2)*(128/64);
uint8_t mode = throttle_mode & 0x03;
rc_dl_values[RADIO_ROLL] = roll;
@@ -47,3 +47,20 @@ void parse_rc_datalink( uint8_t throttle_mode,
rc_dl_frame_available = TRUE;
}
void parse_rc_4ch_datalink(
uint8_t mode,
uint8_t throttle,
int8_t roll,
int8_t pitch,
int8_t yaw)
{
rc_dl_values[RADIO_MODE] = (int8_t)mode;
rc_dl_values[RADIO_THROTTLE] = (int8_t)throttle;
rc_dl_values[RADIO_ROLL] = roll;
rc_dl_values[RADIO_PITCH] = pitch;
rc_dl_values[RADIO_YAW] = yaw;
rc_dl_frame_available = TRUE;
}
sw/airborne/subsystems/radio_control/rc_datalink.h
+14 -3
View File
@@ -43,13 +43,24 @@ extern int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
extern volatile bool_t rc_dl_frame_available;
/**
* Decode datalink message to get rc values
* Decode datalink message to get rc values with RC_3CH message
* Mode and throttle are merge in the same byte
*/
extern void parse_rc_datalink(
extern void parse_rc_3ch_datalink(
uint8_t throttle_mode,
int8_t roll,
int8_t pitch);
/**
* Decode datalink message to get rc values with RC_4CH message
*/
extern void parse_rc_4ch_datalink(
uint8_t mode,
uint8_t throttle,
int8_t roll,
int8_t pitch,
int8_t yaw);
/**
* Macro that normalize rc_dl_values to radio values
*/
@@ -60,7 +71,7 @@ extern void parse_rc_datalink(
Bound(_out[RADIO_PITCH], MIN_PPRZ, MAX_PPRZ); \
_out[RADIO_YAW] = 0; \
Bound(_out[RADIO_YAW], MIN_PPRZ, MAX_PPRZ); \
_out[RADIO_THROTTLE] = ((MAX_PPRZ/64) * _in[RADIO_THROTTLE]); \
_out[RADIO_THROTTLE] = ((MAX_PPRZ/128) * _in[RADIO_THROTTLE]); \
Bound(_out[RADIO_THROTTLE], 0, MAX_PPRZ); \
_out[RADIO_MODE] = MAX_PPRZ * (_in[RADIO_MODE] - 1); \
Bound(_out[RADIO_MODE], MIN_PPRZ, MAX_PPRZ); \
sw/ground_segment/joystick/README
+3 -1
View File
@@ -21,7 +21,9 @@
#
Tools for controling an aircraft with a ground joystick
Tools for controlling an aircraft with a ground joystick
Also used as general purpose joystick to ivybus connector. Can generate arbitrary ivybus messages based on control input from buttons or axes. Supports limiting, exponential and scaling on axes.
sw/ground_segment/joystick/input2ivy.ml
+297 -53
View File
File diff suppressed because it is too large Load Diff
sw/ground_segment/joystick/usb_stick.c
+207 -75
View File
@@ -21,6 +21,11 @@
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* 1/26/2011 - jpeverill added
support for joystick specific event interface. During testing it was found that many devices do not work properly with the standard event interface (such as the PS3 joystick). The joystick interface seems to work better. The old event interface is used if "event" is found in the name, otherwise the joystick interface is used.
now kills the process in the event of a read error (joystick unplugs)
*/
//#define STICK_DBG 1
#include "usb_stick.h"
@@ -33,11 +38,14 @@
#include <errno.h>
#include <math.h>
#include <glib.h>
#include <linux/input.h>
#include <sys/ioctl.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
//needed for joystick interface
#include <linux/input.h>
#include <linux/joystick.h>
#ifdef STICK_DBG
#define dbgprintf fprintf
#else
@@ -62,15 +70,19 @@
int stick_device_handle;
int8_t stick_axis_values[AXIS_COUNT] = {0, 0, 0, 0, 0, 0};
int16_t stick_button_values = 0;
int32_t stick_button_values = 0;
int axis_code[AXIS_COUNT];
int stick_axis_count = 0;
int button_code[BUTTON_COUNT];
int stick_button_count = 0;
int hid_led_count = 0;
int32_t axis_min[AXIS_COUNT], axis_max[AXIS_COUNT];
int event_mode = STICK_MODE_UNKNOWN;
struct stick_code_param_ stick_init_param = {
0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
0, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
@@ -79,17 +91,56 @@ struct stick_code_param_ stick_init_param = {
//struct ff_effect effect;
void led_test( void ) {
struct input_event ev; /* the event */
int lednum;
/* we turn off all the LEDs to start */
ev.type = EV_LED;
while (1) {
for (lednum=0;lednum<32;lednum++) {
ev.code = lednum;
ev.value = 1;
write(stick_device_handle, &ev, sizeof(struct input_event));
}
usleep(1000000);
for (lednum=0;lednum<32;lednum++) {
ev.code = lednum;
ev.value = 0;
write(stick_device_handle, &ev, sizeof(struct input_event));
}
}
}
int init_hid_device(char* device_name)
{
int cnt;
unsigned long key_bits[32],abs_bits[32];
unsigned long buttons,axes;
unsigned long key_bits[32],abs_bits[32],led_bits[32];
// unsigned long ff_bits[32];
int valbuf[16];
char name[256] = "Unknown";
char name[MAX_NAME_LENGTH] = "Unknown";
stick_button_count = 0;
stick_axis_count = 0;
/* Detect device mode, event interface or joystick */
if (strstr( device_name, "event" ) == 0 ) {
dbgprintf(stderr," Using Joystick interface\n");
event_mode = STICK_MODE_JOYSTICK;
} else {
dbgprintf(stderr," Using Event interface\n");
event_mode = STICK_MODE_EVENT;
}
/* Open event device read only (with write permission for ff) */
stick_device_handle = open(device_name,O_RDONLY|O_NONBLOCK);
if (stick_device_handle<0) {
@@ -98,12 +149,35 @@ int init_hid_device(char* device_name)
return(1);
}
/* LED testing */
// led_test( );
/* Which buttons has the device? */
memset(key_bits,0,32*sizeof(unsigned long));
if (ioctl(stick_device_handle,EVIOCGBIT(EV_KEY,32*sizeof(unsigned long)),key_bits)<0) {
dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
strerror(errno),__FILE__,__LINE__);
return(1);
memset(led_bits,0,32*sizeof(unsigned long));
if (event_mode == STICK_MODE_EVENT ) {
if (ioctl(stick_device_handle,EVIOCGBIT(EV_KEY,32*sizeof(unsigned long)),key_bits)<0) {
dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
strerror(errno),__FILE__,__LINE__);
return(1);
}
//read LED count
if (ioctl(stick_device_handle,EVIOCGLED(32*sizeof(led_bits)),led_bits)<0) {
dbgprintf(stderr,"ERROR: can not get LED bits (%s) [%s:%d]\n",
strerror(errno),__FILE__,__LINE__);
return(1);
}
dbgprintf(stderr,"LEDs: %X",*led_bits);
} else {
if (ioctl(stick_device_handle,JSIOCGBUTTONS,&buttons)<0) {
dbgprintf(stderr,"ERROR: can not get key bits (%s) [%s:%d]\n",
strerror(errno),__FILE__,__LINE__);
return(1);
}
}
/* Store buttons */
@@ -118,13 +192,22 @@ int init_hid_device(char* device_name)
memcpy(button_code,stick_init_param.button_code,BUTTON_COUNT*sizeof(int));
}
else {
for (cnt = MIN_BUTTON_CODE; cnt < MAX_BUTTON_CODE; cnt++) {
if (TEST_BIT(cnt, key_bits)) {
if (event_mode == STICK_MODE_EVENT) {
for (cnt = MIN_BUTTON_CODE; cnt < MAX_BUTTON_CODE; cnt++) {
if (TEST_BIT(cnt, key_bits)) {
button_code[stick_button_count++] = cnt;
dbgprintf(stderr,"Available button: %d (0x%x)\n",cnt,cnt);
}
if (stick_button_count == BUTTON_COUNT) break;
}
} else {
for (cnt = 0; cnt < buttons; cnt++) {
button_code[stick_button_count++] = cnt;
dbgprintf(stderr,"Available button: %d (0x%x)\n",cnt,cnt);
if (stick_button_count == BUTTON_COUNT) break;
}
if (stick_button_count == BUTTON_COUNT) break;
}
if (stick_button_count == 0) {
dbgprintf(stderr,"ERROR: no suitable buttons found [%s:%d]\n",__FILE__,__LINE__);
}
@@ -132,10 +215,18 @@ int init_hid_device(char* device_name)
/* Which axis has the device? */
memset(abs_bits,0,32*sizeof(unsigned long));
if (ioctl(stick_device_handle,EVIOCGBIT(EV_ABS,32*sizeof(unsigned long)),abs_bits)<0) {
dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
strerror(errno),__FILE__,__LINE__);
return(1);
if (event_mode == STICK_MODE_EVENT ) {
if (ioctl(stick_device_handle,EVIOCGBIT(EV_ABS,32*sizeof(unsigned long)),abs_bits)<0) {
dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
strerror(errno),__FILE__,__LINE__);
return(1);
}
} else {
if (ioctl(stick_device_handle,JSIOCGAXES,&axes)<0) {
dbgprintf(stderr,"ERROR: can not get abs bits (%s) [%s:%d]\n",
strerror(errno),__FILE__,__LINE__);
return(1);
}
}
/* Store axis */
@@ -150,13 +241,22 @@ int init_hid_device(char* device_name)
memcpy(axis_code,stick_init_param.axis_code,AXIS_COUNT*sizeof(int));
}
else {
for (cnt = MIN_ABS_CODE; cnt < MAX_ABS_CODE; cnt++) {
if (TEST_BIT(cnt, abs_bits)) {
if(event_mode == STICK_MODE_EVENT) {
for (cnt = MIN_ABS_CODE; cnt < MAX_ABS_CODE; cnt++) {
if (TEST_BIT(cnt, abs_bits)) {
axis_code[stick_axis_count++] = cnt;
dbgprintf(stderr,"Available axis: %d (0x%x)\n",cnt,cnt);
}
if (stick_axis_count == AXIS_COUNT) break;
}
} else {
for (cnt = 0; cnt < axes; cnt++) {
axis_code[stick_axis_count++] = cnt;
dbgprintf(stderr,"Available axis: %d (0x%x)\n",cnt,cnt);
if (stick_axis_count == AXIS_COUNT) break;
}
if (stick_axis_count == AXIS_COUNT) break;
}
// at least 2 axis are needed in auto detection
if (stick_axis_count < 2) {
dbgprintf(stderr,"ERROR: no suitable axis found [%s:%d]\n",__FILE__,__LINE__);
@@ -167,14 +267,21 @@ int init_hid_device(char* device_name)
/* Axis param */
for (cnt = 0; cnt < stick_axis_count; cnt++)
{
/* get axis value range */
if (ioctl(stick_device_handle,EVIOCGABS(axis_code[cnt]),valbuf)<0) {
dbgprintf(stderr,"ERROR: can not get axis %d value range (%s) [%s:%d]\n",
cnt,strerror(errno),__FILE__,__LINE__);
return(1);
}
axis_min[cnt]=valbuf[1];
axis_max[cnt]=valbuf[2];
if (event_mode == STICK_MODE_EVENT ) {
/* get axis value range */
if (ioctl(stick_device_handle,EVIOCGABS(axis_code[cnt]),valbuf)<0) {
dbgprintf(stderr,"ERROR: can not get axis %d value range (%s) [%s:%d]\n",
cnt,strerror(errno),__FILE__,__LINE__);
return(1);
}
axis_min[cnt]=valbuf[1];
axis_max[cnt]=valbuf[2];
} else {
// with joystick interface, all axes are signed 16 bit with full range
axis_min[cnt]=-32768;
axis_max[cnt]=32768;
}
if (axis_min[cnt]>=axis_max[cnt]) {
dbgprintf(stderr,"ERROR: bad axis %d value range (%d,%d) [%s:%d]\n",
cnt,axis_min[cnt],axis_max[cnt],__FILE__,__LINE__);
@@ -184,6 +291,9 @@ int init_hid_device(char* device_name)
cnt,axis_min[cnt],axis_max[cnt]);
}
//--------------------------------------------------
// force feedback, TBD feature
//--------------------------------------------------
#if 0
/* Now get some information about force feedback */
memset(ff_bits,0,32*sizeof(unsigned long));
@@ -245,7 +355,11 @@ int init_hid_device(char* device_name)
}
#endif
ioctl(stick_device_handle, EVIOCGNAME(sizeof(name)), name);
if (event_mode == STICK_MODE_EVENT ) {
ioctl(stick_device_handle, EVIOCGNAME(sizeof(name)), name);
} else {
ioctl(stick_device_handle, JSIOCGNAME(MAX_NAME_LENGTH), name);
}
printf("Input device name: \"%s\" on device \"%s\"\n", name, device_name);
return(0);
@@ -256,61 +370,79 @@ int stick_read( void ) {
int cnt;
struct input_event event;
struct js_event jsevent;
/* Get events */
while (read(stick_device_handle,&event,sizeof(event))==sizeof(event)) {
if (event_mode == STICK_MODE_EVENT ) {
/* Get events */
while (read(stick_device_handle,&event,sizeof(event))==sizeof(event)) {
switch (event.type) {
case EV_KEY:
for (cnt = 0; cnt < stick_button_count; cnt++) {
if (event.code == button_code[cnt]) {
if (event.value) stick_button_values |= (1 << cnt); // Set bit
else stick_button_values &= ~(1 << cnt); // Clear bit
break;
switch (event.type) {
case EV_KEY:
for (cnt = 0; cnt < stick_button_count; cnt++) {
if (event.code == button_code[cnt]) {
if (event.value) stick_button_values |= (1 << cnt); // Set bit
else stick_button_values &= ~(1 << cnt); // Clear bit
break;
}
}
}
break;
case EV_ABS:
for (cnt = 0; cnt < stick_axis_count; cnt++) {
if (event.code == axis_code[cnt]) {
stick_axis_values[cnt] = (((event.value) - axis_min[cnt]))*ABS_MAX_VALUE / (axis_max[cnt] - axis_min[cnt]) - ABS_MID_VALUE;
break;
break;
case EV_ABS:
for (cnt = 0; cnt < stick_axis_count; cnt++) {
if (event.code == axis_code[cnt]) {
stick_axis_values[cnt] = (((event.value) - axis_min[cnt]))*ABS_MAX_VALUE / (axis_max[cnt] - axis_min[cnt]) - ABS_MID_VALUE;
break;
}
}
}
break;
default: break;
break;
default: break;
}
}
} else {
/* Get joystick events */
while (read(stick_device_handle,&jsevent,sizeof(jsevent))==sizeof(jsevent)) {
switch (jsevent.type) {
case JS_EVENT_BUTTON:
for (cnt = 0; cnt < stick_button_count; cnt++) {
if (jsevent.number == button_code[cnt]) {
if (jsevent.value) stick_button_values |= (1 << cnt); // Set bit
else stick_button_values &= ~(1 << cnt); // Clear bit
break;
}
}
break;
case JS_EVENT_AXIS:
for (cnt = 0; cnt < stick_axis_count; cnt++) {
if (jsevent.number == axis_code[cnt]) {
stick_axis_values[cnt] = (( (jsevent.value - axis_min[cnt]) * ABS_MAX_VALUE ) / (axis_max[cnt] - axis_min[cnt])) - ABS_MID_VALUE;
break;
}
}
break;
default: break;
}
}
}
dbgprintf(stderr,"buttons %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d | ",
(stick_button_values >> 0) & 1,
(stick_button_values >> 1) & 1,
(stick_button_values >> 2) & 1,
(stick_button_values >> 3) & 1,
(stick_button_values >> 4) & 1,
(stick_button_values >> 5) & 1,
(stick_button_values >> 6) & 1,
(stick_button_values >> 7) & 1,
(stick_button_values >> 8) & 1,
(stick_button_values >> 9) & 1,
(stick_button_values >> 10) & 1,
(stick_button_values >> 11) & 1,
(stick_button_values >> 12) & 1,
(stick_button_values >> 13) & 1,
(stick_button_values >> 14) & 1,
(stick_button_values >> 15) & 1);
dbgprintf(stderr,"axis %d %d %d %d %d %d %d %d %d %d\n",
stick_axis_values[0],
stick_axis_values[1],
stick_axis_values[2],
stick_axis_values[3],
stick_axis_values[4],
stick_axis_values[5],
stick_axis_values[6],
stick_axis_values[7],
stick_axis_values[8],
stick_axis_values[9]);
if (errno != EAGAIN) {
printf("Joystick read error!\n");
exit(1);
}
dbgprintf(stderr,"buttons ");
for (cnt = 0; cnt < stick_button_count; cnt++) {
dbgprintf(stderr,"%d ",(stick_button_values >> cnt) & 1 );
}
dbgprintf(stderr,"| axes ");
for (cnt = 0; cnt < stick_axis_count; cnt++) {
dbgprintf(stderr,"%d ",stick_axis_values[cnt]);
}
dbgprintf(stderr,"\n");
return 0;
}
@@ -318,7 +450,7 @@ int stick_read( void ) {
int stick_init( char * device_name ) {
char devname[256];
char devname[MAX_NAME_LENGTH];
int cnt = 0;
/* test device_name, else look for a suitable device */
sw/ground_segment/joystick/usb_stick.h
+12 -3
View File
@@ -27,17 +27,26 @@
#include <linux/input.h>
/* Max number of axis and buttons */
#define STICK_BUTTON_COUNT 16
#define STICK_AXIS_COUNT 10
/* Increased, many new controllers have pressure sensitive buttons that show up as axes */
#define STICK_BUTTON_COUNT 32
#define STICK_AXIS_COUNT 32
/* Default values for the options */
#define STICK_INPUT_DEV_MAX 15
#define STICK_DEVICE_NAME "/dev/input/event"
/* Event mode switched */
#define STICK_MODE_UNKNOWN 0
#define STICK_MODE_EVENT 1
#define STICK_MODE_JOYSTICK 2
/* Max name length */
#define MAX_NAME_LENGTH 255
/* Global variables about the initialized device */
extern int stick_device_handle;
extern int8_t stick_axis_values[STICK_AXIS_COUNT];
extern int16_t stick_button_values;
extern int32_t stick_button_values;
extern int stick_axis_count, stick_button_count;
/* Structure for custom configuration
sw/ground_segment/misc/Makefile
+7
View File
@@ -0,0 +1,7 @@
all: davis2ivy
davis2ivy: davis2ivy.o
g++ -o davis2ivy davis2ivy.o -s -livy
%.o : %.c
gcc -c -O2 -Wall $<
sw/ground_segment/misc/davis2ivy.c
+269
View File
@@ -0,0 +1,269 @@
/*
* Paparazzi $Id$
*
* Copyright (C) 2011 Andreas Gaeb
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file davis2ivy.c
* \brief Connect a Davis VantagePro weather station to the Paparazzi system
*
* The program communicates with a Davis VantagePro(2) weather station connected
* to a serial port. It asks for new data (Davis' LOOP command) in the
* specified intervals, extracts the relevant data (ambient pressure and
* temperature, wind speed and direction) and broadcasts this via the Ivy bus.
*
* At the moment, the Ivy messages should be sent with the ID of the actually
* flying aircraft, which integrates them into the log file, as long as the
* aircraft sends its alive message.
*
* Useful links:
* - <a href="http://www.davisnet.com/weather/products/vantagepro.asp">Weather Stations</a>
* - <a href="http://www.davisnet.com/support/weather/download/VantageSerialProtocolDocs_v230.pdf">Communication docs</a>
*
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <signal.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyloop.h>
#include <Ivy/timer.h>
typedef enum { FALSE = 0, TRUE } BOOL;
#define PACKET_LENGTH 99
// global variables
int fd, ac_id = 1;
const char *device;
unsigned char packet[PACKET_LENGTH];
TimerId tid;
BOOL want_alive_msg = FALSE;
/// Handler for Ctrl-C, exits the main loop
void sigint_handler(int sig) {
IvyStop();
TimerRemove(tid);
close(fd);
}
/// open the serial port with the appropiate settings
void open_port(const char* device) {
fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY);
if (fd == -1) {
fprintf(stderr, "open_port: unable to open device %s - ", device);
perror(NULL);
exit(EXIT_FAILURE);
}
// setup connection options
struct termios options;
// get the current options
tcgetattr(fd, &options);
// set local mode, enable receiver, set comm. options:
// 8 data bits, 1 stop bit, no parity, 19200 Baud
options.c_cflag = CLOCAL | CREAD | CS8 | B19200;
// write options back to port
tcsetattr(fd, TCSANOW, &options);
}
/// disable transactions and empty queue
void reset_station() {
char newline = '\n', bytes = 0;
fprintf(stderr, "Resetting communication\n");
// send a \n (wakeup and cancel all running transmits)
bytes = write(fd, &newline, 1);
// read and discard everything that might be left in the queue
close(fd);
sleep(1);
open_port(device);
}
/// send a wakeup call to the station
BOOL wakeup(int tries) {
int loops = tries, bytes;
BOOL woken = FALSE;
char buf[] = {0, 0};
char newline = '\n';
do {
// send a \n
bytes = write(fd, &newline, 1);
// wait until station answers with \n\r
usleep(30000);
bytes = read(fd, buf, sizeof(buf));
woken = (buf[0] == 10) && (buf[1] == 13);
} while (!woken && loops-- > 0);
if (!woken) {
fprintf(stderr, "Could not wake up station: ");
if (bytes < 1) fprintf(stderr, "no bytes received\n");
else fprintf(stderr, "received %02x:%02x instead of \\n\\r\n", buf[0], buf[1]);
reset_station();
}
return woken;
}
/// send a LOOP command (read sensor data) to the station and get the packet back
BOOL send_loop() {
char msg[32], ack;
// TODO maybe ask for more packets?
snprintf(msg, sizeof(msg), "LOOP %i\n", 1);
int bytes = write(fd, msg, strlen(msg));
usleep(120000);
bytes = read(fd, &ack, 1);
if (bytes < 1 || ack != 0x06) {
fprintf(stderr, "Failed to receive ACK from station\n");
reset_station();
return FALSE;
}
bytes = read(fd, packet, PACKET_LENGTH);
if (bytes < PACKET_LENGTH) {
fprintf(stderr, "Received packet is incomplete, only %i of %i bytes\n",
bytes, PACKET_LENGTH);
reset_station();
return FALSE;
} else {
return TRUE;
}
}
/// get the relevant data from the packet and sent it as Ivy message
void decode_and_send_to_ivy() {
// check packet integrity
char expected[] = "LOO";
if (strncmp((char *)packet, expected, 3) != 0) {
fprintf(stderr, "Received packet from the weather station which does not match the expected format\n");
reset_station();
return;
}
// TODO CRC checking (is rather involved for the Davis protocol)
// get relevant data and convert to SI units
// see chapter IX.1 of the protocol definition
float
pstatic_Pa = (packet[7] | packet[8] << 8)*3.386388640341, // original is inches Hg / 1000
temp_degC = ((packet[12] | packet[13] << 8)/10.0 - 32.0)*5.0/9.0, // original is deg F / 10
windspeed_mps = packet[14]*0.44704, // original is miles per hour
winddir_deg = packet[16] | packet[17] << 8;
// TODO get the real MD5 for the aircraft id
if (want_alive_msg)
IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", ac_id);
// format has to match declaration in conf/messages.xml
IvySendMsg("%d WEATHER %f %f %f %f\n",
ac_id, pstatic_Pa, temp_degC, windspeed_mps, winddir_deg);
}
/// Get data from the station and send it via Ivy
/** This function is executed by the timer
*/
void handle_timer (TimerId id, void *data, unsigned long delta) {
if (wakeup(3) && send_loop()) decode_and_send_to_ivy();
}
void print_usage(int argc, char ** argv) {
fprintf(stderr, "Usage: %s [-a] [-b <bus>] [-d <device>] [-i <aircraft_id>] [-s <delay time in seconds>]\n",
argv[0]);
};
/// Main function
int main(int argc, char **argv) {
// default values for options
const char
*defaultbus = "127.255.255.255:2010",
*bus = defaultbus,
*defaultdevice = "/dev/ttyUSB1";
device = defaultdevice;
long delay = 1000;
// parse options
char c;
while ((c = getopt (argc, argv, "hab:d:i:s:")) != EOF) {
switch (c) {
case 'h':
print_usage(argc, argv);
exit(EXIT_SUCCESS);
break;
case 'a':
want_alive_msg = TRUE;
break;
case 'b':
bus = optarg;
break;
case 'd':
device = optarg;
break;
case 'i':
ac_id = atoi(optarg);
break;
case 's':
delay = atoi(optarg)*1000;
break;
case '?':
if (optopt == 'a' || optopt == 'b' || optopt == 'd' || optopt == 's')
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
else
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
print_usage(argc, argv);
exit(EXIT_FAILURE);
default:
abort ();
}
}
// make Ctrl-C stop the main loop and clean up properly
signal(SIGINT, sigint_handler);
bzero (packet, PACKET_LENGTH);
open_port(device);
// setup Ivy communication
IvyInit("davis2ivy", "READY", 0, 0, 0, 0);
IvyStart(bus);
// create timer
tid = TimerRepeatAfter (0, delay, handle_timer, 0);
IvyMainLoop();
return 0;
}
sw/ground_segment/misc/readme.txt
+3
View File
@@ -0,0 +1,3 @@
davis2ivy:
A wrapper to communicate with a Davis VantagePro/VantagePro2 weather
station and integrate weather data into the telemetry link.