Skip to content

Commit

Permalink
Merge pull request paparazzi#510 from fvantienen/master
Browse files Browse the repository at this point in the history 
[ardrone2] Magneto axis correction and calibration
  • Loading branch information
@dewagter
dewagter committed Aug 27, 2013
2 parents 42edf3a + bb8707a commit 1330f21
Show file tree
Hide file tree
Showing 4 changed files with 135 additions and 39 deletions.
45 changes: 10 additions & 35 deletions conf/airframes/ardrone2_raw.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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" />
Expand Down Expand Up @@ -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_SIGN" value="1" />
<define name="ACCEL_Y_SIGN" value="-1" />
<define name="ACCEL_Z_SIGN" value="-1" />

<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" />
<define name="ACCEL_X_NEUTRAL" value="2056" />
<define name="ACCEL_Y_NEUTRAL" value="2060" />
<define name="ACCEL_Z_NEUTRAL" value="2052" />

<define name="MAG_X_NEUTRAL" value="-9"/>
<define name="MAG_Y_NEUTRAL" value="1"/>
<define name="MAG_Z_NEUTRAL" value="97"/>


<!-- roll -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg" />
Expand Down Expand Up @@ -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" />
Expand Down
44 changes: 44 additions & 0 deletions sw/airborne/boards/ardrone/electrical_raw.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -51,6 +77,7 @@ void electrical_init(void) {
}

electrical_setup();
electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY;
}

void electrical_setup(void) {
Expand Down Expand Up @@ -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));
}
2 changes: 1 addition & 1 deletion sw/airborne/boards/ardrone/navdata.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
83 changes: 80 additions & 3 deletions sw/airborne/subsystems/imu/imu_ardrone2_raw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand All @@ -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();
Expand Down

0 comments on commit 1330f21

Please sign in to comment.