I haven't had chance to try this out feeding the value back to the crazyflie's heading, but wanted to share it anyway, as I probably wont get chance to work on this much more till next weekend.
I take the heading from the stabilizer and use that to get offset samples of the mag reading for different angles. When you average these, they should be in the center.
This is my first attempt and it assumes that the crazyflie is flat - it doesnt compensate for the angle of the crazyflie itself.
Code: Select all
diff -r e742216f7c44 modules/src/stabilizer.c
--- a/modules/src/stabilizer.c Mon Oct 28 16:50:41 2013 +0100
+++ b/modules/src/stabilizer.c Mon Dec 02 00:34:52 2013 +0000
@@ -42,6 +42,8 @@
#include "ledseq.h"
#include "param.h"
#include "ms5611.h"
+#include "debug.h"
+#include "hmc5883l.h"
#undef max
#define max(a,b) ((a) > (b) ? (a) : (b))
@@ -70,6 +72,7 @@
PRIVATE Axis3f gyro; // Gyro axis data in deg/s
PRIVATE Axis3f acc; // Accelerometer axis data in mG
+PRIVATE Axis3f mag;
PRIVATE float eulerRollActual;
PRIVATE float eulerPitchActual;
@@ -191,6 +194,12 @@
LOG_ADD(LOG_FLOAT, vSpeedAcc, &vSpeedAcc)
LOG_GROUP_STOP(altHold)
+LOG_GROUP_START(magneto)
+LOG_ADD(LOG_FLOAT, x, &mag.x)
+LOG_ADD(LOG_FLOAT, y, &mag.y)
+LOG_ADD(LOG_FLOAT, z, &mag.z)
+LOG_GROUP_STOP(magneto)
+
static bool isInit;
static void stabilizerAltHoldUpdate(void);
@@ -237,13 +246,23 @@
{
uint32_t attitudeCounter = 0;
uint32_t altHoldCounter = 0;
+ uint32_t magCounter = 0;
uint32_t lastWakeTime;
-
+ Axis3f samples[8];
+ int16_t mx, my, mz;
+ int i = 0;
+ for(;i < 8; ++i)
+ {
+ samples[i].x = samples[i].y = samples[i].z = 0;
+ }
+
vTaskSetApplicationTaskTag(0, (void*)TASK_STABILIZER_ID_NBR);
//Wait for the system to be fully started to start stabilization loop
+ DEBUG_PRINT("Task");
systemWaitStart();
+ DEBUG_PRINT("Started");
lastWakeTime = xTaskGetTickCount ();
while(1)
@@ -251,6 +270,11 @@
vTaskDelayUntil(&lastWakeTime, F2T(IMU_UPDATE_FREQ)); // 500Hz
imu6Read(&gyro, &acc);
+
+ hmc5883lGetHeading(&mx, &my, &mz);
+ mag.x = 1.f * mx;
+ mag.y = 1.f * my;
+ mag.z = 1.f * mz;
if (imu6IsCalibrated())
{
@@ -260,9 +284,40 @@
// 250HZ
if (++attitudeCounter >= ATTITUDE_UPDATE_RATE_DIVIDER)
{
+ int i;
sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT);
sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual);
+ i = (int)floor(180 + eulerYawActual)/45.f;
+ samples[i] = mag;
+
+ if(magCounter++ > 500)
+ {
+ uint8_t count = 0;
+ Axis3f acc = {0,0,0};
+ magCounter = 0;
+
+ for(i =0; i< 8; ++i)
+ {
+ if (samples[i].x != 0 && samples[i].y != 0 && samples[i].z != 0)
+ ++count;
+ acc.x += samples[i].x;
+ acc.y += samples[i].y;
+ acc.z += samples[i].z;
+ }
+
+ acc.x /= count;
+ acc.y /= count;
+ acc.z /= count;
+
+ DEBUG_PRINT("C = %d %f %f %f\n", count, acc.x, acc.y, acc.z);
+ DEBUG_PRINT("Offset Mag %f %f %f\n", mag.x - acc.x, mag.y - acc.y, mag.z - acc.z);
+ DEBUG_PRINT("Heading %f", atan2(mag.x - acc.x, mag.y - acc.y)* 180 / M_PI);
+
+ hmc5883lSetMode(HMC5883L_MODE_CONTINUOUS);
+ }
+
+
accWZ = sensfusion6GetAccZWithoutGravity(acc.x, acc.y, acc.z);
// Estimate speed from acc (drifts)
vSpeed += deadband(accWZ, vAccDeadband) * FUSION_UPDATE_DT;
@@ -366,8 +421,7 @@
const float pre_integral = altHoldPID.integ;
// Reset PID controller
- pidInit(&altHoldPID, asl, altHoldKp, altHoldKi, altHoldKd,
- HOVER_UPDATE_DT);
+ pidInit(&altHoldPID, asl, altHoldKp, altHoldKi, altHoldKd, HOVER_UPDATE_DT);
// TODO set low and high limits depending on voltage
// TODO for now just use previous I value and manually set limits for whole voltage range
// pidSetIntegralLimit(&altHoldPID, 12345);