mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 07:53:43 +08:00
Merge remote branch 'paparazzi/analogimu'
This commit is contained in:
Felix Ruess
@@ -29,7 +29,7 @@
|
||||
<set command="PITCH" value="@PITCH"/>
|
||||
</rc_commands>
|
||||
|
||||
<command_laws>
|
||||
<command_laws>
|
||||
<set servo="THROTTLE" value="@THROTTLE"/>
|
||||
<set servo="AILERON" value="@ROLL"/>
|
||||
<set servo="ELEVATOR" value="@PITCH"/>
|
||||
@@ -45,22 +45,39 @@
|
||||
<define name="GYRO_Q_NEUTRAL" value="512"/>
|
||||
<define name="GYRO_R_NEUTRAL" value="512"/>
|
||||
|
||||
<define name="GYRO_P_SENS" value="0.017f" integer="16"/>
|
||||
<define name="GYRO_Q_SENS" value="0.017f" integer="16"/>
|
||||
<define name="GYRO_R_SENS" value="-0.017f" integer="16"/>
|
||||
<define name="GYRO_P_SENS" value="0.017" integer="16"/>
|
||||
<define name="GYRO_Q_SENS" value="0.017" integer="16"/>
|
||||
<define name="GYRO_R_SENS" value="0.017" 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_CHAN" value="3"/>
|
||||
<define name="ACCEL_Y_CHAN" value="4"/>
|
||||
<define name="ACCEL_Z_CHAN" value="5"/>
|
||||
-->
|
||||
<define name="ACCEL_X_SENS" value="0.1f" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="-0.1f" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="0.1f" integer="16"/>
|
||||
<define name="ACCEL_X_SENS" value="0.1" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="0.1" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="0.1" integer="16"/>
|
||||
|
||||
<define name="ACCEL_X_NEUTRAL" value="512"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="512"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="512"/>
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="-1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="1"/>
|
||||
|
||||
<!-- this should not be needed if no mag is used -->
|
||||
<define name="MAG_X_NEUTRAL" value="512"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="512"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="512"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="RadOfDeg(0.)" />
|
||||
<define name="BODY_TO_IMU_THETA" value="RadOfDeg(0.)" />
|
||||
<define name="BODY_TO_IMU_PSI" value="RadOfDeg(0.)" />
|
||||
</section>
|
||||
|
||||
<section name="AUTO1" prefix="AUTO1_">
|
||||
@@ -151,7 +168,7 @@
|
||||
<define name="PITCH_PGAIN" value="-9000."/>
|
||||
<define name="PITCH_DGAIN" value="1.5"/>
|
||||
<define name="ELEVATOR_OF_ROLL" value="1250"/>
|
||||
|
||||
|
||||
</section>
|
||||
|
||||
<section name="NAV">
|
||||
@@ -171,29 +188,29 @@
|
||||
</section>
|
||||
|
||||
<section name="FAILSAFE" prefix="FAILSAFE_">
|
||||
<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
|
||||
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
|
||||
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
|
||||
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
|
||||
<define name="HOME_RADIUS" value="100" unit="m"/>
|
||||
<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
|
||||
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
|
||||
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
|
||||
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
|
||||
<define name="HOME_RADIUS" value="100" unit="m"/>
|
||||
</section>
|
||||
|
||||
<firmware name="fixedwing">
|
||||
<target name="sim" board="pc"/>
|
||||
<target name="ap" board="twog_1.0">
|
||||
<target name="sim" board="pc"/>
|
||||
<target name="ap" board="twog_1.0">
|
||||
<define name="AGR_CLIMB" />
|
||||
<define name="LOITER_TRIM" />
|
||||
<define name="ALT_KALMAN" />
|
||||
</target>
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
<subsystem name="radio_control" type="ppm"/>
|
||||
<!-- Communication -->
|
||||
<subsystem name="telemetry" type="xbee_api">
|
||||
<param name="MODEM_BAUD" value="B9600"/>
|
||||
<subsystem name="telemetry" type="transparent">
|
||||
<param name="MODEM_BAUD" value="B9600"/>
|
||||
</subsystem>
|
||||
<!-- Actuators are automatically chosen according to the board-->
|
||||
<subsystem name="control"/>
|
||||
<!-- Sensors -->
|
||||
<subsystem name="attitude" type="analogimu">
|
||||
<subsystem name="imu" type="analog">
|
||||
<param name="GYRO_P" value="ADC_0"/>
|
||||
<param name="GYRO_Q" value="ADC_1"/>
|
||||
<param name="GYRO_R" value="ADC_2"/>
|
||||
@@ -201,6 +218,8 @@
|
||||
<param name="ACCEL_Y" value="ADC_6"/>
|
||||
<param name="ACCEL_Z" value="ADC_7"/>
|
||||
</subsystem>
|
||||
|
||||
<subsystem name="attitude" type="dcm"/>
|
||||
<subsystem name="gps" type="ublox_lea5h"/>
|
||||
<subsystem name="navigation"/>
|
||||
</firmware>
|
||||
@@ -208,16 +227,6 @@
|
||||
<makefile location="after">
|
||||
ap.CFLAGS += -DANALOGIMU_ROTATED
|
||||
ap.CFLAGS += -DANALOGIMU_ZERO_AVERAGE
|
||||
|
||||
ap.CFLAGS += -DGPS_USE_LATLONG
|
||||
|
||||
//ap.CFLAGS += -DWIND_INFO -DSTRONG_WIND
|
||||
|
||||
//sim.CFLAGS += -DWIND_INFO -DSTRONG_WIND
|
||||
</makefile>
|
||||
|
||||
</airframe>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,35 +0,0 @@
|
||||
# attitude via analog imu
|
||||
|
||||
|
||||
ifeq ($(ARCH), lpc21)
|
||||
ap.CFLAGS += -DANALOG_IMU
|
||||
ap.CFLAGS += -DADC -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_ADC_$(GYRO_R)
|
||||
ap.CFLAGS += -DUSE_ADC_3 -DUSE_$(ACCEL_X) -DUSE_$(ACCEL_y) -DUSE_$(ACCEL_Z)
|
||||
|
||||
ap.CFLAGS += -DADC_CHANNEL_GYRO_P=$(GYRO_P) -DADC_CHANNEL_GYRO_Q=$(GYRO_Q) -DADC_CHANNEL_GYRO_R=$(GYRO_R)
|
||||
ap.CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y) -DADC_CHANNEL_ACCEL_Z=$(ACCEL_Z)
|
||||
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c
|
||||
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/dcm.c
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/analogimu.c
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/dcm/analogimu_util.c
|
||||
|
||||
endif
|
||||
|
||||
# since there is currently no SITL sim for the Analog IMU, we use the infrared sim
|
||||
|
||||
ifeq ($(TARGET), sim)
|
||||
|
||||
sim.CFLAGS += -DIR_ROLL_NEUTRAL_DEFAULT=0
|
||||
sim.CFLAGS += -DIR_PITCH_NEUTRAL_DEFAULT=0
|
||||
|
||||
$(TARGET).CFLAGS += -DUSE_INFRARED
|
||||
$(TARGET).srcs += subsystems/sensors/infrared.c
|
||||
|
||||
sim.srcs += $(SRC_ARCH)/sim_ir.c
|
||||
sim.srcs += $(SRC_ARCH)/sim_analogimu.c
|
||||
|
||||
endif
|
||||
|
||||
jsbsim.srcs += $(SRC_ARCH)/jsbsim_ir.c
|
||||
@@ -0,0 +1,28 @@
|
||||
# attitude estimation for fixedwings via dcm algorithm
|
||||
|
||||
|
||||
ifeq ($(ARCH), lpc21)
|
||||
ap.CFLAGS += -DUSE_ANALOG_IMU
|
||||
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
|
||||
|
||||
endif
|
||||
|
||||
# since there is currently no SITL sim for the Analog IMU, we use the infrared sim
|
||||
|
||||
ifeq ($(TARGET), sim)
|
||||
|
||||
sim.CFLAGS += -DIR_ROLL_NEUTRAL_DEFAULT=0
|
||||
sim.CFLAGS += -DIR_PITCH_NEUTRAL_DEFAULT=0
|
||||
|
||||
sim.CFLAGS += -DUSE_INFRARED
|
||||
sim.srcs += subsystems/sensors/infrared.c
|
||||
|
||||
sim.srcs += $(SRC_ARCH)/sim_ir.c
|
||||
sim.srcs += $(SRC_ARCH)/sim_analogimu.c
|
||||
|
||||
endif
|
||||
|
||||
jsbsim.srcs += $(SRC_ARCH)/jsbsim_ir.c
|
||||
@@ -0,0 +1,62 @@
|
||||
#
|
||||
# Analog IMU connected to MCU ADC ports
|
||||
#
|
||||
#
|
||||
# <subsystem name="imu" type="analog">
|
||||
# <param name="GYRO_P" value="ADC_0"/>
|
||||
# <param name="GYRO_Q" value="ADC_1"/>
|
||||
# <param name="GYRO_R" value="ADC_2"/>
|
||||
# <param name="ACCEL_X" value="ADC_5"/>
|
||||
# <param name="ACCEL_Y" value="ADC_6"/>
|
||||
# <param name="ACCEL_Z" value="ADC_7"/>
|
||||
# </subsystem>
|
||||
#
|
||||
# required xml:
|
||||
# <section name="IMU" prefix="IMU_">
|
||||
#
|
||||
# <define name="GYRO_P_NEUTRAL" value="512"/>
|
||||
# <define name="GYRO_Q_NEUTRAL" value="512"/>
|
||||
# <define name="GYRO_R_NEUTRAL" value="512"/>
|
||||
#
|
||||
# <define name="GYRO_P_SENS" value="0.017" integer="16"/>
|
||||
# <define name="GYRO_Q_SENS" value="0.017" integer="16"/>
|
||||
# <define name="GYRO_R_SENS" value="0.017" 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_SENS" value="0.1" integer="16"/>
|
||||
# <define name="ACCEL_Y_SENS" value="0.1" integer="16"/>
|
||||
# <define name="ACCEL_Z_SENS" value="0.1" integer="16"/>
|
||||
#
|
||||
# <define name="ACCEL_X_NEUTRAL" value="512"/>
|
||||
# <define name="ACCEL_Y_NEUTRAL" value="512"/>
|
||||
# <define name="ACCEL_Z_NEUTRAL" value="512"/>
|
||||
#
|
||||
# <define name="ACCEL_X_SIGN" value="1"/>
|
||||
# <define name="ACCEL_Y_SIGN" value="-1"/>
|
||||
# <define name="ACCEL_Z_SIGN" value="1"/>
|
||||
#
|
||||
# </section>
|
||||
#
|
||||
|
||||
|
||||
ifeq ($(ARCH), lpc21)
|
||||
imu_CFLAGS += -DADC
|
||||
imu_CFLAGS += -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_ADC_$(GYRO_R)
|
||||
imu_CFLAGS += -DUSE_$(ACCEL_X) -DUSE_$(ACCEL_y) -DUSE_$(ACCEL_Z)
|
||||
|
||||
imu_CFLAGS += -DADC_CHANNEL_GYRO_P=$(GYRO_P) -DADC_CHANNEL_GYRO_Q=$(GYRO_Q) -DADC_CHANNEL_GYRO_R=$(GYRO_R)
|
||||
imu_CFLAGS += -DADC_CHANNEL_ACCEL_X=$(ACCEL_X) -DADC_CHANNEL_ACCEL_Y=$(ACCEL_Y) -DADC_CHANNEL_ACCEL_Z=$(ACCEL_Z)
|
||||
|
||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
|
||||
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_analog.c
|
||||
imu_srcs += math/pprz_trig_int.c
|
||||
|
||||
endif
|
||||
|
||||
# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets
|
||||
# see: conf/autopilot/subsystems/lisa_passthrough/imu_b2_v1.1.makefile for example
|
||||
ap.CFLAGS += $(imu_CFLAGS)
|
||||
ap.srcs += $(imu_srcs)
|
||||
@@ -46,9 +46,9 @@
|
||||
</dl_settings>-->
|
||||
|
||||
<dl_settings NAME="IMU">
|
||||
<dl_setting MAX="45" MIN="-45" STEP="0.1" VAR="imu_roll_neutral" shortname="roll neutral" module="subsystems/ahrs/dcm/analogimu" param="IMU_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
|
||||
<dl_setting MAX="45" MIN="-45" STEP="0.1" VAR="imu_pitch_neutral" shortname="pitch neutral" param="IMU_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
|
||||
|
||||
<dl_setting MAX="45" MIN="-45" STEP="0.1" VAR="imu_roll_neutral" shortname="roll neutral" module="subsystems/ahrs/ahrs_float_dcm" param="IMU_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
|
||||
<dl_setting MAX="45" MIN="-45" STEP="0.1" VAR="imu_pitch_neutral" shortname="pitch neutral" module="subsystems/ahrs/ahrs_float_dcm" param="IMU_NEUTRAL_DEFAULT" unit="rad" alt_unit="deg" alt_unit_coef="57.3"/>
|
||||
|
||||
</dl_settings>
|
||||
|
||||
|
||||
|
||||
@@ -65,8 +65,13 @@
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef ANALOG_IMU
|
||||
#include "subsystems/ahrs/dcm/analogimu.h"
|
||||
#ifdef USE_ANALOG_IMU
|
||||
#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
|
||||
|
||||
#if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY
|
||||
@@ -435,12 +440,11 @@ void periodic_task_ap( void ) {
|
||||
#error "Only 20 and 60 allowed for CONTROL_RATE"
|
||||
#endif
|
||||
|
||||
#ifdef ANALOG_IMU
|
||||
#ifdef USE_ANALOG_IMU
|
||||
if (!_20Hz) {
|
||||
estimator_update_state_analog_imu();
|
||||
analog_imu_downlink();
|
||||
}
|
||||
#endif // ANALOG_IMU
|
||||
imu_periodic();
|
||||
}
|
||||
#endif // USE_ANALOG_IMU
|
||||
|
||||
#if CONTROL_RATE == 20
|
||||
if (!_20Hz)
|
||||
@@ -495,9 +499,10 @@ void init_ap( void ) {
|
||||
GpioInit();
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef ANALOG_IMU
|
||||
analog_imu_init();
|
||||
#ifdef USE_ANALOG_IMU
|
||||
imu_init();
|
||||
ahrs_aligner_init();
|
||||
ahrs_init();
|
||||
#endif
|
||||
|
||||
/************* Links initialization ***************/
|
||||
@@ -548,18 +553,16 @@ void init_ap( void ) {
|
||||
#ifdef TRAFFIC_INFO
|
||||
traffic_info_init();
|
||||
#endif
|
||||
|
||||
#ifdef ANALOG_IMU
|
||||
//wait 10secs for init
|
||||
sys_time_usleep(10000000);
|
||||
analog_imu_offset_set();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/*********** EVENT ***********************************************************/
|
||||
void event_task_ap( void ) {
|
||||
|
||||
#ifdef USE_ANALOG_IMU
|
||||
ImuEvent(on_gyro_accel_event, on_mag_event);
|
||||
#endif // USE_ANALOG_IMU
|
||||
|
||||
#ifdef USE_GPS
|
||||
#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */
|
||||
if (GpsBuffer()) {
|
||||
@@ -630,3 +633,30 @@ void event_task_ap( void ) {
|
||||
|
||||
modules_event_task();
|
||||
} /* event_task_ap() */
|
||||
|
||||
#ifdef USE_ANALOG_IMU
|
||||
static inline void on_gyro_accel_event( void ) {
|
||||
ImuScaleGyro(imu);
|
||||
ImuScaleAccel(imu);
|
||||
if (ahrs.status == AHRS_UNINIT) {
|
||||
ahrs_aligner_run();
|
||||
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
|
||||
ahrs_align();
|
||||
}
|
||||
else {
|
||||
ahrs_propagate();
|
||||
ahrs_update_accel();
|
||||
ahrs_update_fw_estimator();
|
||||
}
|
||||
}
|
||||
|
||||
static inline void on_mag_event(void) {
|
||||
/*
|
||||
ImuScaleMag(imu);
|
||||
if (ahrs.status == AHRS_RUNNING) {
|
||||
ahrs_update_mag();
|
||||
ahrs_update_fw_estimator();
|
||||
}
|
||||
*/
|
||||
}
|
||||
#endif // USE_ANALOG_IMU
|
||||
|
||||
@@ -91,7 +91,7 @@ void ahrs_aligner_run(void) {
|
||||
ahrs_aligner.low_noise_cnt++;
|
||||
else
|
||||
if ( ahrs_aligner.low_noise_cnt > 0)
|
||||
ahrs_aligner.low_noise_cnt--;
|
||||
ahrs_aligner.low_noise_cnt--;
|
||||
|
||||
if (ahrs_aligner.low_noise_cnt > LOW_NOISE_TIME) {
|
||||
ahrs_aligner.status = AHRS_ALIGNER_LOCKED;
|
||||
|
||||
@@ -0,0 +1,403 @@
|
||||
/*
|
||||
* Released under Creative Commons License
|
||||
*
|
||||
* 2010 The Paparazzi Team
|
||||
*
|
||||
*
|
||||
* Based on Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
|
||||
* Version 1.0 for flat board updated by Doug Weibel and Jose Julio
|
||||
*
|
||||
* Modified at Hochschule Bremen, Germany
|
||||
* 2010 Heinrich Warmers, Christoph Niemann, Oliver Riesener
|
||||
*
|
||||
*/
|
||||
|
||||
/** \file ahrs_float_dcm.c
|
||||
* \brief Attitude estimation for fixedwings based on the DCM
|
||||
*
|
||||
*/
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#include "subsystems/ahrs.h"
|
||||
#include "subsystems/ahrs/ahrs_float_dcm.h"
|
||||
#include "subsystems/ahrs/ahrs_float_utils.h"
|
||||
#include "subsystems/ahrs/ahrs_aligner.h"
|
||||
#include "subsystems/imu.h"
|
||||
|
||||
#include "subsystems/ahrs/ahrs_float_dcm_algebra.h"
|
||||
|
||||
#ifdef USE_GPS
|
||||
#include "gps.h"
|
||||
#endif
|
||||
|
||||
#include <string.h>
|
||||
|
||||
//FIXME this is still needed for fixedwing integration
|
||||
#include "estimator.h"
|
||||
|
||||
struct AhrsFloatDCM ahrs_impl;
|
||||
|
||||
// remotely settable
|
||||
float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT);
|
||||
float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
|
||||
|
||||
// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
|
||||
// Positive pitch : nose up
|
||||
// Positive roll : right wing down
|
||||
// Positive yaw : clockwise
|
||||
|
||||
// DCM Working variables
|
||||
float G_Dt=0.05;
|
||||
|
||||
struct FloatVect3 accel_float = {0,0,0};
|
||||
|
||||
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
|
||||
float Omega_P[3]= {0,0,0}; //Omega Proportional correction
|
||||
float Omega_I[3]= {0,0,0}; //Omega Integrator
|
||||
float Omega[3]= {0,0,0};
|
||||
|
||||
float DCM_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
|
||||
float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
|
||||
float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
|
||||
#ifdef USE_MAGNETOMETER
|
||||
float MAG_Heading;
|
||||
#endif
|
||||
|
||||
static inline void compute_body_orientation_and_rates(void);
|
||||
void Normalize(void);
|
||||
void Drift_correction(void);
|
||||
void Euler_angles(void);
|
||||
void Matrix_update(void);
|
||||
|
||||
/**************************************************/
|
||||
|
||||
void ahrs_update_fw_estimator( void )
|
||||
{
|
||||
Euler_angles();
|
||||
|
||||
//warning, only eulers written to ahrs struct so far
|
||||
//compute_body_orientation_and_rates();
|
||||
|
||||
// export results to estimator
|
||||
estimator_phi = ahrs_float.ltp_to_imu_euler.phi - imu_roll_neutral;
|
||||
estimator_theta = ahrs_float.ltp_to_imu_euler.theta - imu_pitch_neutral;
|
||||
estimator_psi = ahrs_float.ltp_to_imu_euler.psi;
|
||||
}
|
||||
|
||||
|
||||
void ahrs_init(void) {
|
||||
ahrs_float.status = AHRS_UNINIT;
|
||||
|
||||
/*
|
||||
* Initialises our IMU alignement variables
|
||||
* This should probably done in the IMU code instead
|
||||
*/
|
||||
struct FloatEulers body_to_imu_euler =
|
||||
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
|
||||
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
|
||||
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
|
||||
|
||||
/* set ltp_to_body to zero */
|
||||
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
|
||||
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
|
||||
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
|
||||
FLOAT_RATES_ZERO(ahrs_float.body_rate);
|
||||
|
||||
/* set ltp_to_imu so that body is zero */
|
||||
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
|
||||
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
|
||||
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
|
||||
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
|
||||
}
|
||||
|
||||
void ahrs_align(void)
|
||||
{
|
||||
/* Compute an initial orientation using euler angles */
|
||||
ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
|
||||
|
||||
/* Convert initial orientation in quaternion and rotation matrice representations. */
|
||||
FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
|
||||
FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
|
||||
|
||||
/* Compute initial body orientation */
|
||||
compute_body_orientation_and_rates();
|
||||
|
||||
/* use averaged gyro as initial value for bias */
|
||||
struct Int32Rates bias0;
|
||||
RATES_COPY(bias0, ahrs_aligner.lp_gyro);
|
||||
RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0);
|
||||
|
||||
ahrs.status = AHRS_RUNNING;
|
||||
}
|
||||
|
||||
|
||||
void ahrs_propagate(void)
|
||||
{
|
||||
/* convert imu data to floating point */
|
||||
struct FloatRates gyro_float;
|
||||
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
|
||||
|
||||
/* unbias rate measurement */
|
||||
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
|
||||
|
||||
Matrix_update();
|
||||
Normalize();
|
||||
//INFO, ahrs struct only updated in ahrs_update_fw_estimator
|
||||
}
|
||||
|
||||
void ahrs_update_accel(void)
|
||||
{
|
||||
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
|
||||
|
||||
#ifdef USE_GPS
|
||||
if (gps_mode==3) { //Remove centrifugal acceleration.
|
||||
accel_float.y += gps_speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
|
||||
accel_float.z -= gps_speed_3d/100. * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
|
||||
}
|
||||
#endif
|
||||
|
||||
Drift_correction();
|
||||
}
|
||||
|
||||
void ahrs_update_mag(void)
|
||||
{
|
||||
//TODO
|
||||
}
|
||||
|
||||
void Normalize(void)
|
||||
{
|
||||
float error=0;
|
||||
float temporary[3][3];
|
||||
float renorm=0;
|
||||
boolean problem=FALSE;
|
||||
|
||||
// Find the non-orthogonality of X wrt Y
|
||||
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
|
||||
|
||||
// Add half the XY error to X, and half to Y
|
||||
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
|
||||
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
|
||||
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
|
||||
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
|
||||
|
||||
// The third axis is simply set perpendicular to the first 2. (there is not correction of XY based on Z)
|
||||
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
|
||||
|
||||
// Normalize lenght of X
|
||||
renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
|
||||
// a) if norm is close to 1, use the fast 1st element from the tailer expansion of SQRT
|
||||
// b) if the norm is further from 1, use a real sqrt
|
||||
// c) norm is huge: disaster! reset! mayday!
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
|
||||
|
||||
// Normalize lenght of Y
|
||||
renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
|
||||
|
||||
// Normalize lenght of Z
|
||||
renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
|
||||
|
||||
// Reset on trouble
|
||||
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
|
||||
DCM_Matrix[0][0]= 1.0f;
|
||||
DCM_Matrix[0][1]= 0.0f;
|
||||
DCM_Matrix[0][2]= 0.0f;
|
||||
DCM_Matrix[1][0]= 0.0f;
|
||||
DCM_Matrix[1][1]= 1.0f;
|
||||
DCM_Matrix[1][2]= 0.0f;
|
||||
DCM_Matrix[2][0]= 0.0f;
|
||||
DCM_Matrix[2][1]= 0.0f;
|
||||
DCM_Matrix[2][2]= 1.0f;
|
||||
problem = FALSE;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Drift_correction(void)
|
||||
{
|
||||
//Compensation the Roll, Pitch and Yaw drift.
|
||||
static float Scaled_Omega_P[3];
|
||||
static float Scaled_Omega_I[3];
|
||||
float Accel_magnitude;
|
||||
float Accel_weight;
|
||||
float Integrator_magnitude;
|
||||
|
||||
// Local Working Variables
|
||||
float errorRollPitch[3];
|
||||
float errorYaw[3];
|
||||
float errorCourse;
|
||||
|
||||
//*****Roll and Pitch***************
|
||||
|
||||
// Calculate the magnitude of the accelerometer vector
|
||||
Accel_magnitude = sqrt(accel_float.x*accel_float.x + accel_float.y*accel_float.y + accel_float.z*accel_float.z);
|
||||
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
|
||||
// Dynamic weighting of accelerometer info (reliability filter)
|
||||
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
|
||||
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
|
||||
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
{
|
||||
|
||||
float tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
|
||||
imu_health += tempfloat;
|
||||
Bound(imu_health,129,65405);
|
||||
}
|
||||
#endif
|
||||
|
||||
Vector_Cross_Product(&errorRollPitch[0],&accel_float.x,&DCM_Matrix[2][0]); //adjust the ground of reference
|
||||
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
|
||||
|
||||
//*****YAW***************
|
||||
|
||||
#ifdef USE_MAGNETOMETER
|
||||
// We make the gyro YAW drift correction based on compass magnetic heading
|
||||
float mag_heading_x = cos(MAG_Heading);
|
||||
float mag_heading_y = sin(MAG_Heading);
|
||||
errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
||||
|
||||
#elif defined USE_GPS // Use GPS Ground course to correct yaw gyro drift
|
||||
|
||||
if(gps_mode==3 && gps_gspeed>= 500) { //got a 3d fix and ground speed is more than 0.5 m/s
|
||||
float ground_course = gps_course/10. - 180.; //This is the runaway direction of you "plane" in degrees
|
||||
float COGX = cos(RadOfDeg(ground_course)); //Course overground X axis
|
||||
float COGY = sin(RadOfDeg(ground_course)); //Course overground Y axis
|
||||
|
||||
errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
||||
}
|
||||
#endif
|
||||
|
||||
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
|
||||
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
|
||||
if (Integrator_magnitude > DegOfRad(300)) {
|
||||
Vector_Scale(Omega_I,Omega_I,0.5f*DegOfRad(300)/Integrator_magnitude);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
/**************************************************/
|
||||
|
||||
void Matrix_update(void)
|
||||
{
|
||||
Vector_Add(&Omega[0], &ahrs_float.imu_rate.p, &Omega_I[0]); //adding proportional term
|
||||
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
|
||||
|
||||
#if OUTPUTMODE==1 // With corrected data (drift correction)
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
|
||||
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
|
||||
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
|
||||
Update_Matrix[2][2]=0;
|
||||
#else // Uncorrected data (no drift correction)
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*ahrs_float.imu_rate.r;//-z
|
||||
Update_Matrix[0][2]=G_Dt*ahrs_float.imu_rate.q;//y
|
||||
Update_Matrix[1][0]=G_Dt*ahrs_float.imu_rate.r;//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*ahrs_float.imu_rate.p;
|
||||
Update_Matrix[2][0]=-G_Dt*ahrs_float.imu_rate.q;
|
||||
Update_Matrix[2][1]=G_Dt*ahrs_float.imu_rate.p;
|
||||
Update_Matrix[2][2]=0;
|
||||
#endif
|
||||
|
||||
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
|
||||
|
||||
for(int x=0; x<3; x++) //Matrix Addition (update)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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.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
|
||||
}
|
||||
|
||||
/*
|
||||
* Compute body orientation and rates from imu orientation and rates
|
||||
*/
|
||||
static inline void compute_body_orientation_and_rates(void) {
|
||||
|
||||
FLOAT_QUAT_COMP_INV(ahrs_float.ltp_to_body_quat,
|
||||
ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
|
||||
FLOAT_RMAT_COMP_INV(ahrs_float.ltp_to_body_rmat,
|
||||
ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
|
||||
FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_body_euler, ahrs_float.ltp_to_body_rmat);
|
||||
FLOAT_RMAT_TRANSP_RATEMULT(ahrs_float.body_rate, ahrs_impl.body_to_imu_rmat, ahrs_float.imu_rate);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
/*
|
||||
* Released under Creative Commons License
|
||||
*
|
||||
* 2010 The Paparazzi Team
|
||||
*
|
||||
*
|
||||
* Based on Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
|
||||
* Version 1.0 for flat board updated by Doug Weibel and Jose Julio
|
||||
*
|
||||
* Modified at Hochschule Bremen, Germany
|
||||
* 2010 Heinrich Warmers, Christoph Niemann, Oliver Riesener
|
||||
*
|
||||
*/
|
||||
|
||||
/** \file ahrs_float_dcm.h
|
||||
* \brief Attitude estimation for fixedwings based on the DCM
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef AHRS_FLOAT_DCM_H
|
||||
#define AHRS_FLOAT_DCM_H
|
||||
|
||||
#include <inttypes.h>
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
struct AhrsFloatDCM {
|
||||
struct FloatRates gyro_bias;
|
||||
struct FloatRates rate_correction;
|
||||
/*
|
||||
Holds float version of IMU alignement
|
||||
in order to be able to run against the fixed point
|
||||
version of the IMU
|
||||
*/
|
||||
struct FloatQuat body_to_imu_quat;
|
||||
struct FloatRMat body_to_imu_rmat;
|
||||
};
|
||||
extern struct AhrsFloatDCM ahrs_impl;
|
||||
|
||||
extern float imu_roll_neutral;
|
||||
extern float imu_pitch_neutral;
|
||||
|
||||
void ahrs_update_fw_estimator(void);
|
||||
|
||||
// DCM Parameters
|
||||
|
||||
//#define Kp_ROLLPITCH 0.2
|
||||
#define Kp_ROLLPITCH 0.015
|
||||
#define Ki_ROLLPITCH 0.000010
|
||||
#define Kp_YAW 1.2 //High yaw drift correction gain - use with caution!
|
||||
#define Ki_YAW 0.00005
|
||||
|
||||
#define GRAVITY 9.81
|
||||
|
||||
|
||||
#define OUTPUTMODE 1
|
||||
// Mode 0 = DCM integration without Ki gyro bias
|
||||
// Mode 1 = DCM integration with Kp and Ki
|
||||
// Mode 2 = direct accelerometer -> euler
|
||||
|
||||
|
||||
#define PERFORMANCE_REPORTING 0
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
extern int renorm_sqrt_count;
|
||||
extern int renorm_blowup_count;
|
||||
extern float imu_health;
|
||||
#endif
|
||||
|
||||
#endif // AHRS_FLOAT_DCM_H
|
||||
@@ -0,0 +1,57 @@
|
||||
//Algebra helper functions for DCM
|
||||
|
||||
static inline float Vector_Dot_Product(float vector1[3],float vector2[3])
|
||||
{
|
||||
return vector1[0]*vector2[0] + vector1[1]*vector2[1] + vector1[2]*vector2[2];
|
||||
}
|
||||
|
||||
static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
|
||||
{
|
||||
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
|
||||
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
|
||||
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
|
||||
}
|
||||
|
||||
static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
|
||||
{
|
||||
vectorOut[0]=vectorIn[0]*scale2;
|
||||
vectorOut[1]=vectorIn[1]*scale2;
|
||||
vectorOut[2]=vectorIn[2]*scale2;
|
||||
}
|
||||
|
||||
static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
|
||||
{
|
||||
vectorOut[0]=vectorIn1[0]+vectorIn2[0];
|
||||
vectorOut[1]=vectorIn1[1]+vectorIn2[1];
|
||||
vectorOut[2]=vectorIn1[2]+vectorIn2[2];
|
||||
}
|
||||
|
||||
/*
|
||||
#define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \
|
||||
_m_a2c[0] = (_m_b2c[0]*_m_a2b[0] + _m_b2c[1]*_m_a2b[3] + _m_b2c[2]*_m_a2b[6]); \
|
||||
_m_a2c[1] = (_m_b2c[0]*_m_a2b[1] + _m_b2c[1]*_m_a2b[4] + _m_b2c[2]*_m_a2b[7]); \
|
||||
_m_a2c[2] = (_m_b2c[0]*_m_a2b[2] + _m_b2c[1]*_m_a2b[5] + _m_b2c[2]*_m_a2b[8]); \
|
||||
_m_a2c[3] = (_m_b2c[3]*_m_a2b[0] + _m_b2c[4]*_m_a2b[3] + _m_b2c[5]*_m_a2b[6]); \
|
||||
_m_a2c[4] = (_m_b2c[3]*_m_a2b[1] + _m_b2c[4]*_m_a2b[4] + _m_b2c[5]*_m_a2b[7]); \
|
||||
_m_a2c[5] = (_m_b2c[3]*_m_a2b[2] + _m_b2c[4]*_m_a2b[5] + _m_b2c[5]*_m_a2b[8]); \
|
||||
_m_a2c[6] = (_m_b2c[6]*_m_a2b[0] + _m_b2c[7]*_m_a2b[3] + _m_b2c[8]*_m_a2b[6]); \
|
||||
_m_a2c[7] = (_m_b2c[6]*_m_a2b[1] + _m_b2c[7]*_m_a2b[4] + _m_b2c[8]*_m_a2b[7]); \
|
||||
_m_a2c[8] = (_m_b2c[6]*_m_a2b[2] + _m_b2c[7]*_m_a2b[5] + _m_b2c[8]*_m_a2b[8]); \
|
||||
}
|
||||
*/
|
||||
|
||||
static inline void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
|
||||
{
|
||||
float op[3];
|
||||
for(int x=0; x<3; x++)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
for(int w=0; w<3; w++)
|
||||
{
|
||||
op[w]=a[x][w]*b[w][y];
|
||||
}
|
||||
mat[x][y]=op[0]+op[1]+op[2];
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,164 +0,0 @@
|
||||
/*
|
||||
* $Id: analogimu.c $
|
||||
*
|
||||
* Copyright (C) 2010 Oliver Riesener, Christoph Niemann
|
||||
*
|
||||
* 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 analogimu.c
|
||||
* \brief Analog IMU Routines
|
||||
*
|
||||
*/
|
||||
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#if ! (defined SITL || defined HITL)
|
||||
|
||||
|
||||
// Actual Inertial Measurements
|
||||
#include "subsystems/imu/imu_analog.h"
|
||||
|
||||
// AHRS attitude computations
|
||||
#include "dcm.h"
|
||||
#include "estimator.h"
|
||||
#include "analogimu_util.h"
|
||||
#include "analogimu.h"
|
||||
|
||||
// Debugging and output
|
||||
#include "led.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
//#include "downlink.h"
|
||||
//#include "ap_downlink.h"
|
||||
#include "sys_time.h"
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
// variables
|
||||
uint16_t analog_imu_offset[NB_ANALOG_IMU_ADC];
|
||||
int adc_average[NB_ANALOG_IMU_ADC];
|
||||
|
||||
// remotely settable
|
||||
float imu_roll_neutral = RadOfDeg(IMU_ROLL_NEUTRAL_DEFAULT);
|
||||
float imu_pitch_neutral = RadOfDeg(IMU_PITCH_NEUTRAL_DEFAULT);
|
||||
|
||||
#if ! (defined SITL || defined HITL)
|
||||
|
||||
// functions
|
||||
/**
|
||||
* accel2ms2():
|
||||
*
|
||||
* \return accel[ACC_X], accel[ACC_Y], accel[ACC_Z]
|
||||
*/
|
||||
static void accel2ms2( void ) {
|
||||
accel[ACC_X] = (float)(adc_average[3]) * IMU_ACCEL_X_SENS;
|
||||
accel[ACC_Y] = (float)(adc_average[4]) * IMU_ACCEL_Y_SENS;
|
||||
accel[ACC_Z] = (float)(adc_average[5]) * IMU_ACCEL_Z_SENS;
|
||||
}
|
||||
/**
|
||||
* gyro2rads():
|
||||
*
|
||||
* \return gyro[G_ROLL], gyro[G_PITCH], gyro[G_YAW]
|
||||
*/
|
||||
static void gyro2rads( void ) {
|
||||
/** 150 grad/sec 10Bit, 3,3Volt, 1rad = 2Pi/1024 => Pi/512 */
|
||||
gyro[G_ROLL] = (float)(adc_average[0]) * IMU_GYRO_P_SENS;
|
||||
gyro[G_PITCH] = (float)(adc_average[1]) * IMU_GYRO_Q_SENS;
|
||||
gyro[G_YAW] = (float)(adc_average[2]) * IMU_GYRO_R_SENS;
|
||||
}
|
||||
|
||||
void analog_imu_init( void ) {
|
||||
imu_impl_init();
|
||||
}
|
||||
|
||||
void analog_imu_offset_set( void ) {
|
||||
uint8_t i;
|
||||
|
||||
// read IMU
|
||||
imu_periodic();
|
||||
|
||||
for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
|
||||
analog_imu_offset[i] = analog_imu_values[i];
|
||||
}
|
||||
|
||||
// Z channel should read
|
||||
analog_imu_offset[5] += (9.81f / IMU_ACCEL_Z_SENS);
|
||||
}
|
||||
/**
|
||||
* analog_imu_update():
|
||||
*/
|
||||
|
||||
void analog_imu_update( void ) {
|
||||
uint8_t i;
|
||||
|
||||
// read IMU
|
||||
imu_periodic();
|
||||
|
||||
for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
|
||||
adc_average[i] -= analog_imu_offset[i];
|
||||
}
|
||||
|
||||
accel2ms2();
|
||||
gyro2rads();
|
||||
}
|
||||
|
||||
// functions
|
||||
|
||||
void analog_imu_downlink( void ) {
|
||||
//uint8_t id = 0;
|
||||
//float time = GET_CUR_TIME_FLOAT();
|
||||
//time *= 1000;//secs to msecs
|
||||
//int mx = 0;
|
||||
//int my = 0;
|
||||
//int mz = 0;
|
||||
//DOWNLINK_SEND_HB_FILTER( DefaultChannel,&time, &accel[ACC_X],&accel[ACC_Y],&accel[ACC_Z],&gyro[G_ROLL],&gyro[G_PITCH],&gyro[G_YAW],&heading,&mx,&my,&mz,&euler[EULER_ROLL],&euler[EULER_PITCH],&euler[EULER_YAW], &imu_roll_neutral, &imu_pitch_neutral );
|
||||
}
|
||||
|
||||
|
||||
void estimator_update_state_analog_imu( void ) {
|
||||
|
||||
analog_imu_update();
|
||||
|
||||
/* Offset is set dynamic on Ground*/
|
||||
Gyro_Vector[0]= -gyro_to_zero[G_ROLL] + gyro[G_ROLL];
|
||||
Gyro_Vector[1]= -gyro_to_zero[G_PITCH] + gyro[G_PITCH];
|
||||
Gyro_Vector[2]= -gyro_to_zero[G_PITCH] + gyro[G_YAW];
|
||||
|
||||
Accel_Vector[0] = accel[ACC_X];
|
||||
Accel_Vector[1] = accel[ACC_Y];
|
||||
Accel_Vector[2] = accel[ACC_Z];
|
||||
|
||||
|
||||
Matrix_update();
|
||||
Normalize();
|
||||
|
||||
|
||||
Drift_correction();
|
||||
Euler_angles();
|
||||
|
||||
// return euler angles to phi and theta
|
||||
estimator_phi = euler.phi-imu_roll_neutral;
|
||||
//estimator_phi = angle[ANG_ROLL];
|
||||
estimator_theta = euler.theta-imu_pitch_neutral;
|
||||
//estimator_theta = angle[ANG_PITCH];
|
||||
estimator_psi = euler.psi;
|
||||
|
||||
}
|
||||
#endif
|
||||
@@ -1,46 +0,0 @@
|
||||
/*
|
||||
* $Id: analogimu.h $
|
||||
*
|
||||
* Copyright (C) 2010 Oliver Riesener, Christoph Niemann
|
||||
*
|
||||
* 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 analogimu.h
|
||||
* \brief Analog IMU Interface
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef _ANALOGIMU_H_
|
||||
#define _ANALOGIMU_H_
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
extern float imu_roll_neutral;
|
||||
extern float imu_pitch_neutral;
|
||||
|
||||
//functions
|
||||
void analog_imu_init( void );
|
||||
void analog_imu_update( void );
|
||||
void analog_imu_downlink( void );
|
||||
void analogconversion( void );
|
||||
void estimator_update_state_analog_imu( void );
|
||||
void analog_imu_offset_set( void );
|
||||
|
||||
#endif // _ANALOGIMU_H_
|
||||
@@ -1,100 +0,0 @@
|
||||
/*
|
||||
* $Id: analogimu_util.c $
|
||||
*
|
||||
* Copyright (C) 2010 Christoph Niemann
|
||||
*
|
||||
* 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 analogimu_util.c
|
||||
* \brief Analog IMU Utilities
|
||||
*
|
||||
*/
|
||||
|
||||
#include "mcu_periph/adc.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
|
||||
#include "std.h"
|
||||
#include "subsystems/nav.h"
|
||||
#include "estimator.h"
|
||||
#include "autopilot.h"
|
||||
#include "generated/flight_plan.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "inter_mcu.h"
|
||||
|
||||
#include "analogimu_util.h"
|
||||
|
||||
|
||||
uint16_t analog_raw[NB_ADC];
|
||||
|
||||
// variables
|
||||
/** gyro rate in rad */
|
||||
volatile float gyro[G_LAST] = {0.};
|
||||
/** gyro rate offset in rad */
|
||||
volatile float gyro_to_zero[G_LAST] = {0.};
|
||||
/** acceleration in ms2 */
|
||||
volatile float accel[ACC_LAST] = {0.};
|
||||
/** angle in rad */
|
||||
volatile float angle[ANG_LAST] = {0.};
|
||||
/** magnet heading \todo heading ? */
|
||||
volatile float heading;
|
||||
|
||||
void analogimu_delay( void ) {
|
||||
volatile int i,j;
|
||||
for (i=0;i<1000;i++)
|
||||
for (j=0;j<1000;j++);
|
||||
}
|
||||
|
||||
bool_t analog_imu_reset( void ) {
|
||||
#ifndef ANALOGIMU_ZERO_AVERAGE
|
||||
gyro_to_zero[GO_ROLL] = gyro[G_ROLL];
|
||||
gyro_to_zero[GO_PITCH] = gyro[G_PITCH];
|
||||
gyro_to_zero[GO_YAW] = gyro[G_YAW];
|
||||
#else
|
||||
/** temp_value for calculating the average */
|
||||
volatile float gyro_to_zero_temp[G_LAST] = {0.};
|
||||
|
||||
gyro_to_zero_temp[GO_ROLL] = gyro[G_ROLL];
|
||||
gyro_to_zero_temp[GO_PITCH] = gyro[G_PITCH];
|
||||
gyro_to_zero_temp[GO_YAW] = gyro[G_YAW];
|
||||
|
||||
analogimu_delay();
|
||||
|
||||
gyro_to_zero_temp[GO_ROLL] += gyro[G_ROLL];
|
||||
gyro_to_zero_temp[GO_PITCH] += gyro[G_PITCH];
|
||||
gyro_to_zero_temp[GO_YAW] += gyro[G_YAW];
|
||||
|
||||
analogimu_delay();
|
||||
|
||||
gyro_to_zero_temp[GO_ROLL] += gyro[G_ROLL];
|
||||
gyro_to_zero_temp[GO_PITCH] += gyro[G_PITCH];
|
||||
gyro_to_zero_temp[GO_YAW] += gyro[G_YAW];
|
||||
|
||||
gyro_to_zero_temp[GO_ROLL] /= 3;
|
||||
gyro_to_zero_temp[GO_PITCH] /= 3;
|
||||
gyro_to_zero_temp[GO_YAW] /= 3;
|
||||
|
||||
gyro_to_zero[GO_ROLL] = gyro_to_zero_temp[G_ROLL];
|
||||
gyro_to_zero[GO_PITCH] = gyro_to_zero_temp[G_PITCH];
|
||||
gyro_to_zero[GO_YAW] = gyro_to_zero_temp[G_YAW];
|
||||
#endif
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,66 +0,0 @@
|
||||
/*
|
||||
* $Id: analogimu_imu.h $
|
||||
*
|
||||
* Copyright (C) 2010 Christoph Niemann
|
||||
*
|
||||
* 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 analogimu_util.h
|
||||
* \brief Analog IMU Utilities
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef _ANALOGIMU_UTIL_H_
|
||||
#define _ANALOGIMU_UTIL_H_
|
||||
|
||||
#include "std.h"
|
||||
|
||||
// defines
|
||||
/** defines for gyro[] indicies */
|
||||
enum gyro_idx_t { G_ROLL, G_PITCH, G_YAW, G_LAST };
|
||||
/** defines for accel[] indicies */
|
||||
enum accel_idx_t { ACC_X, ACC_Y, ACC_Z, ACC_LAST };
|
||||
/** defines for stick[] indicies */
|
||||
enum stick_idx_t { STICK_ROLL, STICK_PITCH, STICK_YAW, STICK_THRUST,
|
||||
STICK_SWR, STICK_SWL, STICK_CH6, STICK_CH7, STICK_LAST };
|
||||
/** defines for angle[] indicies */
|
||||
enum angle_idx_t { ANG_ROLL, ANG_PITCH, ANG_YAW, ANG_LAST };
|
||||
/** defines for gyro_to_zero[] indicies */
|
||||
enum gyro_to_zero_idx_t { GO_ROLL, GO_PITCH, GO_YAW, GO_LAST };
|
||||
|
||||
// variables
|
||||
extern volatile float gyro[];
|
||||
/** acceleration in ms2 */
|
||||
extern volatile float accel[];
|
||||
/** angle in rad */
|
||||
extern volatile float angle[];
|
||||
/** magnet heading \todo heading ? */
|
||||
extern volatile float heading;
|
||||
/** analog_raw[] analog downlink arry */
|
||||
extern uint16_t analog_raw[];
|
||||
|
||||
extern volatile float gyro_to_zero[];
|
||||
|
||||
extern bool_t analog_imu_reset( void );
|
||||
|
||||
void analogimu_delay( void );
|
||||
|
||||
|
||||
#endif
|
||||
@@ -1,330 +0,0 @@
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#include "dcm.h"
|
||||
|
||||
// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
|
||||
// Positive pitch : nose up
|
||||
// Positive roll : right wing down
|
||||
// Positive yaw : clockwise
|
||||
|
||||
|
||||
// DCM Working variables
|
||||
float G_Dt=0.05;
|
||||
|
||||
float Gyro_Vector[3] = {0,0,0};
|
||||
float Accel_Vector[3] = {0,0,0};
|
||||
|
||||
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
|
||||
float Omega_P[3]= {0,0,0}; //Omega Proportional correction
|
||||
float Omega_I[3]= {0,0,0}; //Omega Integrator
|
||||
float Omega[3]= {0,0,0};
|
||||
|
||||
float DCM_Matrix[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
|
||||
float Update_Matrix[3][3] = {{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
|
||||
float Temporary_Matrix[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
|
||||
struct FloatEulers euler= {0, 0, 0};
|
||||
float speed_3d = 0;
|
||||
|
||||
|
||||
/**************************************************/
|
||||
|
||||
// Algebra
|
||||
|
||||
static inline float Vector_Dot_Product(float vector1[3],float vector2[3])
|
||||
{
|
||||
return vector1[0]*vector2[0] + vector1[1]*vector2[1] + vector1[2]*vector2[2];
|
||||
}
|
||||
|
||||
static inline void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
|
||||
{
|
||||
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
|
||||
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
|
||||
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
|
||||
}
|
||||
|
||||
static inline void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
|
||||
{
|
||||
vectorOut[0]=vectorIn[0]*scale2;
|
||||
vectorOut[1]=vectorIn[1]*scale2;
|
||||
vectorOut[2]=vectorIn[2]*scale2;
|
||||
}
|
||||
|
||||
static inline void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
|
||||
{
|
||||
vectorOut[0]=vectorIn1[0]+vectorIn2[0];
|
||||
vectorOut[1]=vectorIn1[1]+vectorIn2[1];
|
||||
vectorOut[2]=vectorIn1[2]+vectorIn2[2];
|
||||
}
|
||||
|
||||
/*
|
||||
#define Matrix_Multiply( _m_a2b, _m_b2c, _m_a2c) { \
|
||||
_m_a2c[0] = (_m_b2c[0]*_m_a2b[0] + _m_b2c[1]*_m_a2b[3] + _m_b2c[2]*_m_a2b[6]); \
|
||||
_m_a2c[1] = (_m_b2c[0]*_m_a2b[1] + _m_b2c[1]*_m_a2b[4] + _m_b2c[2]*_m_a2b[7]); \
|
||||
_m_a2c[2] = (_m_b2c[0]*_m_a2b[2] + _m_b2c[1]*_m_a2b[5] + _m_b2c[2]*_m_a2b[8]); \
|
||||
_m_a2c[3] = (_m_b2c[3]*_m_a2b[0] + _m_b2c[4]*_m_a2b[3] + _m_b2c[5]*_m_a2b[6]); \
|
||||
_m_a2c[4] = (_m_b2c[3]*_m_a2b[1] + _m_b2c[4]*_m_a2b[4] + _m_b2c[5]*_m_a2b[7]); \
|
||||
_m_a2c[5] = (_m_b2c[3]*_m_a2b[2] + _m_b2c[4]*_m_a2b[5] + _m_b2c[5]*_m_a2b[8]); \
|
||||
_m_a2c[6] = (_m_b2c[6]*_m_a2b[0] + _m_b2c[7]*_m_a2b[3] + _m_b2c[8]*_m_a2b[6]); \
|
||||
_m_a2c[7] = (_m_b2c[6]*_m_a2b[1] + _m_b2c[7]*_m_a2b[4] + _m_b2c[8]*_m_a2b[7]); \
|
||||
_m_a2c[8] = (_m_b2c[6]*_m_a2b[2] + _m_b2c[7]*_m_a2b[5] + _m_b2c[8]*_m_a2b[8]); \
|
||||
}
|
||||
*/
|
||||
|
||||
static inline void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
|
||||
{
|
||||
float op[3];
|
||||
for(int x=0; x<3; x++)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
for(int w=0; w<3; w++)
|
||||
{
|
||||
op[w]=a[x][w]*b[w][y];
|
||||
}
|
||||
mat[x][y]=op[0]+op[1]+op[2];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void Normalize(void)
|
||||
{
|
||||
float error=0;
|
||||
float temporary[3][3];
|
||||
float renorm=0;
|
||||
boolean problem=FALSE;
|
||||
|
||||
error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
|
||||
|
||||
Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
|
||||
Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
|
||||
|
||||
Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]); //eq.19
|
||||
Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]); //eq.19
|
||||
|
||||
Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
|
||||
|
||||
renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
|
||||
|
||||
renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
|
||||
|
||||
renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]);
|
||||
if (renorm < 1.5625f && renorm > 0.64f) {
|
||||
renorm= .5 * (3-renorm); //eq.21
|
||||
} else if (renorm < 100.0f && renorm > 0.01f) {
|
||||
renorm= 1. / sqrt(renorm);
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_sqrt_count++;
|
||||
#endif
|
||||
} else {
|
||||
problem = TRUE;
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
renorm_blowup_count++;
|
||||
#endif
|
||||
}
|
||||
Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
|
||||
|
||||
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
|
||||
DCM_Matrix[0][0]= 1.0f;
|
||||
DCM_Matrix[0][1]= 0.0f;
|
||||
DCM_Matrix[0][2]= 0.0f;
|
||||
DCM_Matrix[1][0]= 0.0f;
|
||||
DCM_Matrix[1][1]= 1.0f;
|
||||
DCM_Matrix[1][2]= 0.0f;
|
||||
DCM_Matrix[2][0]= 0.0f;
|
||||
DCM_Matrix[2][1]= 0.0f;
|
||||
DCM_Matrix[2][2]= 1.0f;
|
||||
problem = FALSE;
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************************/
|
||||
extern short gps_course;
|
||||
extern short gps_gspeed;
|
||||
extern short gps_climb;
|
||||
extern short gps_mode;
|
||||
|
||||
#ifdef USE_MAGNETOMETER
|
||||
float MAG_Heading;
|
||||
#endif
|
||||
|
||||
|
||||
void Drift_correction(void)
|
||||
{
|
||||
//Compensation the Roll, Pitch and Yaw drift.
|
||||
static float Scaled_Omega_P[3];
|
||||
static float Scaled_Omega_I[3];
|
||||
float Accel_magnitude;
|
||||
float Accel_weight;
|
||||
float Integrator_magnitude;
|
||||
|
||||
// Local Working Variables
|
||||
float errorRollPitch[3];
|
||||
float errorYaw[3];
|
||||
float errorCourse;
|
||||
float ground_speed; // This is the velocity your "plane" is traveling in meters for second, 1Meters/Second= 3.6Km/H = 1.944 knots
|
||||
float ground_course; //This is the runaway direction of you "plane" in degrees
|
||||
float COGX; //Course overground X axis
|
||||
float COGY; //Course overground Y axis
|
||||
|
||||
// hwarm
|
||||
ground_course=gps_course/10.-180.;
|
||||
ground_speed=gps_gspeed/100.;
|
||||
float ground_climb=gps_climb/100.;
|
||||
speed_3d = sqrt(ground_speed*ground_speed+ground_climb*ground_climb);
|
||||
//*****Roll and Pitch***************
|
||||
|
||||
// Calculate the magnitude of the accelerometer vector
|
||||
Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
|
||||
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
|
||||
// Dynamic weighting of accelerometer info (reliability filter)
|
||||
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
|
||||
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
|
||||
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
{
|
||||
|
||||
float tempfloat = ((Accel_weight - 0.5) * 256.0f); //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
|
||||
imu_health += tempfloat;
|
||||
Bound(imu_health,129,65405);
|
||||
}
|
||||
#endif
|
||||
|
||||
Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
|
||||
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
|
||||
|
||||
//*****YAW***************
|
||||
|
||||
#if USE_MAGNETOMETER==1
|
||||
// We make the gyro YAW drift correction based on compass magnetic heading
|
||||
mag_heading_x = cos(MAG_Heading);
|
||||
mag_heading_y = sin(MAG_Heading);
|
||||
errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
||||
#else // Use GPS Ground course to correct yaw gyro drift
|
||||
if(gps_mode==3 && ground_speed>= 0.5) //hwarm
|
||||
{
|
||||
|
||||
COGX = cos(RadOfDeg(ground_course));
|
||||
COGY = sin(RadOfDeg(ground_course));
|
||||
errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX); //Calculating YAW error
|
||||
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
|
||||
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
|
||||
|
||||
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
|
||||
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
|
||||
}
|
||||
#endif
|
||||
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
|
||||
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
|
||||
if (Integrator_magnitude > DegOfRad(300)) {
|
||||
Vector_Scale(Omega_I,Omega_I,0.5f*DegOfRad(300)/Integrator_magnitude);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
/**************************************************/
|
||||
|
||||
void Matrix_update(void)
|
||||
{
|
||||
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
|
||||
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
|
||||
|
||||
if (gps_mode==3) //Remove centrifugal acceleration.
|
||||
{
|
||||
Accel_Vector[1] += speed_3d*Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
|
||||
Accel_Vector[2] -= speed_3d*Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
|
||||
}
|
||||
|
||||
|
||||
#if OUTPUTMODE==1 // With corrected data (drift correction)
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
|
||||
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
|
||||
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
|
||||
Update_Matrix[2][2]=0;
|
||||
#else // Uncorrected data (no drift correction)
|
||||
Update_Matrix[0][0]=0;
|
||||
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
|
||||
Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
|
||||
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
|
||||
Update_Matrix[1][1]=0;
|
||||
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
|
||||
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
|
||||
Update_Matrix[2][2]=0;
|
||||
#endif
|
||||
|
||||
Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
|
||||
|
||||
for(int x=0; x<3; x++) //Matrix Addition (update)
|
||||
{
|
||||
for(int y=0; y<3; y++)
|
||||
{
|
||||
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Euler_angles(void)
|
||||
{
|
||||
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
|
||||
euler.phi = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
|
||||
euler.theta = -asin((Accel_Vector[0])/GRAVITY); // asin(acc_x)
|
||||
euler.psi = 0;
|
||||
#else
|
||||
euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
|
||||
euler.theta = -asin(DCM_Matrix[2][0]);
|
||||
euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
|
||||
euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -1,48 +0,0 @@
|
||||
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
// Inputs for DCM
|
||||
extern float Gyro_Vector[3];
|
||||
extern float Accel_Vector[3];
|
||||
|
||||
// Integrate Inertial
|
||||
void Matrix_update(void);
|
||||
void Normalize(void);
|
||||
|
||||
// Input GPS/Pressure/Magnetometer data
|
||||
|
||||
// Correct
|
||||
void Drift_correction(void);
|
||||
|
||||
// Get outputs
|
||||
void Euler_angles(void);
|
||||
extern struct FloatEulers euler;
|
||||
|
||||
// DCM Parameters
|
||||
|
||||
//#define Kp_ROLLPITCH 0.2
|
||||
#define Kp_ROLLPITCH 0.015
|
||||
#define Ki_ROLLPITCH 0.000010
|
||||
#define Kp_YAW 1.2 //High yaw drift correction gain - use with caution!
|
||||
#define Ki_YAW 0.00005
|
||||
|
||||
#define GRAVITY 9.81
|
||||
|
||||
|
||||
#define OUTPUTMODE 1
|
||||
// Mode 0 = DCM integration without Ki gyro bias
|
||||
// Mode 1 = DCM integration with Kp and Ki
|
||||
// Mode 2 = direct accelerometer -> euler
|
||||
|
||||
#define MAGNETOMETER 1
|
||||
extern float MAG_Heading;
|
||||
|
||||
#define PERFORMANCE_REPORTING 1
|
||||
#if PERFORMANCE_REPORTING == 1
|
||||
extern int renorm_sqrt_count;
|
||||
extern int renorm_blowup_count;
|
||||
extern float imu_health;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@@ -23,8 +23,6 @@
|
||||
|
||||
#include "subsystems/imu.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
|
||||
struct Imu imu;
|
||||
|
||||
void imu_init(void) {
|
||||
@@ -32,6 +30,7 @@ void imu_init(void) {
|
||||
/* initialises neutrals */
|
||||
RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL);
|
||||
VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
|
||||
//FIXME should not assume that every imu has a mag and this id defined?
|
||||
VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL);
|
||||
|
||||
/*
|
||||
|
||||
@@ -26,6 +26,7 @@
|
||||
|
||||
#include "math/pprz_algebra_int.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
/* must be defined by underlying hardware */
|
||||
extern void imu_impl_init(void);
|
||||
|
||||
@@ -1,7 +1,5 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
* Copyright (C) 2010 The Paparazzi Team
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -22,13 +20,10 @@
|
||||
*/
|
||||
|
||||
#include "imu_analog.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "mcu_periph/adc.h"
|
||||
#include "subsystems/imu.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
|
||||
volatile bool_t analog_imu_available;
|
||||
uint16_t analog_imu_values[NB_ANALOG_IMU_ADC];
|
||||
|
||||
static struct adc_buf analog_imu_adc_buf[NB_ANALOG_IMU_ADC];
|
||||
|
||||
@@ -46,10 +41,12 @@ void imu_impl_init(void) {
|
||||
}
|
||||
|
||||
void imu_periodic(void) {
|
||||
uint8_t i;
|
||||
for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
|
||||
analog_imu_values[i] = analog_imu_adc_buf[i].sum / analog_imu_adc_buf[i].av_nb_sample;
|
||||
}
|
||||
|
||||
imu.gyro_unscaled.p = analog_imu_adc_buf[0].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
|
||||
imu.gyro_unscaled.q = analog_imu_adc_buf[1].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
|
||||
imu.gyro_unscaled.r = analog_imu_adc_buf[2].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
|
||||
imu.accel_unscaled.x = analog_imu_adc_buf[3].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
|
||||
imu.accel_unscaled.y = analog_imu_adc_buf[4].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
|
||||
imu.accel_unscaled.z = analog_imu_adc_buf[5].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
|
||||
|
||||
analog_imu_available = TRUE;
|
||||
}
|
||||
|
||||
@@ -1,7 +1,5 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
* Copyright (C) 2010 The Paparazzi Team
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -28,30 +26,23 @@
|
||||
|
||||
#define NB_ANALOG_IMU_ADC 6
|
||||
|
||||
extern uint16_t analog_imu_values[NB_ANALOG_IMU_ADC];
|
||||
extern volatile bool_t analog_imu_available;
|
||||
|
||||
#define ImuEvent(_gyro_accel_handler, _mag_handler) { \
|
||||
if (ADSanalog_imu_available) { \
|
||||
analog_imu_available = FALSE; \
|
||||
imu.gyro_unscaled.p = analog_imu_values[0]; \
|
||||
imu.gyro_unscaled.q = analog_imu_values[1]; \
|
||||
imu.gyro_unscaled.r = analog_imu_values[2]; \
|
||||
imu.accel_unscaled.x = analog_imu_values[3]; \
|
||||
imu.accel_unscaled.y = analog_imu_values[4]; \
|
||||
imu.accel_unscaled.z = analog_imu_values[5]; \
|
||||
_gyro_accel_handler(); \
|
||||
} \
|
||||
ImuMagEvent(_mag_handler); \
|
||||
#define ImuEvent(_gyro_accel_handler, _mag_handler) { \
|
||||
if (analog_imu_available) { \
|
||||
analog_imu_available = FALSE; \
|
||||
_gyro_accel_handler(); \
|
||||
} \
|
||||
ImuMagEvent(_mag_handler); \
|
||||
}
|
||||
|
||||
#define ImuMagEvent(_mag_handler) { \
|
||||
if (false) { \
|
||||
_mag_handler(); \
|
||||
} \
|
||||
#define ImuMagEvent(_mag_handler) { \
|
||||
if (0) { \
|
||||
_mag_handler(); \
|
||||
} \
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /* IMU_CRISTA_H */
|
||||
#endif /* IMU_ANALOG_H */
|
||||
|
||||
Reference in New Issue
Block a user