mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-29 02:38:07 +08:00
Merge master
This commit is contained in:
Christophe De Wagter
+7
-2
@@ -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
|
||||
|
||||
@@ -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,6 +99,9 @@ cockpit: lib
|
||||
tmtc: lib cockpit
|
||||
cd $(TMTC); $(MAKE) all
|
||||
|
||||
misc:
|
||||
cd $(MISC); $(MAKE) all
|
||||
|
||||
multimon:
|
||||
cd $(MULTIMON); $(MAKE)
|
||||
|
||||
|
||||
+164
-124
@@ -1,68 +1,17 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<!-- TwinStart Multiplex
|
||||
Lisa/L board (http://paparazzi.enac.fr/wiki/Lisa)
|
||||
XBee modem
|
||||
LEA 5H GPS
|
||||
robbe R617
|
||||
<!-- Microjet Multiplex (http://www.multiplex-rc.de/)
|
||||
Tiny 1.1 board (http://paparazzi.enac.fr/wiki/Tiny)
|
||||
Tilted infrared sensor
|
||||
Xbee modem in API mode
|
||||
-->
|
||||
|
||||
<airframe name="TwinStar CHIMU Lisa/L">
|
||||
|
||||
<firmware name="fixedwing">
|
||||
<target name="sim" board="pc"/>
|
||||
<target name="ap" board="lisa_l_1.0"/>
|
||||
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
<subsystem name="telemetry" type="transparent"/>
|
||||
<subsystem name="control"/>
|
||||
<subsystem name="gps" type="ublox_lea5h">
|
||||
<configure name="GPS_PORT" value="UART3"/>
|
||||
</subsystem>
|
||||
<subsystem name="navigation"/>
|
||||
</firmware>
|
||||
|
||||
<firmware name="lisa_l_test_progs">
|
||||
<target name="tunnel" board="lisa_l_1.0"/>
|
||||
<target name="test_led" board="lisa_l_1.0"/>
|
||||
<target name="test_uart" board="lisa_l_1.0"/>
|
||||
<target name="test_servos" board="lisa_l_1.0"/>
|
||||
<target name="test_telemetry" board="lisa_l_1.0"/>
|
||||
<target name="test_baro" board="lisa_l_1.0"/>
|
||||
<target name="test_imu_b2" board="lisa_l_1.0"/>
|
||||
<target name="test_imu_b2_2" board="lisa_l_1.0"/>
|
||||
<target name="test_imu_aspirin" board="lisa_l_1.0"/>
|
||||
<target name="test_rc_spektrum" board="lisa_l_1.0"/>
|
||||
<target name="test_rc_ppm" board="lisa_l_1.0"/>
|
||||
<target name="test_adc" board="lisa_l_1.0"/>
|
||||
<target name="test_hmc5843" board="lisa_l_1.0"/>
|
||||
<target name="test_itg3200" board="lisa_l_1.0"/>
|
||||
<target name="test_adxl345" board="lisa_l_1.0"/>
|
||||
<target name="test_esc_mkk_simple" board="lisa_l_1.0"/>
|
||||
<target name="test_esc_asctecv1_simple" board="lisa_l_1.0"/>
|
||||
<target name="test_actuators_mkk" board="lisa_l_1.0"/>
|
||||
<target name="test_actuators_asctecv1" board="lisa_l_1.0"/>
|
||||
</firmware>
|
||||
|
||||
|
||||
<modules>
|
||||
<load name="ins_chimu_uart.xml">
|
||||
<configure name="CHIMU_UART_NR" value="1"/>
|
||||
</load>
|
||||
|
||||
<load name="light.xml">
|
||||
<define name="LIGHT_LED_STROBE" value="3"/>
|
||||
<define name="LIGHT_LED_NAV" value="4"/>
|
||||
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
|
||||
<define name="NAV_LIGHT_MODE_DEFAULT" value="0"/>
|
||||
</load>
|
||||
</modules>
|
||||
|
||||
<airframe name="Microjet Tiny 1.1">
|
||||
|
||||
<servos>
|
||||
<servo name="MOTOR" no="0" min="1000" neutral="1000" max="2000"/>
|
||||
<servo name="AILEVON_LEFT" no="1" min="1900" neutral="1534" max="1100"/>
|
||||
<servo name="AILEVON_RIGHT" no="2" min="1100" neutral="1468" max="1900"/>
|
||||
<servo name="MOTOR" no="0" min="1290" neutral="1290" max="1810"/>
|
||||
<servo name="AILEVON_LEFT" no="1" min="2000" neutral="1510" max="1000"/>
|
||||
<servo name="AILEVON_RIGHT" no="2" min="1000" neutral="1535" max="2000"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
@@ -78,21 +27,143 @@
|
||||
</rc_commands>
|
||||
|
||||
<section name="MIXER">
|
||||
<define name="AILEVON_AILERON_RATE" value="0.45"/>
|
||||
<define name="AILEVON_ELEVATOR_RATE" value="0.8"/>
|
||||
<define name="AILEVON_AILERON_RATE" value="0.75"/>
|
||||
<define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
|
||||
</section>
|
||||
|
||||
<command_laws>
|
||||
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
|
||||
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
|
||||
<set servo="MOTOR" value="@THROTTLE"/>
|
||||
<set servo="MOTOR" value="@THROTTLE"/>
|
||||
<set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
|
||||
<set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
<define name="MAX_ROLL" value="RadOfDeg(50)"/>
|
||||
<define name="MAX_PITCH" value="RadOfDeg(35)"/>
|
||||
<define name="MAX_ROLL" value="0.7"/>
|
||||
<define name="MAX_PITCH" value="0.6"/>
|
||||
</section>
|
||||
|
||||
<section name="INFRARED" prefix="IR_">
|
||||
<define name="ADC_IR1_NEUTRAL" value="2048"/>
|
||||
<define name="ADC_IR2_NEUTRAL" value="2048"/>
|
||||
<define name="ADC_TOP_NEUTRAL" value="2048"/>
|
||||
|
||||
<define name="LATERAL_CORRECTION" value="1."/>
|
||||
<define name="LONGITUDINAL_CORRECTION" value="1."/>
|
||||
<define name="VERTICAL_CORRECTION" value="1.5"/>
|
||||
|
||||
<define name="HORIZ_SENSOR_TILTED" value="1"/>
|
||||
<define name="IR2_SIGN" value="-1"/>
|
||||
<define name="TOP_SIGN" value="-1"/>
|
||||
|
||||
<define name="ROLL_NEUTRAL_DEFAULT" value="-3.6" unit="deg"/>
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="5" unit="deg"/>
|
||||
|
||||
<define name="CORRECTION_UP" value="1."/>
|
||||
<define name="CORRECTION_DOWN" value="1."/>
|
||||
<define name="CORRECTION_LEFT" value="1."/>
|
||||
<define name="CORRECTION_RIGHT" value="1."/>
|
||||
</section>
|
||||
|
||||
<section name="GYRO" prefix="GYRO_">
|
||||
<define name="ADC_ROLL_NEUTRAL" value="500"/>
|
||||
<define name="DYNAMIC_RANGE" value="300" unit="deg/s"/>
|
||||
<define name="ADXRS300_RESISTOR_BRIDGE" value="(3.3/(3.3+1.8))"/>
|
||||
<define name="ADXRS300_SENSITIVITY" value="5" unit="mV/(deg/s)"/>
|
||||
<define name="ROLL_SCALE" value="3.3*1000./1024./(GYRO_ADXRS300_SENSITIVITY*GYRO_ADXRS300_RESISTOR_BRIDGE)" unit="deg/s/adc_unit"/>
|
||||
<define name="ROLL_DIRECTION" value="1."/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
|
||||
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="NOMINAL_AIRSPEED" value="13." unit="m/s"/>
|
||||
<define name="CARROT" value="5." 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\rATRN5\rATTT80\r""/>
|
||||
<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
|
||||
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
|
||||
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||
|
||||
<define name="GLIDE_AIRSPEED" value="10"/>
|
||||
<define name="GLIDE_VSPEED" value="3."/>
|
||||
<define name="GLIDE_PITCH" value="45" unit="deg"/>
|
||||
</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.03"/>
|
||||
<!-- outer loop saturation -->
|
||||
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
|
||||
|
||||
<!-- auto throttle inner loop -->
|
||||
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.32"/>
|
||||
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.25"/>
|
||||
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.65"/>
|
||||
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
|
||||
<define name="AUTO_THROTTLE_DASH_TRIM" value="-4000"/>
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="-0.01"/>
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
|
||||
|
||||
<define name="THROTTLE_SLEW_LIMITER" value="2" unit="s"/>
|
||||
</section>
|
||||
|
||||
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||
<define name="COURSE_PGAIN" value="-1.4"/>
|
||||
|
||||
<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="PITCH_PGAIN" value="-12000."/>
|
||||
<define name="PITCH_DGAIN" value="1.5"/>
|
||||
|
||||
<define name="ELEVATOR_OF_ROLL" value="1250"/>
|
||||
|
||||
<define name="ROLL_SLEW" value="0.1"/>
|
||||
|
||||
<define name="ROLL_ATTITUDE_GAIN" value="-7500"/>
|
||||
<define name="ROLL_RATE_GAIN" value="-1500"/>
|
||||
</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="1.00"/><!-- 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="2" 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"/>
|
||||
</section>
|
||||
|
||||
<section name="DATALINK" prefix="DATALINK_">
|
||||
<define name="DEVICE_TYPE" value="XBEE"/>
|
||||
<define name="DEVICE_ADDRESS" value="...."/>
|
||||
</section>
|
||||
|
||||
|
||||
<section name="SIMU">
|
||||
<define name="JSBSIM_MODEL" value=""Malolo1""/>
|
||||
<define name="JSBSIM_IR_ROLL_NEUTRAL" value="RadOfDeg(0.)"/>
|
||||
<define name="JSBSIM_IR_PITCH_NEUTRAL" value="RadOfDeg(0.)"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
@@ -100,73 +171,42 @@
|
||||
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
|
||||
</section>
|
||||
<modules>
|
||||
<load name="ins_chimu_uart.xml">
|
||||
<configure name="CHIMU_UART_NR" value="3"/>
|
||||
</load>
|
||||
</modules>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="NOMINAL_AIRSPEED" value="15." unit="m/s"/>
|
||||
<define name="CARROT" value="5." unit="s"/>
|
||||
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
|
||||
<define name="CONTROL_RATE" value="60" unit="Hz"/>
|
||||
<firmware name="fixedwing">
|
||||
<target name="ap" board="lisa_l_1.0"/>
|
||||
<target name="sim" board="pc" />
|
||||
<target name="jsbsim" board="pc"/>
|
||||
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="120."/>
|
||||
</section>
|
||||
<define name="AGR_CLIMB" />
|
||||
<define name="LOITER_TRIM" />
|
||||
<define name="ALT_KALMAN" />
|
||||
|
||||
<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.06"/> <!-- -0.024 -->
|
||||
<!-- outer loop saturation -->
|
||||
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
|
||||
<!-- auto throttle inner loop -->
|
||||
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.45"/>
|
||||
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.35"/>
|
||||
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.85"/>
|
||||
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.2" unit="%/(m/s)"/>
|
||||
<define name="AUTO_THROTTLE_PGAIN" value="-0.023"/> <!-- -0.012 -->
|
||||
<define name="AUTO_THROTTLE_IGAIN" value="0.01"/>
|
||||
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
|
||||
<!-- Communication -->
|
||||
<subsystem name="telemetry" type="transparent">
|
||||
<configure name="MODEM_BAUD" value="B9600"/>
|
||||
</subsystem>
|
||||
|
||||
<!-- auto pitch inner loop -->
|
||||
<define name="AUTO_PITCH_PGAIN" value="-0.06"/> <!-- -0.03 -->
|
||||
<define name="AUTO_PITCH_IGAIN" value="0.0"/>
|
||||
<define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
|
||||
<define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
|
||||
<subsystem name="control"/>
|
||||
<!-- Sensors
|
||||
<subsystem name="gyro" type="roll"/> -->
|
||||
<subsystem name="gps" type="ublox_lea4p"/>
|
||||
<subsystem name="navigation"/>
|
||||
|
||||
<define name="THROTTLE_SLEW" value="0.1"/>
|
||||
</section>
|
||||
</firmware>
|
||||
|
||||
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||
<define name="COURSE_PGAIN" value="-0.9"/>
|
||||
<define name="ROLL_MAX_SETPOINT" value="0.70" unit="radians"/> <!-- 0.5 -->
|
||||
<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="6600."/>
|
||||
<define name="AILERON_OF_THROTTLE" value="0.0"/>
|
||||
<define name="PITCH_PGAIN" value="-5500."/>
|
||||
<define name="PITCH_DGAIN" value="0.4"/>
|
||||
|
||||
<define name="ELEVATOR_OF_ROLL" value="2400"/>
|
||||
</section>
|
||||
|
||||
<section name="NAV">
|
||||
<define name="NAV_PITCH" value="0."/>
|
||||
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="FAILSAFE" prefix="FAILSAFE_">
|
||||
<define name="DELAY_WITHOUT_GPS" value="2" 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="SIMU">
|
||||
<define name="YAW_RESPONSE_FACTOR" value="0.5"/>
|
||||
</section>
|
||||
<firmware name="setup">
|
||||
<target name="tunnel" board="tiny_1.1" />
|
||||
<target name="usb_tunnel_0" board="tiny_1.1" />
|
||||
<target name="usb_tunnel_1" board="tiny_1.1" />
|
||||
<target name="setup_actuators" board="tiny_1.1" />
|
||||
</firmware>
|
||||
|
||||
</airframe>
|
||||
|
||||
@@ -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>
|
||||
|
||||
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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">
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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>
|
||||
|
||||
|
||||
|
||||
@@ -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>
|
||||
+16
-2
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
@@ -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>
|
||||
+17
-3
@@ -443,7 +443,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"/>
|
||||
@@ -1325,7 +1325,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 -->
|
||||
|
||||
@@ -1596,7 +1596,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"/>
|
||||
@@ -1942,6 +1947,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"/>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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"/>
|
||||
|
||||
@@ -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="subsystems/gps" 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>
|
||||
|
||||
@@ -187,11 +187,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 */
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
+9
-10
@@ -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;
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
|
||||
@@ -33,8 +33,8 @@ volatile uint8_t new_ins_attitude;
|
||||
|
||||
void ins_init( void )
|
||||
{
|
||||
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0xab, 0x76 }; // 50Hz attitude only + SPI
|
||||
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 25Hz attitude only + SPI
|
||||
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0xab, 0x76 }; // 50Hz attitude only + SPI
|
||||
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 25Hz attitude only + SPI
|
||||
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
|
||||
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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); \
|
||||
|
||||
@@ -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.
|
||||
|
||||
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 */
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -0,0 +1,7 @@
|
||||
all: davis2ivy
|
||||
|
||||
davis2ivy: davis2ivy.o
|
||||
g++ -o davis2ivy davis2ivy.o -s -livy
|
||||
|
||||
%.o : %.c
|
||||
gcc -c -O2 -Wall $<
|
||||
@@ -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;
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
davis2ivy:
|
||||
A wrapper to communicate with a Davis VantagePro/VantagePro2 weather
|
||||
station and integrate weather data into the telemetry link.
|
||||
Reference in New Issue
Block a user