rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

added missing attitude state variable ;)

Browse Source
This commit is contained in:
Corvus Corax 2013-05-20 10:42:23 +02:00
parent 34294d2635
commit 1bd9cdafab
2 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -36,16 +36,18 @@ typedef enum {
gyr_UPDATED = 1 << 0, gyr_UPDATED = 1 << 0,
acc_UPDATED = 1 << 1, acc_UPDATED = 1 << 1,
mag_UPDATED = 1 << 2, mag_UPDATED = 1 << 2,
pos_UPDATED = 1 << 3, att_UPDATED = 1 << 3,
vel_UPDATED = 1 << 4, pos_UPDATED = 1 << 4,
air_UPDATED = 1 << 5, vel_UPDATED = 1 << 5,
bar_UPDATED = 1 << 6, air_UPDATED = 1 << 6,
bar_UPDATED = 1 << 7,
} sensorUpdates; } sensorUpdates;
typedef struct { typedef struct {
float gyr[3]; float gyr[3];
float acc[3]; float acc[3];
float mag[3]; float mag[3];
float att[4];
float pos[3]; float pos[3];
float vel[3]; float vel[3];
float air[2]; float air[2];

14
flight/modules/StateEstimation/stateestimation.c
View File

@ -44,6 +44,7 @@
#include <magstate.h> #include <magstate.h>
#include <barostate.h> #include <barostate.h>
#include <airspeedstate.h> #include <airspeedstate.h>
#include <attitudestate.h>
#include <positionstate.h> #include <positionstate.h>
#include <velocitystate.h> #include <velocitystate.h>
@ -51,7 +52,7 @@
#include "homelocation.h" #include "homelocation.h"
#include "flightstatus.h" #include "flightstatus.h"
// #include "CoordinateConversions.h" #include "CoordinateConversions.h"
// Private constants // Private constants
#define STACK_SIZE_BYTES 2048 #define STACK_SIZE_BYTES 2048
@ -397,6 +398,17 @@ static void StateEstimationCb(void)
statename##Set(&s); \ statename##Set(&s); \
} }
STORE1(BaroState, bar, Altitude); STORE1(BaroState, bar, Altitude);
// attitude nees manual conversion from quaternion to euler
if (ISSET(states.updated, att_UPDATED)) { \
AttitudeStateData s;
AttitudeStateGet(&s);
s.q1 = states.att[0];
s.q2 = states.att[1];
s.q3 = states.att[2];
s.q4 = states.att[3];
Quaternion2RPY(&s.q1, &s.Roll);
AttitudeStateSet(&s);
}
// clear alarms if everything is alright, then schedule callback execution after timeout // clear alarms if everything is alright, then schedule callback execution after timeout