diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index af0286eda..e6e62f084 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -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? +} diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h index 84f3ce201..91838b47d 100644 --- a/flight/AHRS/inc/insgps.h +++ b/flight/AHRS/inc/insgps.h @@ -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]); diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c index 140ca211a..a150d4d54 100644 --- a/flight/AHRS/insgps.c +++ b/flight/AHRS/insgps.c @@ -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 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])); +} + diff --git a/flight/Libraries/inc/CoordinateConversions.h b/flight/Libraries/inc/CoordinateConversions.h index 67128bbdd..3278e5dfe 100644 --- a/flight/Libraries/inc/CoordinateConversions.h +++ b/flight/Libraries/inc/CoordinateConversions.h @@ -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_ diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 83b944262..f82a5e1ce 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -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