Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 04:46:51 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Merge fixes and cleanup from branch 'v3.9'

Browse Source 
* a bit airframe and subsystem cleanup
* some usability improvements for the calibration utility
* fixes for nps simulator
* cleanup of ahrs float_dcm
This commit is contained in:
Felix Ruess
2012-06-04 13:16:24 +02:00
parent 13a5fc5c3a 6b79d6af64
commit 7290c97b37
48 changed files with 1015 additions and 257 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/Makefile.lpc21
+1 -4
View File
@@ -144,7 +144,7 @@ MATH_LIB = -lm
# Linker flags.
LDFLAGS = -n -nostartfiles -Wl,-Map=$(OBJDIR)/$(TARGET).map,--cref
LDFLAGS += -lc
LDFLAGS += $(NEWLIBLPC) $(MATH_LIB)
LDFLAGS += $(MATH_LIB)
LDFLAGS += -lc -lgcc
LDFLAGS += $(CPLUSPLUS_LIB)
LDFLAGS += -Wl,--gc-sections
@@ -153,14 +153,11 @@ LDFLAGS += -Wl,--gc-sections
ifndef LDSCRIPT
ifeq ($(FLASH_MODE),ISP)
LDSCRIPT = $(SRC_ARCH)/LPC2148-ROM.ld
else ifeq ($(FLASH_MODE),ISP)
LDSCRIPT = $(SRC_ARCH)/LPC2148-ROM.ld
else
LDSCRIPT = $(SRC_ARCH)/LPC2148-ROM-bl.ld
endif
endif
LDFLAGS +=-T$(LDSCRIPT)
#endif
conf/airframes/CDW/ChimuJTinyFwSpi.xml
-4
View File
@@ -1,9 +1,5 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
YAPA + XSens + XBee
-->
<airframe name="Yapa v1">
<servos>
conf/airframes/CDW/ChimuTinyFw.xml
-4
View File
@@ -1,9 +1,5 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
YAPA + XSens + XBee
-->
<airframe name="Yapa v1">
<servos>
conf/airframes/CDW/ChimuTinyFwSpi.xml
-3
View File
@@ -1,8 +1,5 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
YAPA + XSens + XBee
-->
<airframe name="Yapa v1">
conf/airframes/CDW/debug_i2c_lpc.xml
+4 -9
View File
@@ -1,7 +1,7 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
YAPA + XSens + XBee
Tiny + XSens + XBee
-->
<airframe name="Yapa v1">
@@ -177,12 +177,7 @@
<modules>
<load name="ins_xsens_MTiG_fixedwing.xml">
<configure name="XSENS_UART_NR" value="0"/>
</load>
<load name="i2c_abuse_test.xml"/>
</modules>
<firmware name="fixedwing">
@@ -207,9 +202,9 @@
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="navigation"/>
<subsystem name="gps" type="xsens"/>
<subsystem name="ins" type="xsens">
<configure name="XSENS_UART_NR" value="0"/>
</subsystem>
</firmware>
</airframe>
conf/airframes/CDW/yapa3_aspirin2.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
YAPA + XSens + XBee
Tiny2 + Aspirin2 + XBee
-->
<airframe name="Yapa v1">
conf/airframes/PPZUAV/fixed-wing/ppzimu_tiny.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">
<!--
YAPA + XSens + XBee
YAPA + PPZIMU + XBee
-->
<airframe name="Yapa v1">
conf/airframes/examples/microjet_lisa_m.xml
+176
View File
@@ -0,0 +1,176 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- Microjet Multiplex (http://www.multiplex-rc.de/)
Lisa/M v1.0 board (http://paparazzi.enac.fr/wiki/Lisa/M_v10)
Aspirin IMU v1.5
Xbee modem in transparent mode
-->
<airframe name="Microjet Lisa/M 1.0">
<firmware name="fixedwing">
<target name="sim" board="pc"/>
<target name="ap" board="lisa_m_1.0"/>
<define name="AGR_CLIMB" />
<define name="LOITER_TRIM" />
<define name="ALT_KALMAN" />
<subsystem name="radio_control" type="ppm"/>
<!-- Communication -->
<subsystem name="telemetry" type="transparent"/>
<!-- Sensors -->
<subsystem name="imu" type="aspirin_v1.5"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="control"/>
<subsystem name="navigation"/>
</firmware>
<modules>
<load name="sys_mon.xml"/>
</modules>
<servos>
<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="3" min="1000" neutral="1535" max="2000"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
</rc_commands>
<section name="MIXER">
<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="AILEVON_LEFT" value="$elevator - $aileron"/>
<set servo="AILEVON_RIGHT" value="$elevator + $aileron"/>
</command_laws>
<section name="IMU" prefix="IMU_">
<!-- ACCEL and GYRO calibration left out to take default datasheet values -->
<!-- replace this with your own mag calibration -->
<define name="MAG_X_NEUTRAL" value="-45"/>
<define name="MAG_Y_NEUTRAL" value="334"/>
<define name="MAG_Z_NEUTRAL" value="7"/>
<define name="MAG_X_SENS" value="4.47647816128" integer="16"/>
<define name="MAG_Y_SENS" value="4.71085671542" integer="16"/>
<define name="MAG_Z_SENS" value="4.41585354498" integer="16"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="AHRS" prefix="AHRS_">
<!-- replace this with your local magnetic field -->
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
</section>
<section name="INS" prefix="INS_">
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0" 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.0"/>
<define name="COURSE_DGAIN" value="0.3"/>
<define name="ROLL_MAX_SETPOINT" value="35" unit="deg"/>
<define name="PITCH_MAX_SETPOINT" value="30" unit="deg"/>
<define name="PITCH_MIN_SETPOINT" value="-30" unit="deg"/>
<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="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="40." unit="deg"/>
<define name="MAX_PITCH" value="35." unit="deg"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="2000"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" 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="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
</section>
</airframe>
conf/airframes/examples/quadrotor_lisa_m_2_pwm.xml
+6 -24
View File
@@ -5,19 +5,15 @@
<airframe name="fraser">
<firmware name="rotorcraft">
<target name="ap" board="lisa_m_2.0">
<subsystem name="actuators" type="pwm_supervision">
<define name="SERVO_HZ" value="400"/>
</subsystem>
</target>
<target name="sim" board="pc">
<subsystem name="fdm" type="nps"/>
<subsystem name="actuators" type="mkk"/>
</target>
<target name="ap" board="lisa_m_2.0"/>
<subsystem name="actuators" type="pwm_supervision">
<define name="SERVO_HZ" value="400"/>
</subsystem>
<subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/>
<subsystem name="imu" type="aspirin_v2.0"/>
<subsystem name="imu" type="aspirin_v2.1"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
@@ -42,13 +38,6 @@
<!-- but can be empty if no additional servos are used -->
</command_laws>
<!-- only for the nps sim -->
<section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
<define name="NB" value="4"/>
<!-- FRONT, BACK, RIGHT, LEFT -->
<define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
</section>
<section name="SUPERVISION" prefix="SUPERVISION_">
<define name="STOP_MOTOR" value="1000"/>
<define name="MIN_MOTOR" value="1100"/>
@@ -84,7 +73,6 @@
</section>
<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_FREQUENCY" value="512"/>
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
@@ -186,12 +174,6 @@
<define name="IGAIN" value="0"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_aspirin.h&quot;"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
conf/airframes/examples/quadrotor_lisa_m_mkk.xml
+1 -2
View File
@@ -67,7 +67,6 @@
</section>
<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_FREQUENCY" value="512"/>
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
@@ -172,7 +171,7 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_aspirin.h&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_aspirin_1.5.h&quot;"/>
</section>
<section name="AUTOPILOT">
conf/airframes/fraser_lisa_m_rotorcraft.xml
+1 -2
View File
@@ -101,7 +101,6 @@
</section>
<section name="AHRS" prefix="AHRS_">
<define name="PROPAGATE_FREQUENCY" value="512"/>
<define name="H_X" value="0.3770441"/>
<define name="H_Y" value="0.0193986"/>
<define name="H_Z" value="0.9259921"/>
@@ -208,7 +207,7 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_aspirin.h&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_aspirin_1.5.h&quot;"/>
</section>
<section name="AUTOPILOT">
conf/firmwares/subsystems/fixedwing/ahrs_float_dcm.makefile
+1 -1
View File
@@ -10,7 +10,7 @@ ifeq ($(TARGET), ap)
ap.CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
ap.CFLAGS += -DUSE_AHRS_ALIGNER
ap.CFLAGS += -DUSE_AHRS
ap.CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
ifneq ($(USE_MAGNETOMETER),0)
ap.CFLAGS += -DUSE_MAGNETOMETER
conf/firmwares/subsystems/fixedwing/ahrs_int_cmpl_euler.makefile
+1 -1
View File
@@ -7,7 +7,7 @@ ifndef USE_MAGNETOMETER
USE_MAGNETOMETER = 1
endif
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR -DUSE_AHRS_CMPL
AHRS_CFLAGS = -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR -DUSE_AHRS_CMPL_EULER
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
ifneq ($(USE_MAGNETOMETER),0)
conf/firmwares/subsystems/fixedwing/imu_aspirin2_i2c.makefile
-3
View File
@@ -18,6 +18,3 @@ endif
ap.CFLAGS += $(IMU_ASPIRIN2_CFLAGS)
ap.srcs += $(IMU_ASPIRIN2_SRCS)
ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY
conf/firmwares/subsystems/fixedwing/imu_ppzuav.makefile
-3
View File
@@ -18,6 +18,3 @@ endif
ap.CFLAGS += $(IMU_PPZUAV_CFLAGS)
ap.srcs += $(IMU_PPZUAV_SRCS)
ap.CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY
conf/firmwares/subsystems/lisa_passthrough/ahrs_cmpl.makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# Complementary filter for attitude estimation
#
stm_passthrough.CFLAGS += -DUSE_AHRS_CMPL
stm_passthrough.CFLAGS += -DUSE_AHRS_CMPL_EULER
ifneq ($(AHRS_ALIGNER_LED),none)
stm_passthrough.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
conf/firmwares/subsystems/rotorcraft/actuators_asctec.makefile
+1 -1
View File
@@ -14,5 +14,5 @@ endif
# Simulator
sim.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
sim.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=150 -DI2C0_SCLH=150 -DI2C0_VIC_SLOT=11
sim.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_DEVICE=i2c0
conf/firmwares/subsystems/rotorcraft/actuators_asctec_v2.makefile
+5
View File
@@ -29,3 +29,8 @@ ifeq ($(ARCH), stm32)
ap.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
ap.CFLAGS += -DUSE_I2C1
endif
# Simulator
sim.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
sim.srcs += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
sim.CFLAGS += -DUSE_I2C0 -DACTUATORS_ASCTEC_DEVICE=i2c0
conf/firmwares/subsystems/rotorcraft/actuators_mkk.makefile
+6 -4
View File
@@ -33,17 +33,19 @@
#
#
ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
ifeq ($(ARCH), lpc21)
# set default i2c timing if not already configured
ifeq ($(MKK_I2C_SCL_TIME), )
MKK_I2C_SCL_TIME=150
endif
ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c0
ap.CFLAGS += -DUSE_I2C0 -DI2C0_SCLL=$(MKK_I2C_SCL_TIME) -DI2C0_SCLH=$(MKK_I2C_SCL_TIME) -DI2C0_VIC_SLOT=11
else ifeq ($(ARCH), stm32)
ap.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1
ap.CFLAGS += -DUSE_I2C1
conf/firmwares/subsystems/rotorcraft/actuators_pwm_supervision.makefile
+6 -1
View File
@@ -1,7 +1,12 @@
# add actuatos arch to include directories
ap.CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH)
$(TARGET).CFLAGS += -I$(SRC_FIRMWARE)/actuators/arch/$(ARCH)
ap.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
ap.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm_supervision.c
ap.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c
# Simulator
sim.srcs += $(SRC_FIRMWARE)/actuators/supervision.c
sim.srcs += $(SRC_FIRMWARE)/actuators/actuators_pwm_supervision.c
sim.srcs += $(SRC_FIRMWARE)/actuators/arch/$(ARCH)/actuators_pwm_arch.c
conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_euler.makefile
+1 -1
View File
@@ -7,7 +7,7 @@ ifndef USE_MAGNETOMETER
USE_MAGNETOMETER = 1
endif
AHRS_CFLAGS = -DUSE_AHRS -DUSE_AHRS_CMPL
AHRS_CFLAGS = -DUSE_AHRS -DUSE_AHRS_CMPL_EULER
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
ifneq ($(USE_MAGNETOMETER),0)
conf/firmwares/subsystems/shared/imu_aspirin_common.makefile
+1 -1
View File
@@ -52,7 +52,7 @@ IMU_ASPIRIN_SRCS += peripherals/hmc5843.c $(SRC_ARCH)/peripherals/hmc5843_arch
IMU_ASPIRIN_CFLAGS += -DUSE_I2C2
ifeq ($(ARCH), lpc21)
#TODO
$(error The aspirin subsystem (using SPI) is currently not implemnented for the lpc21. Please use the aspirin_i2c subsystem.)
else ifeq ($(ARCH), stm32)
IMU_ASPIRIN_CFLAGS += -DUSE_EXTI15_10_IRQ # Gyro Int on PC14
IMU_ASPIRIN_CFLAGS += -DUSE_EXTI9_5_IRQ # Mag Int on PB5
conf/messages.xml
+10 -9
View File
@@ -1048,6 +1048,7 @@
<field name="gr" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="noise" type="int32"/>
<field name="cnt" type="int32"/>
<field name="status" type="uint8" values="UNINIT|RUNNING|LOCKED"/>
</message>
<!-- 138 is free -->
@@ -1643,7 +1644,7 @@
<field name="br_err" type="float" unit="rad/s" alt_unit="degres/s" alt_unit_coef="57.29578" />
</message>
<message name="BOOZ_SIM_SENSORS_SCALED" id="197">
<message name="NPS_SENSORS_SCALED" id="197">
<field name="acc_x" type="float" />
<field name="acc_y" type="float" />
<field name="acc_z" type="float" />
@@ -1815,7 +1816,7 @@
<!--236 is free -->
<!--237 is free -->
<message name="BOOZ_SIM_POS_LLH" id="238">
<message name="NPS_POS_LLH" id="238">
<field name="pprz_lat" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="lat_geod" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
<field name="lat_geoc" type="float" unit="rad" alt_unit="deg" alt_unit_coef="57.29578"/>
@@ -1827,14 +1828,14 @@
<field name="asl" type="float" unit="m"/>
</message>
<message name="BOOZ_SIM_RPMS" id="239">
<message name="NPS_RPMS" id="239">
<field name="front" type="float" unit="RPM"/>
<field name="back" type="float" unit="RPM"/>
<field name="right" type="float" unit="RPM"/>
<field name="left" type="float" unit="RPM"/>
</message>
<message name="BOOZ_SIM_SPEED_POS" id="240">
<message name="NPS_SPEED_POS" id="240">
<field name="ltpp_xdd" type="float" unit="m/s2"/>
<field name="ltpp_ydd" type="float" unit="m/s2"/>
<field name="ltpp_zdd" type="float" unit="m/s2"/>
@@ -1846,7 +1847,7 @@
<field name="ltpp_z" type="float" unit="m"/>
</message>
<message name="BOOZ_SIM_RATE_ATTITUDE" id="241">
<message name="NPS_RATE_ATTITUDE" id="241">
<field name="p" type="float" unit="degres/s"/>
<field name="q" type="float" unit="degres/s"/>
<field name="r" type="float" unit="degres/s"/>
@@ -1855,18 +1856,18 @@
<field name="psi" type="float" unit="degres"/>
</message>
<message name="BOOZ_SIM_GYRO_BIAS" id="242">
<message name="NPS_GYRO_BIAS" id="242">
<field name="bp" type="float" unit="degres/s"/>
<field name="bq" type="float" unit="degres/s"/>
<field name="br" type="float" unit="degres/s"/>
</message>
<message name="BOOZ_SIM_RANGE_METER" id="243">
<message name="NPS_RANGE_METER" id="243">
<field name="dist" type="float" unit="adc"/>
</message>
<message name="BOOZ_SIM_WIND" id="244">
<message name="NPS_WIND" id="244">
<field name="vx" type="float" unit="m/s"/>
<field name="vy" type="float" unit="m/s"/>
<field name="vz" type="float" unit="m/s"/>
@@ -1885,7 +1886,7 @@
<field name="errno" type="uint8"/>
</message>
<message name="BOOZ_SIM_ACCEL_LTP" id="248">
<message name="NPS_ACCEL_LTP" id="248">
<field name="xdd" type="float" unit="m/s2"/>
<field name="ydd" type="float" unit="m/s2"/>
<field name="zdd" type="float" unit="m/s2"/>
conf/settings/control/ctl_new_airspeed.xml
+21 -19
View File
@@ -12,12 +12,12 @@
</dl_settings>
<dl_settings NAME="attitude">
<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" 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" param="H_CTL_PITCH_IGAIN" handler="SetPitchIGain"/>
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN"/>
<dl_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_roll_igain" shortname="roll_igain" param="H_CTL_ROLL_IGAIN" handler="SetRollIGain"/>
<dl_setting MAX="25000" MIN="0" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain" param="H_CTL_PITCH_PGAIN"/>
<dl_setting MAX="50000" MIN="0" STEP="250" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN"/>
<dl_setting MAX="5000" MIN="0" 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"/>
@@ -26,14 +26,14 @@
</dl_settings>
<dl_settings NAME="feedforward">
<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_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_roll_Kffa" shortname="roll_Kffa" param="H_CTL_ROLL_KFFA"/>
<dl_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_roll_Kffd" shortname="roll_Kffd" param="H_CTL_ROLL_KFFD"/>
<dl_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_pitch_Kffa" shortname="pitch_Kffa" param="H_CTL_PITCH_KFFA"/>
<dl_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_pitch_Kffd" shortname="pitch_Kffd" param="H_CTL_PITCH_KFFD"/>
</dl_settings>
<dl_settings name="alt">
<dl_setting MAX="0" MIN="-0.2" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain" param="V_CTL_ALTITUDE_PGAIN"/>
<dl_setting MAX="0.2" MIN="0" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain" param="V_CTL_ALTITUDE_PGAIN"/>
</dl_settings>
<dl_settings name="climb">
@@ -42,15 +42,17 @@
<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_min_cruise_throttle" shortname="min cruise thr" module="guidance/guidance_v" param="V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE"/>
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_max_cruise_throttle" shortname="max cruise thr" module="guidance/guidance_v" param="V_CTL_AUTO_THROTTLE_MAX_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="guidance/guidance_v_n"/>
<dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_pgain" shortname="pitch_p" param="V_CTL_AUTO_PITCH_PGAIN"/>
<dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_igain" shortname="pitch_i" param="V_CTL_AUTO_PITCH_IGAIN"/>
<dl_setting MAX="0.1" MIN="0" 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"/>
<dl_setting MAX="0" MIN="-0.1" STEP="0.001" VAR="v_ctl_auto_throttle_dgain" shortname="throttle_d" param="V_CTL_AUTO_THROTTLE_DGAIN"/>
<dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_p" param="V_CTL_AUTO_THROTTLE_PGAIN"/>
<dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_throttle_igain" shortname="throttle_i" param="V_CTL_AUTO_THROTTLE_IGAIN"/>
<dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_throttle_dgain" shortname="throttle_d" param="V_CTL_AUTO_THROTTLE_DGAIN"/>
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr" param="V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT"/>
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_pitch_of_vz_pgain" shortname="pitch_of_vz" param="V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN"/>
@@ -70,7 +72,7 @@
</dl_settings>
<dl_settings name="nav">
<dl_setting MAX="-0.1" MIN="-3" STEP="0.05" VAR="h_ctl_course_pgain" shortname="course pgain" param="H_CTL_COURSE_PGAIN"/>
<dl_setting MAX="3" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pgain" shortname="course pgain" param="H_CTL_COURSE_PGAIN"/>
<dl_setting MAX="2" MIN="0" STEP="0.1" VAR="h_ctl_course_dgain" shortname="course dgain"/>
<dl_setting MAX="2" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pre_bank_correction" shortname="pre bank cor"/>
<dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="nav_glide_pitch_trim" shortname="glide pitch trim" param="NAV_GLIDE_PITCH_TRIM"/>
@@ -82,7 +84,7 @@
<dl_setting MAX="20" MIN="-20" STEP="1" VAR="fp_pitch"/>
<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="0.2" MIN="0" 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_settings>
</dl_settings>
conf/simulator/jsbsim/aircraft/Quad_LisaM.xml
+396
View File
@@ -0,0 +1,396 @@
<?xml version="1.0"?>
<?xml-stylesheet type="text/xsl" href="http://jsbsim.sourceforge.net/JSBSim.xsl"?>
<fdm_config name="QUAD COMPLETE EXT" version="2.0" release="BETA" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSim.xsd">
<fileheader>
<author> Gustavo Violato &amp; Antoine Drouin</author>
<filecreationdate> 24-02-2009 </filecreationdate>
<version> Version 0.9 - beta</version>
<description> Simple Quadrotor without rotor dynamic </description>
</fileheader>
<metrics>
<wingarea unit="IN2"> 78.53 </wingarea>
<wingspan unit="IN"> 10 </wingspan>
<chord unit="IN"> 6.89 </chord>
<htailarea unit="FT2"> 0 </htailarea>
<htailarm unit="FT"> 0 </htailarm>
<vtailarea unit="FT2"> 0 </vtailarea>
<vtailarm unit="FT"> 0 </vtailarm>
<location name="AERORP" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="EYEPOINT" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="VRP" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</metrics>
<mass_balance>
<ixx unit="SLUG*FT2"> 0.005 </ixx>
<iyy unit="SLUG*FT2"> 0.005 </iyy>
<izz unit="SLUG*FT2"> 0.010 </izz>
<ixy unit="SLUG*FT2"> 0. </ixy>
<ixz unit="SLUG*FT2"> 0. </ixz>
<iyz unit="SLUG*FT2"> 0. </iyz>
<emptywt unit="LBS"> 0.84 </emptywt>
<location name="CG" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</mass_balance>
<ground_reactions>
<contact type="STRUCTURE" name="CONTACT_FRONT">
<location unit="M">
<x>-0.15 </x>
<y> 0 </y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
<contact type="STRUCTURE" name="CONTACT_BACK">
<location unit="M">
<x> 0.15</x>
<y> 0 </y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
<contact type="STRUCTURE" name="CONTACT_RIGHT">
<location unit="M">
<x> 0. </x>
<y> 0.15</y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
<contact type="STRUCTURE" name="CONTACT_LEFT">
<location unit="M">
<x> 0. </x>
<y>-0.15</y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
</ground_reactions>
<external_reactions>
<property>fcs/front_motor</property>
<property>fcs/back_motor</property>
<property>fcs/right_motor</property>
<property>fcs/left_motor</property>
<!-- First the lift forces produced by each propeller -->
<force name="front_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/front_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>-6.89</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>
<force name="back_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/back_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>6.89</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>
<force name="right_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/right_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>
<force name="left_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/left_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>-6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>
<!-- Then the Moment Couples -->
<!-- Front Engine -->
<force name="front_couple1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/front_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>-6.89</x>
<!-- Necessary arm in IN to produce a moment ten times
"weaker" then the force when both are measured in the SI.
front and back motors turning clockwise, left and right motors
turning anti-clockwise when view from up-->
<y>1.9685</y>
<z>0</z>
</location>
<direction>
<x>1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<force name="front_couple2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/front_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>-6.89</x>
<y>-1.9685</y>
<z>0</z>
</location>
<direction>
<x>-1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<!-- Back Engine -->
<force name="back_couple1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/back_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>6.89</x>
<y>1.9685</y>
<z>0</z>
</location>
<direction>
<x>1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<force name="back_couple2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/back_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>6.89</x>
<y>-1.9685</y>
<z>0</z>
</location>
<direction>
<x>-1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<!-- Right Engine -->
<force name="right_couple1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/right_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>-1.9685</x>
<y>6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>1</y>
<z>0</z>
</direction>
</force>
<force name="right_couple2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/right_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>1.9685</x>
<y>6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>-1</y>
<z>0</z>
</direction>
</force>
<!-- Left Engine -->
<force name="left_couple1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/left_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>-1.9685</x>
<y>-6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>1</y>
<z>0</z>
</direction>
</force>
<force name="left_couple2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/left_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>1.9685</x>
<y>-6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>-1</y>
<z>0</z>
</direction>
</force>
</external_reactions>
<propulsion/>
<flight_control name="FGFCS"/>
<aerodynamics>
<axis name="DRAG">
<function name="aero/coefficient/CD">
<description>Drag</description>
<product>
<property>aero/qbar-psf</property>
<value>47.9</value> <!-- Conversion to pascals -->
<value>0.0151</value> <!-- CD x Area (m^2) -->
<value>0.224808943</value> <!-- N to LBS -->
</product>
</function>
</axis>
</aerodynamics>
</fdm_config>
conf/simulator/nps/nps_sensors_params_aspirin.h → conf/simulator/nps/nps_sensors_params_aspirin_1.5.h
+11 -13
View File
@@ -1,7 +1,5 @@
/*
* $Id$
*
* Copyright (C) 2009 Antoine Drouin
* Copyright (C) 2012 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
@@ -19,13 +17,13 @@
* 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.
*
*/
#ifndef NPS_SENSORS_PARAMS_H
#define NPS_SENSORS_PARAMS_H
#include "generated/airframe.h"
#include "subsystems/imu.h"
#if 1
#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI
@@ -104,17 +102,17 @@
#define NPS_MAG_MIN -2047
#define NPS_MAG_MAX 2047
#define NPS_MAG_IMU_TO_SENSOR_PHI 0.
#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
#define NPS_MAG_IMU_TO_SENSOR_PSI RadOfDeg(45.)
#define NPS_MAG_IMU_TO_SENSOR_PHI 0.
#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
#define NPS_MAG_IMU_TO_SENSOR_PSI 0.
#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(-1./4.94075530)
#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL( 1./5.10207664)
#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(-1./4.90788848)
#define NPS_MAG_SENSITIVITY_XX IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS)
#define NPS_MAG_SENSITIVITY_YY IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS)
#define NPS_MAG_SENSITIVITY_ZZ IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS)
#define NPS_MAG_NEUTRAL_X 2358
#define NPS_MAG_NEUTRAL_Y 2362
#define NPS_MAG_NEUTRAL_Z 2119
#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL
#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL
#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL
#define NPS_MAG_NOISE_STD_DEV_X 2e-3
#define NPS_MAG_NOISE_STD_DEV_Y 2e-3
sw/airborne/arch/sim/subsystems/imu/imu_aspirin_arch.c
+2 -2
View File
@@ -26,7 +26,7 @@
#include "generated/airframe.h"
void imu_aspirin_arch_init(void) {
imu_aspirin.status = AspirinStatusIdle;
}
@@ -35,7 +35,7 @@ void imu_aspirin_arch_init(void) {
void imu_feed_gyro_accel(void) {
#if 1
#if 0
// the high byte is in buf[0], low byte in buf[1], etc.
imu_aspirin.i2c_trans_gyro.buf[0] = ((int16_t)sensors.gyro.value.x) >> 8;
imu_aspirin.i2c_trans_gyro.buf[1] = ((int16_t)sensors.gyro.value.x) & 0xff;
sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_mkk_arch.c
-3
View File
@@ -1,3 +0,0 @@
#include "actuators/actuators_mkk.h"
void actuators_mkk_arch_init(void) {}
sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_mkk_arch.h → sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_pwm_arch.c
+16 -11
View File
@@ -1,29 +1,34 @@
/*
* $Id$
* Copyright (C) 2010 The Paparazzi Team
*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
* This file is part of Paparazzi.
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* 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,
* 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
* 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.
*/
#ifndef ACTUATORS_MKK_ARCH_H
#define ACTUATORS_MKK_ARCH_H
/** @file arch/sim/actuators_pwm_arch.c
* dummy servos handling for sim
*/
#define ActuatorsMkkArchSend() {}
#include "firmwares/rotorcraft/actuators/actuators_pwm.h"
#endif /* ACTUATORS_MKK_ARCH_H */
void actuators_pwm_arch_init(void) {
}
void actuators_pwm_commit(void) {
}
sw/airborne/firmwares/rotorcraft/actuators/arch/sim/actuators_pwm_arch.h
+38
View File
@@ -0,0 +1,38 @@
/*
* Copyright (C) 2010 The Paparazzi Team
*
* 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 arch/sim/actuators_pwm_arch.h
* dummy servos handling for sim
*/
#ifndef ACTUATORS_PWM_ARCH_H
#define ACTUATORS_PWM_ARCH_H
#define ACTUATORS_PWM_NB 8
extern void actuators_pwm_arch_init(void);
extern void actuators_pwm_commit(void);
#define ChopServo(_x,_a,_b) Chop(_x, _a, _b)
#define Actuator(_x) actuators_pwm_values[_x]
#define SERVOS_TICS_OF_USEC(_v) (_v)
#endif /* ACTUATORS_PWM_ARCH_H */
sw/airborne/firmwares/rotorcraft/telemetry.h
+12 -11
View File
@@ -304,15 +304,16 @@
#include "subsystems/ahrs/ahrs_aligner.h"
#define PERIODIC_SEND_FILTER_ALIGNER(_trans, _dev) { \
DOWNLINK_SEND_FILTER_ALIGNER(_trans, _dev, \
&ahrs_aligner.lp_gyro.p, \
&ahrs_aligner.lp_gyro.q, \
&ahrs_aligner.lp_gyro.r, \
&imu.gyro.p, \
&imu.gyro.q, \
&imu.gyro.r, \
&ahrs_aligner.noise, \
&ahrs_aligner.low_noise_cnt); \
DOWNLINK_SEND_FILTER_ALIGNER(_trans, _dev, \
&ahrs_aligner.lp_gyro.p, \
&ahrs_aligner.lp_gyro.q, \
&ahrs_aligner.lp_gyro.r, \
&imu.gyro.p, \
&imu.gyro.q, \
&imu.gyro.r, \
&ahrs_aligner.noise, \
&ahrs_aligner.low_noise_cnt, \
&ahrs_aligner.status); \
}
@@ -325,7 +326,7 @@
}
#if USE_AHRS_CMPL
#if USE_AHRS_CMPL_EULER
#include "subsystems/ahrs/ahrs_int_cmpl_euler.h"
#define PERIODIC_SEND_FILTER(_trans, _dev) { \
DOWNLINK_SEND_FILTER(_trans, _dev, \
@@ -832,7 +833,7 @@
#ifdef BOOZ2_TRACK_CAM
#include "cam_track.h"
#define PERIODIC_SEND_CAM_TRACK(_trans, _dev) DOWNLINK_SEND_BOOZ_SIM_SPEED_POS(_trans, _dev, \
#define PERIODIC_SEND_CAM_TRACK(_trans, _dev) DOWNLINK_SEND_NPS_SPEED_POS(_trans, _dev, \
&target_accel_ned.x, \
&target_accel_ned.y, \
&target_accel_ned.z, \
sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
+4
View File
@@ -50,6 +50,10 @@
#warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course."
#endif
#ifndef AHRS_PROPAGATE_FREQUENCY
#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
#endif
void ahrs_update_mag_full(void);
void ahrs_update_mag_2d(void);
void ahrs_update_mag_2d_dumb(void);
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+65 -53
View File
@@ -35,8 +35,6 @@
#include <string.h>
// FIXME this is still needed for fixedwing integration
#include "estimator.h"
#include "led.h"
// FIXME Debugging Only
@@ -48,11 +46,22 @@
#include "subsystems/datalink/downlink.h"
struct AhrsFloatDCM ahrs_impl;
#ifdef AHRS_UPDATE_FW_ESTIMATOR
// FIXME this is still needed for fixedwing integration
#include "estimator.h"
// remotely settable
#ifndef INS_ROLL_NEUTRAL_DEFAULT
#define INS_ROLL_NEUTRAL_DEFAULT 0
#endif
#ifndef INS_PITCH_NEUTRAL_DEFAULT
#define INS_PITCH_NEUTRAL_DEFAULT 0
#endif
float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
#endif /* AHRS_UPDATE_FW_ESTIMATOR */
struct AhrsFloatDCM ahrs_impl;
// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
// Positive pitch : nose up
@@ -78,6 +87,7 @@ float MAG_Heading_X = 1;
float MAG_Heading_Y = 0;
#endif
static inline void compute_ahrs_representations(void);
static inline void compute_body_orientation_and_rates(void);
static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat);
@@ -114,54 +124,6 @@ static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
}
/**************************************************/
void ahrs_update_fw_estimator( void )
{
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
ahrs_float.ltp_to_imu_euler.psi = 0;
#else
ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
/* set quaternion and rotation matrix representations as well */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
compute_body_orientation_and_rates();
// export results to estimator
estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral;
estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral;
estimator_psi = ahrs_float.ltp_to_body_euler.psi;
estimator_p = ahrs_float.body_rate.p;
estimator_q = ahrs_float.body_rate.q;
estimator_r = ahrs_float.body_rate.r;
/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice,
&(DCM_Matrix[0][0]),
&(DCM_Matrix[0][1]),
&(DCM_Matrix[0][2]),
&(DCM_Matrix[1][0]),
&(DCM_Matrix[1][1]),
&(DCM_Matrix[1][2]),
&(DCM_Matrix[2][0]),
&(DCM_Matrix[2][1]),
&(DCM_Matrix[2][2])
));
*/
}
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
@@ -244,9 +206,10 @@ void ahrs_propagate(void)
#endif
Matrix_update();
// INFO, ahrs struct only updated in ahrs_update_fw_estimator
Normalize();
compute_ahrs_representations();
}
void ahrs_update_accel(void)
@@ -562,3 +525,52 @@ static inline void compute_body_orientation_and_rates(void) {
FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate);
}
static inline void compute_ahrs_representations(void) {
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
ahrs_float.ltp_to_imu_euler.psi = 0;
#else
ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
/* set quaternion and rotation matrix representations as well */
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_euler);
compute_body_orientation_and_rates();
/*
RunOnceEvery(6,DOWNLINK_SEND_RMAT_DEBUG(DefaultChannel, DefaultDevice,
&(DCM_Matrix[0][0]),
&(DCM_Matrix[0][1]),
&(DCM_Matrix[0][2]),
&(DCM_Matrix[1][0]),
&(DCM_Matrix[1][1]),
&(DCM_Matrix[1][2]),
&(DCM_Matrix[2][0]),
&(DCM_Matrix[2][1]),
&(DCM_Matrix[2][2])
));
*/
}
#ifdef AHRS_UPDATE_FW_ESTIMATOR
void ahrs_update_fw_estimator( void ) {
// export results to estimator
estimator_phi = ahrs_float.ltp_to_body_euler.phi - ins_roll_neutral;
estimator_theta = ahrs_float.ltp_to_body_euler.theta - ins_pitch_neutral;
estimator_psi = ahrs_float.ltp_to_body_euler.psi;
estimator_p = ahrs_float.body_rate.p;
estimator_q = ahrs_float.body_rate.q;
estimator_r = ahrs_float.body_rate.r;
}
#endif //AHRS_UPDATE_FW_ESTIMATOR
sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+4
View File
@@ -37,10 +37,14 @@ struct AhrsFloatDCM {
};
extern struct AhrsFloatDCM ahrs_impl;
#ifdef AHRS_UPDATE_FW_ESTIMATOR
extern float ins_roll_neutral;
extern float ins_pitch_neutral;
void ahrs_update_fw_estimator(void);
#endif
// DCM Parameters
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+4
View File
@@ -46,6 +46,10 @@ static inline void ahrs_update_mag_2d(void);
#warning "Using magnetometer and GPS course to update heading. Probably better to set USE_MAGNETOMETER=0 if you want to use GPS course."
#endif
#ifndef AHRS_PROPAGATE_FREQUENCY
#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY
#endif
struct AhrsIntCmpl ahrs_impl;
static inline void compute_imu_euler_and_rmat_from_quat(void);
sw/airborne/subsystems/imu/imu_aspirin.h
+32 -1
View File
@@ -65,12 +65,13 @@
#define IMU_ACCEL_Z_SIGN 1
#endif
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#ifdef IMU_ASPIRIN_VERSION_1_5
/** default gyro sensitivy and neutral from the datasheet
* IMU-3000 has 16.4 LSB/(deg/s) at 2000deg/s range
* sens = 1/16.4 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/16.4 * pi/180 * 4096 = 4.359066229
*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS 4.359
#define IMU_GYRO_P_SENS_NUM 4359
#define IMU_GYRO_P_SENS_DEN 1000
@@ -80,6 +81,22 @@
#define IMU_GYRO_R_SENS 4.359
#define IMU_GYRO_R_SENS_NUM 4359
#define IMU_GYRO_R_SENS_DEN 1000
#else
/** default gyro sensitivy and neutral from the datasheet
* ITG3200 has 14.375 LSB/(deg/s)
* sens = 1/14.375 * pi/180 * 2^INT32_RATE_FRAC
* sens = 1/14.375 * pi/180 * 4096 = 4.973126
*/
#define IMU_GYRO_P_SENS 4.973
#define IMU_GYRO_P_SENS_NUM 4973
#define IMU_GYRO_P_SENS_DEN 1000
#define IMU_GYRO_Q_SENS 4.973
#define IMU_GYRO_Q_SENS_NUM 4973
#define IMU_GYRO_Q_SENS_DEN 1000
#define IMU_GYRO_R_SENS 4.973
#define IMU_GYRO_R_SENS_NUM 4973
#define IMU_GYRO_R_SENS_DEN 1000
#endif // IMU_ASPIRIN_VERSION_1_5
#endif
@@ -236,8 +253,22 @@ static inline void imu_aspirin_event(void (* _gyro_handler)(void), void (* _acce
}
#ifndef SITL
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) do { \
imu_aspirin_event(_gyro_handler, _accel_handler, _mag_handler); \
} while(0);
#else
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
if (imu_aspirin.accel_available) { \
imu_aspirin.accel_available = FALSE; \
_accel_handler(); \
} \
if (imu_aspirin.gyro_available_blaaa) { \
imu_aspirin.gyro_available_blaaa = FALSE; \
_gyro_handler(); \
} \
ImuMagEvent(_mag_handler); \
}
#endif
#endif /* IMU_ASPIRIN_H */
sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c
+2 -2
View File
@@ -59,7 +59,7 @@ gboolean timeout_callback(gpointer data) {
ahrs_float.ltp_to_imu_euler.theta,
ahrs_float.ltp_to_imu_euler.psi);
IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
IvySendMsg("183 NPS_RATE_ATTITUDE %f %f %f %f %f %f",
DegOfRad(aos.imu_rates.p),
DegOfRad(aos.imu_rates.q),
DegOfRad(aos.imu_rates.r),
@@ -67,7 +67,7 @@ gboolean timeout_callback(gpointer data) {
DegOfRad(aos.ltp_to_imu_euler.theta),
DegOfRad(aos.ltp_to_imu_euler.psi));
IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f",
IvySendMsg("183 NPS_GYRO_BIAS %f %f %f",
DegOfRad(aos.gyro_bias.p),
DegOfRad(aos.gyro_bias.q),
DegOfRad(aos.gyro_bias.r));
sw/logalizer/export.ml
+1 -1
View File
@@ -152,7 +152,7 @@ let export_values = fun ?(sep="tab") ?(export_geo_pos=true) (model:GTree.tree_st
(* Print the header *)
fprintf f "Time%sUTC" sep;
if export_geo_pos then
fprintf f "%sGPS lat(deg)%sGPS long(deg)" sep sep;
fprintf f "%sGPS_lat(deg)%sGPS_long(deg)" sep sep;
List.iter (fun (m,field) -> fprintf f "%s%s:%s" sep m field) !fields_to_export;
fprintf f "\n%!";
sw/simulator/flightModel.ml
+9 -2
View File
@@ -214,8 +214,15 @@ module Make(A:Data.MISSION) = struct
(* Minimum complexity *)
(*
http://controls.ae.gatech.edu/papers/johnson_dasc_01.pdf
http://controls.ae.gatech.edu/papers/johnson_mst_01.pdf
Johnson, E.N., Fontaine, S.G., and Kahn, A.D.,
“Minimum Complexity Uninhabited Air Vehicle Guidance And Flight Control System,”
Proceedings of the 20th Digital Avionics Systems Conference, 2001.
http://www.ae.gatech.edu/~ejohnson/JohnsonFontaineKahn.pdf
Johnson, E.N. and Fontaine, S.G.,
“Use Of Flight Simulation To Complement Flight Testing Of Low-Cost UAVs,”
Proceedings of the AIAA Modeling and Simulation Technology Conference, 2001.
http://www.ae.gatech.edu/~ejohnson/AIAA%202001-4059.pdf
*)
let state_update = fun state nominal_airspeed (wx, wy, wz) agl dt ->
let now = state.t +. dt in
sw/simulator/nps/nps_autopilot_rotorcraft.c
+31 -5
View File
@@ -1,3 +1,24 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* 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.
*/
#include "nps_autopilot_rotorcraft.h"
#include "firmwares/rotorcraft/main.h"
@@ -21,7 +42,6 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha
nps_radio_control_init(type_rc, num_rc_script, rc_dev);
nps_bypass_ahrs = TRUE;
// nps_bypass_ahrs = FALSE;
main_init();
@@ -73,16 +93,22 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
handle_periodic_tasks();
if (time < 8) { /* start with a little bit of hovering */
/* override the commands for the first 8 seconds...
* start with a little bit of hovering
*/
if (time < 8) {
int32_t init_cmd[4];
init_cmd[COMMAND_THRUST] = 0.253*SUPERVISION_MAX_MOTOR;
init_cmd[COMMAND_THRUST] = 0.253*MAX_PPRZ;
init_cmd[COMMAND_ROLL] = 0;
init_cmd[COMMAND_PITCH] = 0;
init_cmd[COMMAND_YAW] = 0;
supervision_run(TRUE, FALSE, init_cmd);
}
for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++)
autopilot.commands[i] = (double)supervision.commands[i] / SUPERVISION_MAX_MOTOR;
/* scale final motor commands to 0-1 for feeding the fdm */
/* FIXME: autopilot.commands is of length NB_COMMANDS instead of number of motors */
for (uint8_t i=0; i<SUPERVISION_NB_MOTOR; i++)
autopilot.commands[i] = (double)(supervision.commands[i] - SUPERVISION_MIN_MOTOR) / SUPERVISION_MAX_MOTOR;
}
sw/simulator/nps/nps_autopilot_rotorcraft.h
+3 -3
View File
@@ -1,5 +1,5 @@
#ifndef NPS_AUTOPILOT_BOOZ_H
#define NPS_AUTOPILOT_BOOZ_H
#ifndef NPS_AUTOPILOT_ROTORCRAFT_H
#define NPS_AUTOPILOT_ROTORCRAFT_H
#include "nps_autopilot.h"
@@ -7,4 +7,4 @@
extern bool_t nps_bypass_ahrs;
extern void sim_overwrite_ahrs(void);
#endif /* NPS_AUTOPILOT_BOOZ_H */
#endif /* NPS_AUTOPILOT_ROTORCRAFT_H */
sw/simulator/nps/nps_fdm.h
+21
View File
@@ -1,3 +1,24 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* 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.
*/
#ifndef NPS_FDM
#define NPS_FDM
sw/simulator/nps/nps_fdm_jsbsim.c
+21
View File
@@ -1,3 +1,24 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* 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.
*/
#include <FGFDMExec.h>
#include <FGJSBBase.h>
#include <models/FGPropulsion.h>
sw/simulator/nps/nps_ivy.c
+26 -34
View File
@@ -109,23 +109,15 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
void nps_ivy_display(void) {
/*
IvySendMsg("%d COMMANDS %f %f %f %f",
AC_ID,
autopilot.commands[SERVO_FRONT],
autopilot.commands[SERVO_BACK],
autopilot.commands[SERVO_RIGHT],
autopilot.commands[SERVO_LEFT]);
*/
IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
AC_ID,
DegOfRad(fdm.body_ecef_rotvel.p),
DegOfRad(fdm.body_ecef_rotvel.q),
DegOfRad(fdm.body_ecef_rotvel.r),
DegOfRad(fdm.ltp_to_body_eulers.phi),
DegOfRad(fdm.ltp_to_body_eulers.theta),
DegOfRad(fdm.ltp_to_body_eulers.psi));
IvySendMsg("%d BOOZ_SIM_POS_LLH %f %f %f %f %f %f %f %f %f",
IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
AC_ID,
DegOfRad(fdm.body_ecef_rotvel.p),
DegOfRad(fdm.body_ecef_rotvel.q),
DegOfRad(fdm.body_ecef_rotvel.r),
DegOfRad(fdm.ltp_to_body_eulers.phi),
DegOfRad(fdm.ltp_to_body_eulers.theta),
DegOfRad(fdm.ltp_to_body_eulers.psi));
IvySendMsg("%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f",
AC_ID,
(fdm.lla_pos_pprz.lat),
(fdm.lla_pos_geod.lat),
@@ -136,28 +128,28 @@ void nps_ivy_display(void) {
(fdm.lla_pos_geod.alt),
(fdm.agl),
(fdm.hmsl));
IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f %f %f %f",
AC_ID,
(fdm.ltpprz_ecef_accel.x),
(fdm.ltpprz_ecef_accel.y),
(fdm.ltpprz_ecef_accel.z),
(fdm.ltpprz_ecef_vel.x),
(fdm.ltpprz_ecef_vel.y),
(fdm.ltpprz_ecef_vel.z),
(fdm.ltpprz_pos.x),
(fdm.ltpprz_pos.y),
(fdm.ltpprz_pos.z));
IvySendMsg("%d BOOZ_SIM_GYRO_BIAS %f %f %f",
AC_ID,
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.x)+sensors.gyro.bias_initial.x),
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.y)+sensors.gyro.bias_initial.y),
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.z)+sensors.gyro.bias_initial.z));
IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f",
AC_ID,
(fdm.ltpprz_ecef_accel.x),
(fdm.ltpprz_ecef_accel.y),
(fdm.ltpprz_ecef_accel.z),
(fdm.ltpprz_ecef_vel.x),
(fdm.ltpprz_ecef_vel.y),
(fdm.ltpprz_ecef_vel.z),
(fdm.ltpprz_pos.x),
(fdm.ltpprz_pos.y),
(fdm.ltpprz_pos.z));
IvySendMsg("%d NPS_GYRO_BIAS %f %f %f",
AC_ID,
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.x)+sensors.gyro.bias_initial.x),
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.y)+sensors.gyro.bias_initial.y),
DegOfRad(RATE_FLOAT_OF_BFP(sensors.gyro.bias_random_walk_value.z)+sensors.gyro.bias_initial.z));
/* transform magnetic field to body frame */
struct DoubleVect3 h_body;
FLOAT_QUAT_VMULT(h_body, fdm.ltp_to_body_quat, fdm.ltp_h);
IvySendMsg("%d BOOZ_SIM_SENSORS_SCALED %f %f %f %f %f %f",
IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
AC_ID,
((sensors.accel.value.x - sensors.accel.neutral.x)/NPS_ACCEL_SENSITIVITY_XX),
((sensors.accel.value.y - sensors.accel.neutral.y)/NPS_ACCEL_SENSITIVITY_YY),
sw/simulator/nps/nps_radio_control.c
+21
View File
@@ -1,3 +1,24 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* 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.
*/
#include "nps_radio_control.h"
#include "nps_radio_control_spektrum.h"
sw/simulator/nps/nps_radio_control.h
+21
View File
@@ -1,3 +1,24 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* 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.
*/
#ifndef NPS_RADIO_CONTROL_H
#define NPS_RADIO_CONTROL_H
sw/simulator/old_booz/booz2_sim_main.c
+5 -5
View File
@@ -260,13 +260,13 @@ static void booz2_sim_display(void) {
if (sim_time >= disp_time) {
disp_time+= DT_DISPLAY;
// booz_flightgear_send();
IvySendMsg("%d BOOZ_SIM_RPMS %f %f %f %f",
IvySendMsg("%d NPS_RPMS %f %f %f %f",
AC_ID,
RPM_OF_RAD_S(bfm.omega->ve[SERVO_FRONT]),
RPM_OF_RAD_S(bfm.omega->ve[SERVO_BACK]),
RPM_OF_RAD_S(bfm.omega->ve[SERVO_RIGHT]),
RPM_OF_RAD_S(bfm.omega->ve[SERVO_LEFT]) );
IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
AC_ID,
DegOfRad(bfm.ang_rate_body->ve[AXIS_X]),
DegOfRad(bfm.ang_rate_body->ve[AXIS_Y]),
@@ -274,7 +274,7 @@ static void booz2_sim_display(void) {
DegOfRad(bfm.eulers->ve[AXIS_X]),
DegOfRad(bfm.eulers->ve[AXIS_Y]),
DegOfRad(bfm.eulers->ve[AXIS_Z]));
IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f",
IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f",
AC_ID,
(bfm.speed_ltp->ve[AXIS_X]),
(bfm.speed_ltp->ve[AXIS_Y]),
@@ -283,13 +283,13 @@ static void booz2_sim_display(void) {
(bfm.pos_ltp->ve[AXIS_Y]),
(bfm.pos_ltp->ve[AXIS_Z]));
#if 0
IvySendMsg("%d BOOZ_SIM_WIND %f %f %f",
IvySendMsg("%d NPS_WIND %f %f %f",
AC_ID,
bwm.velocity->ve[AXIS_X],
bwm.velocity->ve[AXIS_Y],
bwm.velocity->ve[AXIS_Z]);
#endif
IvySendMsg("%d BOOZ_SIM_ACCEL_LTP %f %f %f",
IvySendMsg("%d NPS_ACCEL_LTP %f %f %f",
AC_ID,
bfm.accel_ltp->ve[AXIS_X],
bfm.accel_ltp->ve[AXIS_Y],
sw/tools/calibration/calibrate.py
+25 -12
View File
@@ -1,6 +1,5 @@
#! /usr/bin/env python
# $Id$
# Copyright (C) 2010 Antoine Drouin
#
# This file is part of Paparazzi.
@@ -18,7 +17,7 @@
# 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.
# Boston, MA 02111-1307, USA.
#
@@ -31,7 +30,7 @@ from scipy import optimize
import calibration_utils
def main():
usage = "usage: %prog [options] log_filename"
usage = "usage: %prog [options] log_filename.data"
parser = OptionParser(usage)
parser.add_option("-i", "--id", dest="ac_id",
action="store",
@@ -50,31 +49,45 @@ def main():
else:
print args[0] + " not found"
sys.exit(1)
if options.sensor == "GYRO":
print "You can't calibate gyros with this!"
sys.exit(1)
ac_ids = calibration_utils.get_ids_in_log(filename)
# import code; code.interact(local=locals())
if options.ac_id == None and len(ac_ids) == 1:
options.ac_id = ac_ids[0]
if options.verbose:
print "reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor
measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor)
if options.verbose:
print "found "+str(len(measurements))+" records"
if options.sensor == "ACCEL":
sensor_ref = 9.81
sensor_res = 10
noise_window = 20;
noise_threshold = 40;
else: # MAG
elif options.sensor == "MAG":
sensor_ref = 1.
sensor_res = 11
noise_window = 10;
noise_threshold = 1000;
elif options.sensor == "GYRO":
parser.error("You can't calibate gyros with this!")
else:
parser.error("Specify a valid sensor.")
if not filename.endswith(".data"):
parser.error("Please specify a *.data log file")
if options.verbose:
print "reading file "+filename+" for aircraft "+options.ac_id+" and sensor "+options.sensor
# read raw measurements from log file
measurements = calibration_utils.read_log(options.ac_id, filename, options.sensor)
if len(measurements) == 0:
print "Error: found zero IMU_"+options.sensor+"_RAW measurements for aircraft with id "+options.ac_id+" in log file!"
sys.exit(1)
if options.verbose:
print "found "+str(len(measurements))+" records"
# filter out noisy measurements
flt_meas, flt_idx = calibration_utils.filter_meas(measurements, noise_window, noise_threshold)
if options.verbose:
print "remaining "+str(len(flt_meas))+" after low pass"
# get an initial min/max guess
p0 = calibration_utils.get_min_max_guess(flt_meas, sensor_ref)
cp0, np0 = calibration_utils.scale_measurements(flt_meas, p0)
print "initial guess : avg "+str(np0.mean())+" std "+str(np0.std())