Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Automatic USE_ADC

Browse Source
This commit is contained in:
Christophe De Wagter
2010-12-09 14:16:00 +01:00
parent c92c59196a
commit 5a5a66e99b
5 changed files with 33 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/example_twog_analogimu.xml
+8 -7
View File
@@ -74,12 +74,6 @@
<define name="GYRO_NB_SAMPLES" value="16" />
<define name="ACCEL_NB_SAMPLES" value="32" />
<define name="GYRO_P" value="ADC_0"/>
<define name="GYRO_Q" value="ADC_1"/>
<define name="GYRO_R" value="ADC_2"/>
<define name="ACCEL_X" value="ADC_5"/>
<define name="ACCEL_Y" value="ADC_6"/>
<define name="ACCEL_Z" value="ADC_7"/>
</section>
<!-- settings for the Analog IMU -->
@@ -199,7 +193,14 @@
<!-- Actuators are automatically chosen according to the board-->
<subsystem name="control"/>
<!-- Sensors -->
<subsystem name="attitude" type="analogimu"/>
<subsystem name="attitude" type="analogimu">
<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>
<subsystem name="gps" type="ublox_lea5h"/>
<subsystem name="navigation"/>
</firmware>
conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
+9 -5
View File
@@ -2,28 +2,32 @@
ifeq ($(ARCH), lpc21)
ap.CFLAGS += -DANALOG_IMU -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -DUSE_ADC_4 -DUSE_ADC_5 -DUSE_ADC_6 -DUSE_ADC_7
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)/imu/imu_analog.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
# 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
sw/airborne/subsystems/ahrs/dcm/analogimu.c
+3 -3
View File
@@ -154,11 +154,11 @@ void estimator_update_state_analog_imu( void ) {
Euler_angles();
// return euler angles to phi and theta
estimator_phi = euler[EULER_ROLL]-imu_roll_neutral;
estimator_phi = euler.phi-imu_roll_neutral;
//estimator_phi = angle[ANG_ROLL];
estimator_theta = euler[EULER_PITCH]-imu_pitch_neutral;
estimator_theta = euler.theta-imu_pitch_neutral;
//estimator_theta = angle[ANG_PITCH];
estimator_psi = euler[EULER_YAW];
estimator_psi = euler.psi;
}
#endif
sw/airborne/subsystems/ahrs/dcm/dcm.c
+10 -11
View File
@@ -24,7 +24,9 @@ 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}};
float euler[EULER_LAST] = {0.};
struct FloatEulers euler= {0, 0, 0};
float speed_3d = 0;
/**************************************************/
@@ -235,9 +237,6 @@ float MAG_Heading;
#endif
// Stored for Accelerometer Tab
float speed_3d = 0;
void Drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
@@ -378,17 +377,17 @@ void Matrix_update(void)
void Euler_angles(void)
{
#if (OUTPUTMODE==2) // Only accelerometer info (debugging purposes)
euler[EULER_ROLL] = atan2(Accel_Vector[1],Accel_Vector[2]); // atan2(acc_y,acc_z)
euler[EULER_PITCH] = -asin((Accel_Vector[0])/GRAVITY); // asin(acc_x)
euler[EULER_YAW] = 0;
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
//pitch = -asin(DCM_Matrix[2][0]);
//roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
//yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
euler[EULER_PITCH] = -asin(DCM_Matrix[2][0]);
euler[EULER_ROLL] = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
euler[EULER_YAW] = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
euler[EULER_YAW] += M_PI; // Rotating the angle 180deg to fit for PPRZ
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
}
sw/airborne/subsystems/ahrs/dcm/dcm.h
+3 -2
View File
@@ -1,4 +1,6 @@
#include "math/pprz_algebra_float.h"
// Inputs for DCM
extern float Gyro_Vector[3];
extern float Accel_Vector[3];
@@ -14,8 +16,7 @@ void Drift_correction(void);
// Get outputs
void Euler_angles(void);
enum euler_idx_t { EULER_ROLL, EULER_PITCH, EULER_YAW, EULER_LAST };
extern float euler[3];
extern struct FloatEulers euler;
// DCM Parameters