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

debugging variables and gyro-cross components

Browse Source
This commit is contained in:
Christophe De Wagter
2010-12-23 14:41:02 +01:00
parent 264679e18c
commit 968c7d36be
7 changed files with 51 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/TU_Delft/MicrojetCDW.xml
+20 -13
View File
@@ -4,13 +4,13 @@
<!-- 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"/>
<servo name="THROTTLE" no="0" min="1050" neutral="1050" max="1900"/>
<servo name="AILEVON_RIGHT" no="1" min="1850" neutral="1480" max="1100"/> <!-- 400 - 380 -->
<servo name="AILEVON_LEFT" no="2" min="1250" neutral="1580" max="1980"/> <!-- 300 - 400 -->
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="THROTTLE" failsafe_value="9600"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
</commands>
@@ -22,8 +22,8 @@
</rc_commands>
<section name="MIXER">
<define name="AILEVON_AILERON_RATE" value="0.4"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.7"/>
<define name="AILEVON_AILERON_RATE" value="0.75"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.9"/>
</section>
<command_laws>
@@ -61,6 +61,13 @@
<define name="GYRO_Q_SENS" value="2.234" integer="16"/>
<define name="GYRO_R_SENS" value="2.234" integer="16"/>
<define name="GYRO_P_Q" value="0." />
<define name="GYRO_P_R" value="(1.0f/75.0f)" />
<define name="GYRO_Q_P" value="0." />
<define name="GYRO_Q_R" value="0." />
<define name="GYRO_R_P" value="0." />
<define name="GYRO_R_Q" value="0." />
<define name="GYRO_P_SIGN" value="1"/>
<define name="GYRO_Q_SIGN" value="-1"/>
<define name="GYRO_R_SIGN" value="-1"/>
@@ -145,18 +152,18 @@
<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_MAX_SETPOINT" value="0.4" unit="radians"/>
<define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
<define name="PITCH_PGAIN" value="-10000."/>
<define name="PITCH_DGAIN" value="0."/>
<define name="PITCH_PGAIN" value="-6000."/>
<define name="PITCH_DGAIN" value="5."/>
<define name="ELEVATOR_OF_ROLL" value="0."/>
<define name="ELEVATOR_OF_ROLL" value="1200."/>
<define name="ROLL_SLEW" value="0."/>
<define name="ROLL_ATTITUDE_GAIN" value="-10000."/>
<define name="ROLL_RATE_GAIN" value="0."/>
<define name="ROLL_ATTITUDE_GAIN" value="-5000."/>
<define name="ROLL_RATE_GAIN" value="500."/>
</section>
<section name="AGGRESSIVE" prefix="AGR_">
conf/settings/tuning_ins.xml
+1 -1
View File
@@ -34,7 +34,7 @@
<dl_settings NAME="ins">
<dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_health" shortname="health" module="subsystems/ahrs" />
<dl_setting MAX="100" MIN="0" STEP="1" VAR="renorm_sqrt_count" shortname="err_norm" module="subsystems/ahrs" />
<dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun_error" shortname="err_adc" module="subsystems/imu" />
<dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun" shortname="ADC 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
+4 -1
View File
@@ -23,7 +23,7 @@
#include "subsystems/imu.h"
int imu_overrun_error = 0;
int imu_overrun = 0;
volatile uint8_t imu_ssp_status;
static void SSP_ISR(void) __attribute__((naked));
#if 0
@@ -86,7 +86,10 @@ void imu_b2_arch_init(void) {
void imu_periodic(void) {
// check ssp idle
if (imu_ssp_status != IMU_SSP_STA_IDLE)
{
imu_overrun++;
return; //, DEBUG_IMU, IMU_ERR_OVERUN);
}
// setup 16 bits
ImuSetSSP16bits();
sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h
+1 -1
View File
@@ -45,7 +45,7 @@
#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;
extern int imu_overrun;
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+13 -2
View File
@@ -129,7 +129,6 @@ void ahrs_update_fw_estimator( void )
));
// estimator_p =
}
@@ -188,8 +187,20 @@ 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);
/* Uncouple Motions */
#ifdef GYRO_P_Q
float dp=0,dq=0,dr=0;
dp += ahrs_float.imu_rate.q * GYRO_P_Q;
dp += ahrs_float.imu_rate.r * GYRO_P_R;
dq += ahrs_float.imu_rate.p * GYRO_Q_P;
dq += ahrs_float.imu_rate.r * GYRO_Q_R;
dr += ahrs_float.imu_rate.p * GYRO_R_P;
dr += ahrs_float.imu_rate.q * GYRO_R_Q;
ahrs_float.imu_rate.p += dp;
ahrs_float.imu_rate.q += dq;
ahrs_float.imu_rate.r += dr;
#endif
Matrix_update();
// INFO, ahrs struct only updated in ahrs_update_fw_estimator
sw/airborne/subsystems/imu/imu_analog.c
+11
View File
@@ -24,12 +24,14 @@
#include "mcu_periph/uart.h"
volatile bool_t analog_imu_available;
int imu_overrun;
static struct adc_buf analog_imu_adc_buf[NB_ANALOG_IMU_ADC];
void imu_impl_init(void) {
analog_imu_available = FALSE;
imu_overrun = 0;
adc_buf_channel(ADC_CHANNEL_GYRO_P, &analog_imu_adc_buf[0], ADC_CHANNEL_GYRO_NB_SAMPLES);
adc_buf_channel(ADC_CHANNEL_GYRO_Q, &analog_imu_adc_buf[1], ADC_CHANNEL_GYRO_NB_SAMPLES);
@@ -41,6 +43,15 @@ void imu_impl_init(void) {
}
void imu_periodic(void) {
// Actual Nr of ADC measurements per channel per periodic loop
static int last_head = 0;
imu_overrun = analog_imu_adc_buf[0].head - last_head;
if (imu_overrun < 0)
imu_overrun += ADC_CHANNEL_GYRO_NB_SAMPLES;
last_head = analog_imu_adc_buf[0].head;
// Read All Measurements
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;
sw/airborne/subsystems/imu/imu_analog.h
+1
View File
@@ -27,6 +27,7 @@
#define NB_ANALOG_IMU_ADC 6
extern volatile bool_t analog_imu_available;
extern int imu_overrun;
#define ImuEvent(_gyro_accel_handler, _mag_handler) { \
if (analog_imu_available) { \