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

Merge pull request #510 from fvantienen/master

Browse Source 
[ardrone2] Magneto axis correction and calibration
This commit is contained in:
Christophe De Wagter
2013-08-27 08:29:51 -07:00
parent 42edf3a1ae bb8707ad9b
commit 1330f21064
4 changed files with 133 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ardrone2_raw.xml
+8 -33
View File
@@ -22,18 +22,16 @@
<subsystem name="actuators" type="ardrone2" />
<subsystem name="imu" type="ardrone2" />
<subsystem name="gps" type="sirf" />
<subsystem name="gps" type="ublox" />
<subsystem name="stabilization" type="int_quat" />
<subsystem name="ahrs" type="int_cmpl_quat" />
<subsystem name="ins" />
</firmware>
<!--
<modules main_freq="512" >
<load name="gps_ubx_ucenter.xml" />
</modules>
-->
<commands>
<axis name="PITCH" failsafe_value="0" />
@@ -71,38 +69,14 @@
</command_laws>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_X_NEUTRAL" value="2072" />
<define name="ACCEL_Y_NEUTRAL" value="2040" />
<define name="ACCEL_Z_NEUTRAL" value="2047" />
<define name="ACCEL_X_SENS" value="19.1079036954" integer="16" />
<define name="ACCEL_Y_SENS" value="19.5393623518" integer="16" />
<define name="ACCEL_Z_SENS" value="19.6539180847" integer="16" />
<define name="ACCEL_X_NEUTRAL" value="2056" />
<define name="ACCEL_Y_NEUTRAL" value="2060" />
<define name="ACCEL_Z_NEUTRAL" value="2052" />
<define name="ACCEL_X_SIGN" value="1" />
<define name="ACCEL_Y_SIGN" value="-1" />
<define name="ACCEL_Z_SIGN" value="-1" />
<define name="MAG_X_NEUTRAL" value="-9"/>
<define name="MAG_Y_NEUTRAL" value="1"/>
<define name="MAG_Z_NEUTRAL" value="97"/>
<define name="GYRO_P_SENS_NUM" value="4359" />
<define name="GYRO_P_SENS_DEN" value="1000" />
<define name="GYRO_Q_SENS_NUM" value="4359" />
<define name="GYRO_Q_SENS_DEN" value="1000" />
<define name="GYRO_R_SENS_NUM" value="4359" />
<define name="GYRO_R_SENS_DEN" value="1000" />
<define name="GYRO_P_SIGN" value="1" />
<define name="GYRO_Q_SIGN" value="-1" />
<define name="GYRO_R_SIGN" value="-1" />
<define name="MAG_X_NEUTRAL" value="118" />
<define name="MAG_Y_NEUTRAL" value="-65" />
<define name="MAG_Z_NEUTRAL" value="110" />
<define name="MAG_X_SENS" value="14.6416865144" integer="16" />
<define name="MAG_Y_SENS" value="14.5167340835" integer="16" />
<define name="MAG_Z_SENS" value="15.1670335124" integer="16" />
<define name="MAG_X_SIGN" value="-1" />
<define name="MAG_Y_SIGN" value="1" />
<define name="MAG_Z_SIGN" value="-1" />
<!-- roll -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg" />
@@ -232,6 +206,7 @@
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="7500"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V" />
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V" />
<define name="LOW_BAT_LEVEL" value="9.7" unit="V" />
sw/airborne/boards/ardrone/electrical_raw.c
+44
View File
@@ -36,9 +36,35 @@
#include <unistd.h>
#include <math.h>
#include "i2c-dev.h"
#include "subsystems/commands.h"
#include "generated/airframe.h"
struct Electrical electrical;
#if defined ADC_CHANNEL_VSUPPLY || defined ADC_CHANNEL_CURRENT || defined MILLIAMP_AT_FULL_THROTTLE
static struct {
#ifdef ADC_CHANNEL_VSUPPLY
struct adc_buf vsupply_adc_buf;
#endif
#ifdef ADC_CHANNEL_CURRENT
struct adc_buf current_adc_buf;
#endif
#ifdef MILLIAMP_AT_FULL_THROTTLE
float nonlin_factor;
#endif
} electrical_priv;
#endif
#if defined COMMAND_THROTTLE
#define COMMAND_CURRENT_ESTIMATION COMMAND_THROTTLE
#elif defined COMMAND_THRUST
#define COMMAND_CURRENT_ESTIMATION COMMAND_THRUST
#endif
#ifndef CURRENT_ESTIMATION_NONLINEARITY
#define CURRENT_ESTIMATION_NONLINEARITY 1.2
#endif
int fd;
void electrical_init(void) {
@@ -51,6 +77,7 @@ void electrical_init(void) {
}
electrical_setup();
electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY;
}
void electrical_setup(void) {
@@ -94,4 +121,21 @@ void electrical_periodic(void) {
//9.0V=662, 9.5V=698, 10.0V=737,10.5V=774, 11.0V=811, 11.5V=848, 12.0V=886, 12.5V=923
//leading to our 0.13595166 magic number for decivolts conversion
electrical.vsupply = raw_voltage*0.13595166;
/*
* Superellipse: abs(x/a)^n + abs(y/b)^n = 1
* with a = 1
* b = mA at full throttle
* n = 1.2 This defines nonlinearity (1 = linear)
* x = throttle
* y = current
*
* define CURRENT_ESTIMATION_NONLINEARITY in your airframe file to change the default nonlinearity factor of 1.2
*/
float b = (float)MILLIAMP_AT_FULL_THROTTLE;
float x = ((float)commands[COMMAND_CURRENT_ESTIMATION]) / ((float)MAX_PPRZ);
/* electrical.current y = ( b^n - (b* x/a)^n )^1/n
* a=1, n = electrical_priv.nonlin_factor
*/
electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor));
}
sw/airborne/boards/ardrone/navdata.h
+1 -1
View File
@@ -81,8 +81,8 @@ typedef struct
int32_t pressure;
uint16_t temperature_pressure;
int16_t mx;
int16_t my;
int16_t mx;
int16_t mz;
uint16_t chksum;
sw/airborne/subsystems/imu/imu_ardrone2_raw.h
+80 -3
View File
@@ -31,6 +31,83 @@
#include "generated/airframe.h"
#include "navdata.h"
#if !defined IMU_MAG_X_SIGN & !defined IMU_MAG_Y_SIGN & !defined IMU_MAG_Z_SIGN
#define IMU_MAG_X_SIGN 1
#define IMU_MAG_Y_SIGN 1
#define IMU_MAG_Z_SIGN 1
#endif
#if !defined IMU_GYRO_P_SIGN & !defined IMU_GYRO_Q_SIGN & !defined IMU_GYRO_R_SIGN
#define IMU_GYRO_P_SIGN 1
#define IMU_GYRO_Q_SIGN 1
#define IMU_GYRO_R_SIGN 1
#endif
#if !defined IMU_ACCEL_X_SIGN & !defined IMU_ACCEL_Y_SIGN & !defined IMU_ACCEL_Z_SIGN
#define IMU_ACCEL_X_SIGN 1
#define IMU_ACCEL_Y_SIGN 1
#define IMU_ACCEL_Z_SIGN 1
#endif
/** default gyro sensitivy and neutral from the datasheet
* MPU with 2000 deg/s
*/
#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
#define IMU_GYRO_P_SENS 4.359
#define IMU_GYRO_P_SENS_NUM 4359
#define IMU_GYRO_P_SENS_DEN 1000
#define IMU_GYRO_Q_SENS 4.359
#define IMU_GYRO_Q_SENS_NUM 4359
#define IMU_GYRO_Q_SENS_DEN 1000
#define IMU_GYRO_R_SENS 4.359
#define IMU_GYRO_R_SENS_NUM 4359
#define IMU_GYRO_R_SENS_DEN 1000
#endif
#if !defined IMU_GYRO_P_NEUTRAL & !defined IMU_GYRO_Q_NEUTRAL & !defined IMU_GYRO_R_NEUTRAL
#define IMU_GYRO_P_NEUTRAL 0
#define IMU_GYRO_Q_NEUTRAL 0
#define IMU_GYRO_R_NEUTRAL 0
#endif
/** default accel sensitivy from the datasheet
* 512 LSB/g
*/
#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
#define IMU_ACCEL_X_SENS 19.5
#define IMU_ACCEL_X_SENS_NUM 195
#define IMU_ACCEL_X_SENS_DEN 10
#define IMU_ACCEL_Y_SENS 19.5
#define IMU_ACCEL_Y_SENS_NUM 195
#define IMU_ACCEL_Y_SENS_DEN 10
#define IMU_ACCEL_Z_SENS 19.5
#define IMU_ACCEL_Z_SENS_NUM 195
#define IMU_ACCEL_Z_SENS_DEN 10
#endif
#if !defined IMU_ACCEL_X_NEUTRAL & !defined IMU_ACCEL_Y_NEUTRAL & !defined IMU_ACCEL_Z_NEUTRAL
#define IMU_ACCEL_X_NEUTRAL 2048
#define IMU_ACCEL_Y_NEUTRAL 2048
#define IMU_ACCEL_Z_NEUTRAL 2048
#endif
#if !defined IMU_MAG_X_SENS & !defined IMU_MAG_Y_SENS & !defined IMU_MAG_Z_SENS
#define IMU_MAG_X_SENS 16.0
#define IMU_MAG_X_SENS_NUM 16
#define IMU_MAG_X_SENS_DEN 1
#define IMU_MAG_Y_SENS 16.0
#define IMU_MAG_Y_SENS_NUM 16
#define IMU_MAG_Y_SENS_DEN 1
#define IMU_MAG_Z_SENS 16.0
#define IMU_MAG_Z_SENS_NUM 16
#define IMU_MAG_Z_SENS_DEN 1
#endif
#if !defined IMU_MAG_X_NEUTRAL & !defined IMU_MAG_Y_NEUTRAL & !defined IMU_MAG_Z_NEUTRAL
#define IMU_MAG_X_NEUTRAL 0
#define IMU_MAG_Y_NEUTRAL 0
#define IMU_MAG_Z_NEUTRAL 0
#endif
void navdata_event(void);
static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
@@ -39,9 +116,9 @@ static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _a
//checks if the navboard has a new dataset ready
if (navdata_imu_available == TRUE) {
navdata_imu_available = FALSE;
RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, navdata->vy, navdata->vz);
VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, navdata->ay, navdata->az);
VECT3_ASSIGN(imu.mag_unscaled, navdata->mx, navdata->my, navdata->mz);
RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, -navdata->vy, -navdata->vz);
VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, 4096-navdata->ay, 4096-navdata->az);
VECT3_ASSIGN(imu.mag_unscaled, -navdata->mx, -navdata->my, -navdata->mz);
_gyro_handler();
_accel_handler();