Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Booz IMU for Fixedwings

Browse Source
This commit is contained in:
Christophe De Wagter
2010-12-14 23:43:58 +01:00
parent c83abf02d2
commit 01d4ab332c
7 changed files with 402 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TUDelft/MicrojetCDW.xml
+219
View File
@@ -0,0 +1,219 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="Microjet CDW Tiny 1.1">
<!-- commands section -->
<servos>
<servo name="THROTTLE" no="0" min="1110" neutral="1110" max="1900"/>
<servo name="AILEVON_LEFT" no="1" min="1000" neutral="1450" max="2000"/>
<servo name="AILEVON_RIGHT" no="2" min="2000" neutral="1450" max="1000"/>
</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.4"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.7"/>
</section>
<command_laws>
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
<set servo="THROTTLE" 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="0.7"/>
<define name="MAX_PITCH" value="0.6"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- MAX1168 ADC CHANNELS -->
<define name="GYRO_P_CHAN" value="0"/>
<define name="GYRO_Q_CHAN" value="1"/>
<define name="GYRO_R_CHAN" value="2"/>
<define name="ACCEL_X_CHAN" value="4"/>
<define name="ACCEL_Y_CHAN" value="3"/>
<define name="ACCEL_Z_CHAN" value="5"/>
<!-- Calibration Neutral -->
<define name="GYRO_P_NEUTRAL" value="33924"/>
<define name="GYRO_Q_NEUTRAL" value="33417"/>
<define name="GYRO_R_NEUTRAL" value="32809"/>
<define name="GYRO_P_SENS" value=" 1.00" integer="16"/>
<define name="GYRO_Q_SENS" value=" 1.00" integer="16"/>
<define name="GYRO_R_SENS" value=" 1.00" integer="16"/>
<define name="GYRO_P_SIGN" value="1"/>
<define name="GYRO_Q_SIGN" value="1"/>
<define name="GYRO_R_SIGN" value="1"/>
<define name="ACCEL_X_NEUTRAL" value="26000"/>
<define name="ACCEL_Y_NEUTRAL" value="26000"/>
<define name="ACCEL_Z_NEUTRAL" value="26000"/>
<define name="ACCEL_X_SENS" value="-2.50411474" integer="16"/>
<define name="ACCEL_Y_SENS" value="-2.48126183" integer="16"/>
<define name="ACCEL_Z_SENS" value="-2.51396167" integer="16"/>
<define name="ACCEL_X_SIGN" value="1"/>
<define name="ACCEL_Y_SIGN" value="1"/>
<define name="ACCEL_Z_SIGN" value="-1"/>
<define name="MAG_X_NEUTRAL" value="2358"/>
<define name="MAG_Y_NEUTRAL" value="2362"/>
<define name="MAG_Z_NEUTRAL" value="2119"/>
<define name="MAG_X_SENS" value="-3.4936416" integer="16"/>
<define name="MAG_Y_SENS" value=" 3.607713" integer="16"/>
<define name="MAG_Z_SENS" value="-4.90788848" integer="16"/>
<!-- <define name="MAG_45_HACK" value="1"/> -->
<define name="BODY_TO_IMU_PHI" value="0"/>
<define name="BODY_TO_IMU_THETA" value="0"/>
<define name="BODY_TO_IMU_PSI" value="0"/>
<define name="ROLL_NEUTRAL_DEFAULT" value="0"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="20000"/>
<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="&quot;ATPL2\rATRN5\rATTT80\r&quot;"/>
<!-- <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="0."/>
<define name="PITCH_DGAIN" value="0."/>
<define name="ELEVATOR_OF_ROLL" value="0."/>
<define name="ROLL_SLEW" value="0."/>
<define name="ROLL_ATTITUDE_GAIN" value="0."/>
<define name="ROLL_RATE_GAIN" value="0."/>
</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>
<firmware name="fixedwing">
<target name="ap" board="tiny_1.1"/>
<target name="sim" board="pc" />
<define name="AGR_CLIMB" />
<define name="LOITER_TRIM" />
<define name="ALT_KALMAN" />
<subsystem name="radio_control" type="ppm"/>
<!-- Communication -->
<subsystem name="telemetry" type="xbee_api">
<param name="MODEM_BAUD" value="B57600"/>
</subsystem>
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="imu" type="booz"/>
<subsystem name="attitude" type="dcm"/>
<subsystem name="gps" type="ublox_lea4p"/>
<subsystem name="navigation"/>
</firmware>
<firmware name="setup">
<target name="tunnel" board="tiny_1.1" />
</firmware>
<modules>
<load name="light.xml">
<param name="LIGHT_LED_STROBE" value="2"/>
<param name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
</load>
</modules>
</airframe>
conf/autopilot/subsystems/fixedwing/imu_analog.makefile
+3
View File
@@ -43,6 +43,9 @@
ifeq ($(ARCH), lpc21) ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_analog.h\"
imu_CFLAGS += -DADC imu_CFLAGS += -DADC
imu_CFLAGS += -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_$(GYRO_R) imu_CFLAGS += -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_$(GYRO_R)
imu_CFLAGS += -DUSE_$(ACCEL_X) -DUSE_$(ACCEL_y) -DUSE_$(ACCEL_Z) imu_CFLAGS += -DUSE_$(ACCEL_X) -DUSE_$(ACCEL_y) -DUSE_$(ACCEL_Z)
conf/autopilot/subsystems/fixedwing/imu_booz.makefile
+86
View File
@@ -0,0 +1,86 @@
#
# Booz2 IMU booz2v1.2
#
#
# required xml:
# <section name="IMU" prefix="IMU_">
#
# <define name="GYRO_X_NEUTRAL" value="33924"/>
# <define name="GYRO_Y_NEUTRAL" value="33417"/>
# <define name="GYRO_Z_NEUTRAL" value="32809"/>
#
# <define name="GYRO_X_SENS" value=" 1.01" integer="16"/>
# <define name="GYRO_Y_SENS" value="-1.01" integer="16"/>
# <define name="GYRO_Z_SENS" value="-1.01" integer="16"/>
#
# <define name="ACCEL_X_NEUTRAL" value="32081"/>
# <define name="ACCEL_Y_NEUTRAL" value="33738"/>
# <define name="ACCEL_Z_NEUTRAL" value="32441"/>
#
# <define name="ACCEL_X_SENS" value="-2.50411474" integer="16"/>
# <define name="ACCEL_Y_SENS" value="-2.48126183" integer="16"/>
# <define name="ACCEL_Z_SENS" value="-2.51396167" integer="16"/>
#
# <define name="MAG_X_NEUTRAL" value="2358"/>
# <define name="MAG_Y_NEUTRAL" value="2362"/>
# <define name="MAG_Z_NEUTRAL" value="2119"/>
#
# <define name="MAG_X_SENS" value="-3.4936416" integer="16"/>
# <define name="MAG_Y_SENS" value=" 3.607713" integer="16"/>
# <define name="MAG_Z_SENS" value="-4.90788848" integer="16"/>
# <define name="MAG_45_HACK" value="1"/>
#
# </section>
#
#
#
# param: MAX_1168_DRDY_PORT
# imu Booz2 v1.0, v1.1, v1.2, YAI v1.0
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c
imu_srcs += $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
imu_srcs += peripherals/max1168.c
imu_srcs += $(SRC_ARCH)/peripherals/max1168_arch.c
#ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DSSP_VIC_SLOT=9
imu_CFLAGS += -DMAX1168_EOC_VIC_SLOT=8
#FIXME ms2001 not used on this imu
#else ifeq ($(ARCH), stm32)
#imu_CFLAGS += -DUSE_SPI2 -DUSE_DMA1_C4_IRQ -DUSE_EXTI2_IRQ -DUSE_SPI2_IRQ
#imu_CFLAGS += -DMAX_1168_DRDY_PORT=$(MAX_1168_DRDY_PORT)
#imu_CFLAGS += -DMAX_1168_DRDY_PORT_SOURCE=$(MAX_1168_DRDY_PORT_SOURCE)
#imu_CFLAGS += -DUSE_I2C2 -DUSE_EXTI9_5_IRQ
#endif
ap.srcs += $(imu_srcs)
ap.CFLAGS += $(imu_CFLAGS)
imu_srcs += math/pprz_trig_int.c
#
# Simulator
#
sim.CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
#FIXME, should be HMC5843
sim.CFLAGS += -DIMU_B2_MAG_TYPE=IMU_B2_MAG_AMI601
#FIXME, should be verision 1.2
sim.CFLAGS += -DIMU_B2_VERSION_1_1
sim.srcs += $(SRC_SUBSYSTEMS)/imu.c
sim.srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c
sim.srcs += $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
sim.srcs += peripherals/max1168.c
sim.srcs += $(SRC_ARCH)/peripherals/max1168_arch.c
sim.CFLAGS += -DUSE_AMI601
sim.srcs += peripherals/ami601.c
sim.CFLAGS += -DUSE_I2C1
conf/telemetry/default_fixedwing_imu.xml
+78
View File
@@ -0,0 +1,78 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
<telemetry>
<process name="Ap">
<mode name="default">
<message name="AIRSPEED" period="1"/>
<message name="ALIVE" period="5"/>
<message name="GPS" period="0.25"/>
<message name="NAVIGATION" period="1."/>
<message name="ATTITUDE" period="0.5"/>
<message name="ESTIMATOR" period="0.5"/>
<message name="ENERGY" period="2.5"/>
<message name="WP_MOVED" period="0.5"/>
<message name="CIRCLE" period="1.05"/>
<message name="DESIRED" period="1.05"/>
<message name="BAT" period="1.1"/>
<message name="BARO_MS5534A" period="1.0"/>
<message name="SCP_STATUS" period="1.0"/>
<message name="SEGMENT" period="1.2"/>
<message name="CALIBRATION" period="2.1"/>
<message name="NAVIGATION_REF" period="9."/>
<message name="PPRZ_MODE" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="GYRO_RATES" period="1.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="2.0"/>
<message name="IMU_ACCEL" period=".1"/>
<message name="IMU_GYRO" period=".05"/>
</mode>
<mode name="minimal">
<message name="ALIVE" period="5"/>
<message name="ATTITUDE" period="4"/>
<message name="GPS" period="1.05"/>
<message name="ESTIMATOR" period="1.3"/>
<message name="WP_MOVED" period="1.4"/>
<message name="CIRCLE" period="3.05"/>
<message name="DESIRED" period="4.05"/>
<message name="BAT" period="1.1"/>
<message name="SEGMENT" period="3.2"/>
<message name="CALIBRATION" period="5.1"/>
<message name="NAVIGATION_REF" period="9."/>
<message name="NAVIGATION" period="3."/>
<message name="PPRZ_MODE" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="GYRO_RATES" period="10.1"/>
<message name="SURVEY" period="2.1"/>
<message name="GPS_SOL" period="5.0"/>
</mode>
<mode name="extremal">
<message name="ALIVE" period="5"/>
<message name="GPS" period="5.1"/>
<message name="ESTIMATOR" period="5.3"/>
<message name="BAT" period="10.1"/>
<message name="DESIRED" period="10.2"/>
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="5.5"/>
<message name="DOWNLINK" period="5.7"/>
</mode>
</process>
<process name="Fbw">
<mode name="default">
<message name="COMMANDS" period="5"/>
<message name="FBW_STATUS" period="2"/>
<message name="ACTUATORS" period="5"/> <!-- For trimming -->
</mode>
<mode name="debug">
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="COMMANDS" period="0.5"/>
<message name="FBW_STATUS" period="1"/>
<message name="ACTUATORS" period="5"/> <!-- For trimming -->
</mode>
</process>
</telemetry>
sw/airborne/ap_downlink.h
+13
View File
@@ -138,6 +138,19 @@
#define PERIODIC_SEND_SEGMENT(_chan) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_chan, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); } #define PERIODIC_SEND_SEGMENT(_chan) if (nav_in_segment) { DOWNLINK_SEND_SEGMENT(_chan, &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); }
#ifdef IMU_TYPE_H
#include "subsystems/imu.h"
#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z)}
#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r)}
#define PERIODIC_SEND_IMU_ACCEL(_chan) { DOWNLINK_SEND_IMU_ACCEL_RAW(_chan, &imu.accel.x, &imu.accel.y, &imu.accel.z)}
#define PERIODIC_SEND_IMU_GYRO(_chan) { DOWNLINK_SEND_IMU_GYRO_RAW(_chan, &imu.gyro.p, &imu.gyro.q, &imu.gyro.r)}
#else
#define PERIODIC_SEND_IMU_ACCEL_RAW(_chan) {}
#define PERIODIC_SEND_IMU_GYRO_RAW(_chan) {}
#define PERIODIC_SEND_IMU_ACCEL(_chan) {}
#define PERIODIC_SEND_IMU_GYRO(_chan) {}
#endif
#ifdef IMU_ANALOG #ifdef IMU_ANALOG
#define PERIODIC_SEND_IMU(_chan) { int16_t dummy = 42; DOWNLINK_SEND_IMU(_chan, &(from_fbw.euler_dot[0]), &(from_fbw.euler_dot[1]), &(from_fbw.euler_dot[2]), &dummy, &dummy, &dummy); } #define PERIODIC_SEND_IMU(_chan) { int16_t dummy = 42; DOWNLINK_SEND_IMU(_chan, &(from_fbw.euler_dot[0]), &(from_fbw.euler_dot[1]), &(from_fbw.euler_dot[2]), &dummy, &dummy, &dummy); }
#else #else
sw/airborne/firmwares/fixedwing/main_ap.c
+1 -1
View File
@@ -66,10 +66,10 @@
#ifdef USE_ANALOG_IMU #ifdef USE_ANALOG_IMU
#include "subsystems/imu.h"
#include "subsystems/ahrs.h" #include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h" #include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ahrs/ahrs_float_dcm.h" #include "subsystems/ahrs/ahrs_float_dcm.h"
#include "subsystems/imu/imu_analog.h"
static inline void on_gyro_accel_event( void ); static inline void on_gyro_accel_event( void );
static inline void on_mag_event( void ); static inline void on_mag_event( void );
#endif #endif
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+2 -2
View File
@@ -378,8 +378,8 @@ void Matrix_update(void)
void Euler_angles(void) void Euler_angles(void)
{ {
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes) #if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
ahrs_float.ltp_to_imu_euler.phi = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z) 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_Vector[0])/GRAVITY); // asin(acc_x) ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
ahrs_float.ltp_to_imu_euler.psi = 0; ahrs_float.ltp_to_imu_euler.psi = 0;
#else #else
ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);