rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Changes in files supporting AHRS for new initialization methods.

Browse Source 
Changes in ahrs.c for new initialization and to fix issues with outdoor algorithm.  The changes in ahrs.c are pretty messy, but committed mostly to get the code to Peabody for more extensive restructuring of ahrs.c.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2150 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
dschin 2010-11-24 01:27:43 +00:00 committed by dschin
parent cfbdad5e0d
commit b2a538375c
6 changed files with 284 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
flight/AHRS/ahrs.c
View File

@ -41,7 +41,7 @@
#define MAX_OVERSAMPLING 50 /* cannot have more than 50 samples */
#define INSGPS_GPS_TIMEOUT 2 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINSAT 7 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINSAT 6 /* 2 seconds triggers reinit of position */
#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates */
#define INSGPS_MAGLEN 1000
#define INSGPS_MAGTOL 0.2 /* error in magnetic vector length to use */
@ -49,13 +49,13 @@
// For debugging the raw sensors
//#define DUMP_RAW
//#define DUMP_FRIENDLY
//#define DUMP_EKF
#define DUMP_EKF
#ifdef DUMP_EKF
#define NUMX 13 // number of states, X is the state vector
#define NUMW 9 // number of plant noise inputs, w is disturbance noise vector
#define NUMV 10 // number of measurements, v is the measurement noise vector
#define NUMU 6 // number of deterministic inputs, U is the input vector
#define NUMU 7 // number of deterministic inputs, U is the input vector
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
@ -63,6 +63,7 @@ extern float K[NUMX][NUMV]; // feedback gain matrix
#endif
volatile int8_t ahrs_algorithm;
volatile int8_t last_ahrs_algorithm;
/**
* @addtogroup AHRS_Structures Local Structres
@ -170,6 +171,7 @@ void altitude_callback(AhrsObjHandle obj);
void calibration_callback(AhrsObjHandle obj);
void gps_callback(AhrsObjHandle obj);
void settings_callback(AhrsObjHandle obj);
void InitAlgorithm(void);
volatile uint32_t last_counter_idle_start = 0;
volatile uint32_t last_counter_idle_end = 0;
@ -199,6 +201,7 @@ static uint8_t adc_oversampling = 20;
*/
int main()
{
uint8_t spike=0;
float gyro[3], accel[3];
float vel[3] = { 0, 0, 0 };
uint8_t gps_dirty = 1;
@ -309,12 +312,17 @@ for all data to be up to date before doing anything*/
/******************* Main EKF loop ****************************/
while(1) {
AhrsPoll();
AhrsStatusData status;
AhrsStatusGet(&status);
status.CPULoad = ((float)running_counts /
(float)(idle_counts + running_counts)) * 100;
status.IdleTimePerCyle = idle_counts / (timer_rate() / 10000);
// status.IdleTimePerCyle = idle_counts / (timer_rate() / 10000);
// ***************************
status.IdleTimePerCyle=spike;
// ***************************
status.RunningTimePerCyle = running_counts / (timer_rate() / 10000);
status.DroppedUpdates = ekf_too_slow;
up_time = timer_count();
@ -381,14 +389,34 @@ for all data to be up to date before doing anything*/
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1); // mag update (45)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4); // mag data (46:57)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:82)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:85)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * NUMX); // X (83:134)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * NUMX); // X (86:137)
for(uint8_t i = 0; i < NUMX; i++)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &(P[i][i]), 4); // diag(P) (135:186)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &(P[i][i]), 4); // diag(P) (138:189)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); // BaroAlt (190:193)
#endif
//*****************************************
//*****************************************
//*****************************************
//*****************************************
if (ahrs_algorithm != last_ahrs_algorithm)
InitAlgorithm();
last_ahrs_algorithm = ahrs_algorithm;
//*****************************************
//*****************************************
//*****************************************
//*****************************************
/******************** INS ALGORITHM **************************/
if (ahrs_algorithm != AHRSSETTINGS_ALGORITHM_SIMPLE) {
// format data for INS algo
@ -428,21 +456,17 @@ for all data to be up to date before doing anything*/
vel[2] = 0;
INSSetPosVelVar(0.004);
// INSSetPosVelVar(0.004);
INSSetPosVelVar(0.04);
if (mag_data.updated && !gps_dirty) {
//TOOD: add check for altitude updates
FullCorrection(mag_data.scaled.axis,
gps_data.NED,
vel,
altitude_data.
altitude);
//FullCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude);
GpsMagCorrection(mag_data.scaled.axis, gps_data.NED, vel);
mag_data.updated = 0;
} else if (!gps_dirty) {
GpsBaroCorrection(gps_data.NED,
vel,
altitude_data.
altitude);
//GpsBaroCorrection(gps_data.NED, vel, altitude_data.altitude);
GpsBaroCorrection(gps_data.NED, vel, -gps_data.NED[2]);
} else { // GPS hasn't updated for a bit
INSPosVelReset(gps_data.NED,vel);
}
@ -452,8 +476,11 @@ for all data to be up to date before doing anything*/
&& mag_data.updated == 1) {
MagCorrection(mag_data.scaled.axis); // only trust mags if outdoors
mag_data.updated = 0;
} else {
}
else if (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR){ //Do no update
} else {
// Indoors, update with zero position and velocity and high covariance
spike++;
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
INSSetPosVelVar(settings.IndoorVelocityVariance);
@ -479,8 +506,7 @@ for all data to be up to date before doing anything*/
INSPosVelReset(vel,vel);
if((mag_data.updated == 1) && (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
MagVelBaroCorrection(mag_data.scaled.axis,
vel,altitude_data.altitude); // only trust mags if outdoors
MagVelBaroCorrection(mag_data.scaled.axis,vel,altitude_data.altitude); // only trust mags if outdoors
mag_data.updated = 0;
} else {
VelBaroCorrection(vel, altitude_data.altitude);
@ -864,6 +890,18 @@ void gps_callback(AhrsObjHandle obj)
HomeLocationData home;
HomeLocationGet(&home);
// convert from cm back to meters
double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
// put in local NED frame
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
gps_data.heading = pos.Heading;
gps_data.groundspeed = pos.Groundspeed;
gps_data.quality = 1; /* currently unused */
gps_data.updated = true;
// if poor don't use this update
if((ahrs_algorithm != AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) ||
(pos.Satellites < INSGPS_GPS_MINSAT) ||
(pos.PDOP >= INSGPS_GPS_MINPDOP) ||
@ -872,18 +910,8 @@ void gps_callback(AhrsObjHandle obj)
{
gps_data.quality = 0;
gps_data.updated = false;
return;
}
double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
// convert from cm back to meters
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
gps_data.heading = pos.Heading;
gps_data.groundspeed = pos.Groundspeed;
gps_data.quality = 1; /* currently unused */
gps_data.updated = true;
}
void altitude_callback(AhrsObjHandle obj)
@ -934,3 +962,45 @@ void homelocation_callback(AhrsObjHandle obj)
/**
* @}
*/
void InitAlgorithm()
{
float Rbe[3][3], q[4], accels[3], rpy[3], mag;
float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[13]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5};
bool using_mags, using_gps;
HomeLocationData home;
HomeLocationGet(&home);
accels[0]=accel_data.filtered.x;
accels[1]=accel_data.filtered.y;
accels[2]=accel_data.filtered.z;
using_mags = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR);
using_gps = (ahrs_algorithm == AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR);
if (using_mags){
RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
R2Quaternion(Rbe,q);
if (using_gps)
INSSetState(gps_data.NED, zeros, q, zeros);
else
INSSetState(zeros, zeros, q, zeros);
}
else{
// assume yaw = 0
mag = VectorMagnitude(accels);
rpy[1] = asinf(-accels[0]/mag);
rpy[0] = atan2(accels[1]/mag,accels[2]/mag);
rpy[2] = 0;
RPY2Quaternion(rpy,q);
if (using_gps)
INSSetState(gps_data.NED, zeros, q, zeros);
else
INSSetState(zeros, zeros, q, zeros);
}
INSResetP(Pdiag);
// TODO: include initial estimate of gyro bias?
}

2
flight/AHRS/inc/insgps.h
View File

@ -53,6 +53,8 @@ void INSGPSInit();
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
void INSCovariancePrediction(float dT);
void INSResetP(float PDiag[13]);
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3]);
void INSSetPosVelVar(float PosVar);
void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]);

31
flight/AHRS/insgps.c
View File

@ -104,6 +104,37 @@ void INSGPSInit() //pretty much just a place holder for now
R[9] = .05; // High freq altimeter noise variance (m^2)
}
void INSResetP(float PDiag[NUMX])
{
uint8_t i,j;
// if PDiag[i] nonzero then clear row and column and set diagonal element
for (i=0;i<NUMX;i++){
if (PDiag != 0){
for (j=0;j<NUMX;j++)
P[i][j]=P[j][i]=0;
P[i][i]=PDiag[i];
}
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3])
{
X[0] = pos[0];
X[1] = pos[1];
X[2] = pos[2];
X[3] = vel[0];
X[4] = vel[1];
X[5] = vel[2];
X[6] = q[0];
X[7] = q[1];
X[8] = q[2];
X[9] = q[3];
X[10] = gyro_bias[0];
X[11] = gyro_bias[1];
X[12] = gyro_bias[2];
}
void INSPosVelReset(float pos[3], float vel[3])
{
for (int i = 0; i < 6; i++) {

133
flight/Libraries/CoordinateConversions.c
View File

@ -137,6 +137,8 @@ void Quaternion2RPY(float q[4], float rpy[3])
rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
rpy[2] = RAD2DEG * atan2f(R12, R11);
rpy[0] = RAD2DEG * atan2f(R23, R33);
//TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2
}
// ****** find quaternion from roll, pitch, yaw ********
@ -215,3 +217,134 @@ void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]
NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
}
// ****** convert Rotation Matrix to Quaternion ********
// ****** if R converts from e to b, q is rotation from e to b ****
void R2Quaternion(float R[3][3], float q[4])
{
float m[4], mag;
uint8_t index,i;
m[0] = 1 + R[0][0] + R[1][1] + R[2][2];
m[1] = 1 + R[0][0] - R[1][1] - R[2][2];
m[2] = 1 - R[0][0] + R[1][1] - R[2][2];
m[3] = 1 - R[0][0] - R[1][1] + R[2][2];
// find maximum divisor
index = 0;
mag = m[0];
for (i=1;i<4;i++){
if (m[i] > mag){
mag = m[i];
index = i;
}
}
mag = 2*sqrt(mag);
if (index == 0) {
q[0] = mag/4;
q[1] = (R[1][2]-R[2][1])/mag;
q[2] = (R[2][0]-R[0][2])/mag;
q[3] = (R[0][1]-R[1][0])/mag;
}
else if (index == 1) {
q[1] = mag/4;
q[0] = (R[1][2]-R[2][1])/mag;
q[2] = (R[0][1]+R[1][0])/mag;
q[3] = (R[0][2]+R[2][0])/mag;
}
else if (index == 2) {
q[2] = mag/4;
q[0] = (R[2][0]-R[0][2])/mag;
q[1] = (R[0][1]+R[1][0])/mag;
q[3] = (R[1][2]+R[2][1])/mag;
}
else {
q[3] = mag/4;
q[0] = (R[0][1]-R[1][0])/mag;
q[1] = (R[0][2]+R[2][0])/mag;
q[2] = (R[1][2]+R[2][1])/mag;
}
// q0 positive, i.e. angle between pi and -pi
if (q[0] < 0){
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
}
// ****** Rotation Matrix from Two Vector Directions ********
// ****** given two vector directions (v1 and v2) known in two frames (b and e) find Rbe ***
// ****** solution is approximate if can't be exact ***
uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3])
{
float Rib[3][3], Rie[3][3];
float mag;
uint8_t i,j,k;
// identity rotation in case of error
for (i=0;i<3;i++){
for (j=0;j<3;j++)
Rbe[i][j]=0;
Rbe[i][i]=1;
}
// The first rows of rot matrices chosen in direction of v1
mag = VectorMagnitude(v1b);
if (fabs(mag) < 1e-30)
return (-1);
for (i=0;i<3;i++)
Rib[0][i]=v1b[i]/mag;
mag = VectorMagnitude(v1e);
if (fabs(mag) < 1e-30)
return (-1);
for (i=0;i<3;i++)
Rie[0][i]=v1e[i]/mag;
// The second rows of rot matrices chosen in direction of v1xv2
CrossProduct(v1b,v2b,&Rib[1][0]);
mag = VectorMagnitude(&Rib[1][0]);
if (fabs(mag) < 1e-30)
return (-1);
for (i=0;i<3;i++)
Rib[1][i]=Rib[1][i]/mag;
CrossProduct(v1e,v2e,&Rie[1][0]);
mag = VectorMagnitude(&Rie[1][0]);
if (fabs(mag) < 1e-30)
return (-1);
for (i=0;i<3;i++)
Rie[1][i]=Rie[1][i]/mag;
// The third rows of rot matrices are XxY (Row1xRow2)
CrossProduct(&Rib[0][0],&Rib[1][0],&Rib[2][0]);
CrossProduct(&Rie[0][0],&Rie[1][0],&Rie[2][0]);
// Rbe = Rbi*Rie = Rib'*Rie
for (i=0;i<3;i++)
for(j=0;j<3;j++){
Rbe[i][j]=0;
for(k=0;k<3;k++)
Rbe[i][j] += Rib[k][i]*Rie[k][j];
}
return 1;
}
// ****** Vector Cross Product ********
void CrossProduct(const float v1[3], const float v2[3], float result[3])
{
result[0] = v1[1]*v2[2] - v2[1]*v1[2];
result[1] = v2[0]*v1[2] - v1[0]*v2[2];
result[2] = v1[0]*v2[1] - v2[0]*v1[1];
}
// ****** Vector Magnitude ********
float VectorMagnitude(const float v[3])
{
return(sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
}

17
flight/Libraries/inc/CoordinateConversions.h
View File

@ -53,4 +53,21 @@ void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
// ****** Express ECEF in a local NED Base Frame ********
void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3]);
// ****** convert Rotation Matrix to Quaternion ********
// ****** if R converts from e to b, q is rotation from e to b ****
void R2Quaternion(float R[3][3], float q[4]);
// ****** Rotation Matrix from Two Vector Directions ********
// ****** given two vector directions (v1 and v2) known in two frames (b and e) find Rbe ***
// ****** solution is approximate if can't be exact ***
uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3]);
// ****** Vector Cross Product ********
void CrossProduct(const float v1[3], const float v2[3], float result[3]);
// ****** Vector Magnitude ********
float VectorMagnitude(const float v[3]);
#endif // COORDINATECONVERSIONS_H_

2
flight/OpenPilot/Makefile
View File

@ -477,7 +477,7 @@ UNAME := $(shell uname)
ifeq ($(UNAME), Darwin)
OOCD_CL+=-f ../Project/OpenOCD/floss-jtag.openpilot.osx.cfg -f ../Project/OpenOCD/stm32.cfg
else
OOCD_CL+=-f ../Project/OpenOCD/floss-jtag-revb.cfg -f ../Project/OpenOCD/stm32.cfg
OOCD_CL+=-f ../Project/OpenOCD/floss-jtag.openpilot.cfg -f ../Project/OpenOCD/stm32.cfg
endif
# initialize
OOCD_CL+=-c init