diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml
index 09f5b3f97d..f72c47161c 100644
--- a/conf/airframes/ardrone2_raw.xml
+++ b/conf/airframes/ardrone2_raw.xml
@@ -22,18 +22,16 @@
-
+
-
@@ -71,16 +69,16 @@
-
-
-
+
+
+
-
-
+
+
@@ -90,19 +88,19 @@
-
-
+
+
-
-
-
-
-
-
+
+
+
+
+
+
-
+
-
+
@@ -232,6 +230,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..99a9f39d52 100644
--- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h
+++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h
@@ -39,9 +39,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();