Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-20 02:53:15 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

High Rate Fixedwing Filter Runs on Microjet CDW

Browse Source
This commit is contained in:
Christophe De Wagter
2010-12-21 17:22:24 +01:00
parent 3aaca5025a
commit eef42a332a
10 changed files with 111 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TU_Delft/MicrojetCDW.xml
+30 -26
View File
@@ -52,37 +52,39 @@
<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_NEUTRAL" value="21480"/>
<define name="GYRO_Q_NEUTRAL" value="21410"/>
<define name="GYRO_R_NEUTRAL" value="21940"/>
<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"/>
<!-- SENS = 2mV/deg/sec * 57.6 deg/rad = 114.59 mV/rad/sec * 16 LSB/mV = 1833.465 LSB/rad/sec / 12bit FRAC -->
<define name="GYRO_P_SENS" value="2.234" integer="16"/>
<define name="GYRO_Q_SENS" value="2.234" integer="16"/>
<define name="GYRO_R_SENS" value="2.234" 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="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_NEUTRAL" value="26424"/>
<define name="ACCEL_Y_NEUTRAL" value="26640"/>
<define name="ACCEL_Z_NEUTRAL" value="26732"/>
<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"/>
<!-- SENS = 660mV/g / 9.81 ms2/g = 67.27 mV/ms2 * 16 LSB/mV = 1076 LSB/ms2 / 10bit FRAC -->
<define name="ACCEL_X_SENS" value="0.9372" integer="16"/>
<define name="ACCEL_Y_SENS" value="0.9346" integer="16"/>
<define name="ACCEL_Z_SENS" value="0.9178" integer="16"/>
<define name="ACCEL_X_SIGN" value="1"/>
<define name="ACCEL_X_SIGN" value="-1"/>
<define name="ACCEL_Y_SIGN" value="1"/>
<define name="ACCEL_Z_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_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="0"/>
<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_X_SENS" value="1" integer="16"/>
<define name="MAG_Y_SENS" value="1" integer="16"/>
<define name="MAG_Z_SENS" value="1" integer="16"/>
<!-- <define name="MAG_45_HACK" value="1"/> -->
@@ -146,14 +148,14 @@
<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_PGAIN" value="-10000."/>
<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_ATTITUDE_GAIN" value="-10000."/>
<define name="ROLL_RATE_GAIN" value="0."/>
</section>
@@ -181,8 +183,10 @@
</section>
<firmware name="fixedwing">
<target name="ap" board="tiny_1.1" >
<param name="PERIODIC_FREQUENCY" value="480" />
<target name="ap" board="tiny_1.1">
<param name="PERIODIC_FREQUENCY" value="960" />
<param name="AHRS_ALIGNER_LED" value="1" />
<param name="CPU_LED" value="2" />
</target>
<target name="sim" board="pc" />
conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
+6 -1
View File
@@ -5,11 +5,16 @@ $(TARGET).CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DUSE_IMU
ap.CFLAGS += -DUSE_AHRS
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
ifdef CPU_LED
ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
endif
endif
conf/autopilot/subsystems/fixedwing/imu_analog.makefile
+1 -1
View File
@@ -44,7 +44,7 @@
ifeq ($(ARCH), lpc21)
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_analog.h\"
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_analog.h\" -DUSE_IMU
imu_CFLAGS += -DADC
imu_CFLAGS += -DUSE_$(GYRO_P) -DUSE_$(GYRO_Q) -DUSE_$(GYRO_R)
conf/autopilot/subsystems/fixedwing/imu_booz.makefile
+1 -1
View File
@@ -41,7 +41,7 @@
# imu Booz2 v1.0, v1.1, v1.2, YAI v1.0
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\"
imu_CFLAGS += -DIMU_TYPE_H=\"subsystems/imu/imu_b2.h\" -DUSE_IMU
imu_srcs += $(SRC_SUBSYSTEMS)/imu.c
imu_srcs += $(SRC_SUBSYSTEMS)/imu/imu_b2.c
imu_srcs += $(SRC_ARCH)/subsystems/imu/imu_b2_arch.c
conf/settings/tuning_ins.xml
+1 -1
View File
@@ -41,7 +41,7 @@
<dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>
<dl_setting MAX="1" MIN="0" STEP="0.05" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT"/>
<dl_setting MAX="000" MIN="-25000" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain" param="H_CTL_PITCH_PGAIN"/>
<dl_setting MAX="0" MIN="-50000" STEP="10" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN"/>
<dl_setting MAX="1000" MIN="-1000" STEP="1" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN"/>
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_elevator_of_roll" shortname="elevator_of_roll" param="H_CTL_ELEVATOR_OF_ROLL"/>
<dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle"/>
conf/telemetry/default_fixedwing_imu.xml
+3 -3
View File
@@ -7,7 +7,7 @@
<message name="ALIVE" period="5"/>
<message name="GPS" period="0.25"/>
<message name="NAVIGATION" period="1."/>
<message name="ATTITUDE" period="0.5"/>
<message name="ATTITUDE" period="0.1"/>
<message name="ESTIMATOR" period="0.5"/>
<message name="ENERGY" period="2.5"/>
<message name="WP_MOVED" period="0.5"/>
@@ -26,8 +26,8 @@
<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"/>
<message name="IMU_ACCEL" period=".8"/>
<message name="IMU_GYRO" period=".6"/>
</mode>
<mode name="minimal">
<message name="ALIVE" period="5"/>
sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
+2 -1
View File
@@ -84,7 +84,8 @@ void imu_b2_arch_init(void) {
void imu_periodic(void) {
// check ssp idle
// ASSERT((imu_status == IMU_STA_IDLE), DEBUG_IMU, IMU_ERR_OVERUN);
if (imu_ssp_status != IMU_SSP_STA_IDLE)
return; //, DEBUG_IMU, IMU_ERR_OVERUN);
// setup 16 bits
ImuSetSSP16bits();
sw/airborne/firmwares/fixedwing/main_ap.c
+46 -18
View File
@@ -65,8 +65,10 @@
#endif
#ifdef USE_ANALOG_IMU
#ifdef USE_IMU
#include "subsystems/imu.h"
#endif
#ifdef USE_AHRS
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ahrs/ahrs_float_dcm.h"
@@ -377,6 +379,21 @@ void periodic_task_ap( void ) {
static uint8_t _4Hz = 0;
static uint8_t _1Hz = 0;
#ifdef USE_IMU
// Run at PERIODIC_FREQUENCY (60Hz if not defined)
#ifdef AHRS_CPU_LED
LED_ON(AHRS_CPU_LED);
#endif
imu_periodic();
#ifdef AHRS_CPU_LED
LED_OFF(AHRS_CPU_LED);
#endif
#endif // USE_IMU
#ifdef PERIODIC_FREQUENCY
#warning Using HighSpeed Periodic: Manually check to make sure PERIODIC_FREQUENCY is a multiple of 60.
static uint8_t _60Hz = 0;
@@ -392,13 +409,8 @@ void periodic_task_ap( void ) {
#endif
#ifdef USE_IMU
// if (!_20Hz) {
imu_periodic();
// }
#endif // USE_IMU
// Rest of the periodic function still runs at 60Hz like always
_20Hz++;
if (_20Hz>=3) _20Hz=0;
_10Hz++;
@@ -518,8 +530,10 @@ void init_ap( void ) {
GpioInit();
#endif
#ifdef USE_ANALOG_IMU
#ifdef USE_IMU
imu_init();
#endif
#ifdef USE_AHRS
ahrs_aligner_init();
ahrs_init();
#endif
@@ -578,9 +592,9 @@ void init_ap( void ) {
/*********** EVENT ***********************************************************/
void event_task_ap( void ) {
#ifdef USE_ANALOG_IMU
#ifdef USE_AHRS
ImuEvent(on_gyro_accel_event, on_mag_event);
#endif // USE_ANALOG_IMU
#endif // USE_AHRS
#ifdef USE_GPS
#if !(defined HITL) && !(defined UBX_EXTERNAL) /** else comes through the datalink */
@@ -653,10 +667,14 @@ void event_task_ap( void ) {
modules_event_task();
} /* event_task_ap() */
#ifdef USE_ANALOG_IMU
#ifdef USE_AHRS
static inline void on_gyro_accel_event( void ) {
static uint8_t _60Hz = 0;
LED_ON(2);
static uint8_t _reduced_rate = 0;
#ifdef AHRS_CPU_LED
LED_ON(AHRS_CPU_LED);
#endif
ImuScaleGyro(imu);
ImuScaleAccel(imu);
@@ -667,16 +685,26 @@ static inline void on_gyro_accel_event( void ) {
ahrs_align();
}
else {
ahrs_propagate();
if (_60Hz >= 8)
//if (_reduced_rate % 4)
{
_60Hz = 0;
ahrs_propagate();
}
_reduced_rate++;
if (_reduced_rate >= (PERIODIC_FREQUENCY / 60))
{
_reduced_rate = 0;
ahrs_update_accel();
ahrs_update_fw_estimator();
}
}
LED_OFF(2);
#ifdef AHRS_CPU_LED
LED_OFF(AHRS_CPU_LED);
#endif
}
static inline void on_mag_event(void) {
@@ -688,4 +716,4 @@ static inline void on_mag_event(void) {
}
*/
}
#endif // USE_ANALOG_IMU
#endif // USE_AHRS
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+20 -18
View File
@@ -73,14 +73,23 @@ float MAG_Heading;
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();
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
ahrs_float.ltp_to_imu_euler.phi = atan2(accel_float.y,accel_float.z); // atan2(acc_y,acc_z)
ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
ahrs_float.ltp_to_imu_euler.psi = 0;
#else
ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
//warning, only eulers written to ahrs struct so far
//compute_body_orientation_and_rates();
@@ -89,6 +98,8 @@ void ahrs_update_fw_estimator( void )
estimator_phi = ahrs_float.ltp_to_imu_euler.phi - ins_roll_neutral;
estimator_theta = ahrs_float.ltp_to_imu_euler.theta - ins_pitch_neutral;
estimator_psi = ahrs_float.ltp_to_imu_euler.psi;
// estimator_p =
}
@@ -147,13 +158,18 @@ void ahrs_propagate(void)
/* unbias rate measurement */
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
ahrs_float.imu_rate.p += (ahrs_float.imu_rate.r / 75);
Matrix_update();
Normalize();
//INFO, ahrs struct only updated in ahrs_update_fw_estimator
// INFO, ahrs struct only updated in ahrs_update_fw_estimator
// Normalize();
}
void ahrs_update_accel(void)
{
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
#ifdef USE_GPS
@@ -379,20 +395,6 @@ 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_float.y,accel_float.z); // atan2(acc_y,acc_z)
ahrs_float.ltp_to_imu_euler.theta = -asin((accel_float.x)/GRAVITY); // asin(acc_x)
ahrs_float.ltp_to_imu_euler.psi = 0;
#else
ahrs_float.ltp_to_imu_euler.phi = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
ahrs_float.ltp_to_imu_euler.theta = -asin(DCM_Matrix[2][0]);
ahrs_float.ltp_to_imu_euler.psi = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
ahrs_float.ltp_to_imu_euler.psi += M_PI; // Rotating the angle 180deg to fit for PPRZ
#endif
}
/*
* Compute body orientation and rates from imu orientation and rates
*/
sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+1 -1
View File
@@ -47,7 +47,7 @@ void ahrs_update_fw_estimator(void);
//#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 Kp_YAW 0.9 //High yaw drift correction gain - use with caution!
#define Ki_YAW 0.00005
#define GRAVITY 9.81