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 25 Packages Projects Releases Wiki Activity

continue analogimu integration

Browse Source
This commit is contained in:
Felix Ruess
2010-12-09 20:14:33 +01:00
parent 3596726e86
commit 3c00ca3d71
4 changed files with 86 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/example_twog_analogimu.xml
+37 -30
View File
@@ -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_">
@@ -157,7 +174,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">
@@ -177,46 +194,36 @@
</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="gps" type="ublox_lea5h"/>
<subsystem name="attitude" type="analogimu"/>
<subsystem name="gps" type="ublox_lea5h"/>
<subsystem name="navigation"/>
</firmware>
<!-- Carefull: add the location after!! -->
<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>
conf/autopilot/subsystems/fixedwing/attitude_analogimu.makefile
+1 -1
View File
@@ -7,7 +7,7 @@ ap.CFLAGS += -DUSE_ANALOG_IMU -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_AD
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
ap.srcs += $(SRC_SUBSYSTEMS)/imu.c
endif
sw/airborne/firmwares/fixedwing/main_ap.c
+22 -4
View File
@@ -66,7 +66,10 @@
#ifdef USE_ANALOG_IMU
#include "subsystems/imu/imu_analog.h"
#include "subsystems/ahrs/dcm/analogimu.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
@@ -437,9 +440,9 @@ void periodic_task_ap( void ) {
#ifdef USE_ANALOG_IMU
if (!_20Hz) {
estimator_update_state_analog_imu();
analog_imu_downlink();
}
imu_periodic();
//analog_imu_downlink();
}
#endif // USE_ANALOG_IMU
#if CONTROL_RATE == 20
@@ -495,7 +498,6 @@ void init_ap( void ) {
GpioInit();
#endif
#ifdef USE_ANALOG_IMU
analog_imu_init();
#endif
@@ -560,6 +562,10 @@ void init_ap( void ) {
/*********** 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 +636,15 @@ 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);
estimator_update_state_analog_imu();
}
static inline void on_mag_event(void) {
//ImuScaleMag(imu);
}
#endif // USE_ANALOG_IMU
sw/airborne/subsystems/ahrs/dcm/analogimu.c
+26 -54
View File
@@ -47,6 +47,7 @@
//#include "downlink.h"
//#include "ap_downlink.h"
#include "sys_time.h"
#include "math/pprz_algebra_int.h"
#endif
@@ -61,62 +62,24 @@ 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
// read IMU, really?
imu_periodic();
for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
analog_imu_offset[i] = analog_imu_values[i];
}
imu.gyro_neutral.p = analog_imu_values[0];
imu.gyro_neutral.q = analog_imu_values[1];
imu.gyro_neutral.r = analog_imu_values[2];
imu.accel_neutral.x = analog_imu_values[3];
imu.accel_neutral.y = analog_imu_values[4];
imu.accel_neutral.z = analog_imu_values[5];
// 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();
// FIXME uugh...
//imu.accel_neutral.z += (9.81f / IMU_ACCEL_Z_SENS);
}
// functions
@@ -134,16 +97,25 @@ void analog_imu_downlink( void ) {
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[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];
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];
//FIXME run aligner to set gyro neutrals on ground
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro);
Gyro_Vector[0]= gyro_float.p;
Gyro_Vector[1]= gyro_float.q;
Gyro_Vector[2]= gyro_float.r;
struct FloatVect3 accel_float;
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
Accel_Vector[0] = accel_float.x;
Accel_Vector[1] = accel_float.y;
Accel_Vector[2] = accel_float.z;
Matrix_update();