mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 22:17:01 +08:00
Booz IMU for Fixedwings
This commit is contained in:
Christophe De Wagter
@@ -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=""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="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>
|
||||
@@ -43,6 +43,9 @@
|
||||
|
||||
|
||||
ifeq ($(ARCH), lpc21)
|
||||
|
||||
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_analog.h\"
|
||||
|
||||
imu_CFLAGS += -DADC
|
||||
imu_CFLAGS += -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_$(GYRO_R)
|
||||
imu_CFLAGS += -DUSE_$(ACCEL_X) -DUSE_$(ACCEL_y) -DUSE_$(ACCEL_Z)
|
||||
|
||||
@@ -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
|
||||
@@ -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>
|
||||
@@ -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); }
|
||||
|
||||
#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
|
||||
#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
|
||||
|
||||
@@ -66,10 +66,10 @@
|
||||
|
||||
|
||||
#ifdef USE_ANALOG_IMU
|
||||
#include "subsystems/imu.h"
|
||||
#include "subsystems/ahrs.h"
|
||||
#include "subsystems/ahrs/ahrs_aligner.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_mag_event( void );
|
||||
#endif
|
||||
|
||||
@@ -378,8 +378,8 @@ void Matrix_update(void)
|
||||
void Euler_angles(void)
|
||||
{
|
||||
#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.theta = -asin((Accel_Vector[0])/GRAVITY); // asin(acc_x)
|
||||
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]);
|
||||
|
||||
Reference in New Issue
Block a user