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

Separate Definition of Periodic/IMU freq, Propagation Freq and Correction Freq

Browse Source
This commit is contained in:
Christophe De Wagter
2010-12-22 11:05:07 +01:00
parent 69dfac36a6
commit 85220cee54
7 changed files with 75 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TU_Delft/MicrojetCDW.xml
+5 -3
View File
@@ -184,9 +184,11 @@
<firmware name="fixedwing">
<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" />
<configure name="PERIODIC_FREQUENCY" value="960" /> <!-- IMU FREQ -->
<configure name="AHRS_PROPAGATE_FREQUENCY" value="120" />
<configure name="AHRS_CORRECT_FREQUENCY" value="60" />
<configure name="AHRS_ALIGNER_LED" value="1" />
<configure name="CPU_LED" value="2" />
</target>
<target name="sim" board="pc" />
conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
+19 -6
View File
@@ -7,14 +7,27 @@ ifeq ($(ARCH), lpc21)
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.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
ifdef CPU_LED
ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
endif
ifdef AHRS_PROPAGATE_FREQUENCY
else
AHRS_PROPAGATE_FREQUENCY = 60
endif
ifdef AHRS_CORRECT_FREQUENCY
else
AHRS_CORRECT_FREQUENCY = 60
endif
ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
endif
conf/settings/tuning_ins.xml
+1
View File
@@ -32,6 +32,7 @@
<dl_settings NAME="control">
<dl_settings NAME="ins">
<dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun_error" shortname="imu_overrun" module="subsystems/imu" />
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
</dl_settings>
sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
+1
View File
@@ -23,6 +23,7 @@
#include "subsystems/imu.h"
int imu_overrun_error = 0;
volatile uint8_t imu_ssp_status;
static void SSP_ISR(void) __attribute__((naked));
#if 0
sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h
+2
View File
@@ -45,6 +45,8 @@
#define IMU_SSP_STA_BUSY_MAX1168 1
#define IMU_SSP_STA_BUSY_MS2100 2
extern volatile uint8_t imu_ssp_status;
extern int imu_overrun_error;
sw/airborne/firmwares/fixedwing/main_ap.c
+46 -29
View File
@@ -381,15 +381,7 @@ void periodic_task_ap( void ) {
#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
@@ -669,42 +661,67 @@ void event_task_ap( void ) {
#ifdef USE_AHRS
static inline void on_gyro_accel_event( void ) {
static uint8_t _reduced_rate = 0;
static uint8_t _reduced_propagation_rate = 0;
static uint8_t _reduced_correction_rate = 0;
static struct Int32Vect3 acc_avg, gyr_avg;
#ifdef AHRS_CPU_LED
LED_ON(AHRS_CPU_LED);
#endif
ImuScaleGyro(imu);
ImuScaleAccel(imu);
if (ahrs.status == AHRS_UNINIT) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
gyr_avg.x += imu.gyro_unscaled.p;
gyr_avg.y += imu.gyro_unscaled.q;
gyr_avg.z += imu.gyro_unscaled.r;
INT32_VECT3_ADD(acc_avg, imu.accel_unscaled);
_reduced_propagation_rate++;
if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY))
{
}
else {
else
{
_reduced_propagation_rate = 0;
INT32_VECT3_SDIV(gyr_avg, gyr_avg, (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY) );
imu.gyro_unscaled.p = gyr_avg.x;
imu.gyro_unscaled.q = gyr_avg.y;
imu.gyro_unscaled.r = gyr_avg.z;
INT_VECT3_ZERO(gyr_avg);
ImuScaleGyro(imu);
if (ahrs.status == AHRS_UNINIT) {
ImuScaleAccel(imu);
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
}
else {
//if (_reduced_rate % 4)
{
ahrs_propagate();
_reduced_correction_rate++;
if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY))
{
_reduced_correction_rate = 0;
INT32_VECT3_SDIV(acc_avg, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) );
INT32_VECT3_COPY(imu.accel_unscaled, acc_avg);
INT_VECT3_ZERO(acc_avg);
ImuScaleAccel(imu);
ahrs_update_accel();
ahrs_update_fw_estimator();
}
}
_reduced_rate++;
if (_reduced_rate >= (PERIODIC_FREQUENCY / 60))
{
_reduced_rate = 0;
ahrs_update_accel();
ahrs_update_fw_estimator();
}
}
#ifdef AHRS_CPU_LED
LED_OFF(AHRS_CPU_LED);
#endif
}
static inline void on_mag_event(void) {
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+1 -4
View File
@@ -50,10 +50,7 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
// Positive yaw : clockwise
// DCM Working variables
#ifndef PERIODIC_FREQUENCY
#define PERIODIC_FREQUENCY 60
#endif
float G_Dt=1. / ((float) PERIODIC_FREQUENCY );
float G_Dt=1. / ((float) AHRS_PROPAGATE_FREQUENCY );
struct FloatVect3 accel_float = {0,0,0};