diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml
index 09f5b3f97d..1a4f10df16 100644
--- a/conf/airframes/ardrone2_raw.xml
+++ b/conf/airframes/ardrone2_raw.xml
@@ -22,18 +22,16 @@
-
+
-
@@ -71,38 +69,14 @@
-
-
-
-
-
-
+
+
+
-
-
-
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -232,6 +206,7 @@
+
diff --git a/sw/airborne/boards/ardrone/electrical_raw.c b/sw/airborne/boards/ardrone/electrical_raw.c
index 6dd9e93cd0..f47f0157bd 100644
--- a/sw/airborne/boards/ardrone/electrical_raw.c
+++ b/sw/airborne/boards/ardrone/electrical_raw.c
@@ -36,9 +36,35 @@
#include
#include
#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));
}
diff --git a/sw/airborne/boards/ardrone/navdata.h b/sw/airborne/boards/ardrone/navdata.h
index ec40241dcc..922bb2e46b 100644
--- a/sw/airborne/boards/ardrone/navdata.h
+++ b/sw/airborne/boards/ardrone/navdata.h
@@ -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;
diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h
index 9688459bca..18b1b1bbad 100644
--- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h
+++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h
@@ -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();