Work on the INS algorithm

flight/Libraries/inc/insgps.h

@@ -39,6 +39,8 @@
   * @{
   */
 #define POS_SENSORS 0x007
+#define HORIZ_POS_SENSORS 0x003
+#define VER_POS_SENSORS 0x004
 #define HORIZ_SENSORS 0x018
 #define VERT_SENSORS  0x020
 #define MAG_SENSORS 0x1C0

flight/Modules/Attitude/revolution/attitude.c

@@ -62,6 +62,7 @@
 #include "baroaltitude.h"
 #include "flightstatus.h"
 #include "homelocation.h"
+#include "gpsposition.h"
 #include "CoordinateConversions.h"
 
 // Private constants
@@ -95,8 +96,6 @@ static float accelKp = 0;
 static float yawBiasRate = 0;
 static float gyroGain = 0.42;
 static int16_t accelbias[3];
-static float R[3][3];
-static int8_t rotate = 0;
 static bool zero_during_arming = false;
 
 
@@ -140,9 +139,9 @@ int32_t AttitudeInitialize(void)
 	gyrosBias.z = 0;
 	GyrosBiasSet(&gyrosBias);
 	
-	for(uint8_t i = 0; i < 3; i++)
+	//for(uint8_t i = 0; i < 3; i++)
-		for(uint8_t j = 0; j < 3; j++)
+	//	for(uint8_t j = 0; j < 3; j++)
-			R[i][j] = 0;
+	//		R[i][j] = 0;
 	
 	AttitudeSettingsConnectCallback(&settingsUpdatedCb);
 	
@@ -197,7 +196,7 @@ static void AttitudeTask(void *parameters)
 	while (1) {
 
 		// This  function blocks on data queue
-		if(1) 
+		if(0) 
 			updateAttitudeComplimentary(first_run);
 		else
 			updateAttitudeINSGPS(first_run);
@@ -398,6 +397,9 @@ static int32_t updateAttitudeComplimentary(bool first_run)
 #include "insgps.h"
 int32_t ins_failed = 0;
 extern struct NavStruct Nav;
+bool outdoor_mode = false;
+int32_t init_stage = 0;
+
 static int32_t updateAttitudeINSGPS(bool first_run)
 {
 	UAVObjEvent ev;
@@ -405,12 +407,25 @@ static int32_t updateAttitudeINSGPS(bool first_run)
 	AccelsData accelsData;
 	MagnetometerData magData;
 	BaroAltitudeData baroData;
+	GPSPositionData gpsData;
+	GyrosBiasData gyrosBias;
+
+	static bool mag_updated;
+	static bool baro_updated;
+	static bool gps_updated;
 
 	static uint32_t ins_last_time = 0;
-
 	static bool inited;
-	if (first_run)
+
-		inited = false;
+	float NED[3] = {0.0f, 0.0f, 0.0f};
+	float vel[3] = {0.0f, 0.0f, 0.0f};
+	float zeros[3] = {0.0f, 0.0f, 0.0f};
+
+	// Perform the update
+	uint16_t sensors = 0;
+	float dT;
+
+	inited = first_run ? false : inited;
 
 	// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
 	if ( (xQueueReceive(gyroQueue, &ev, 10 / portTICK_RATE_MS) != pdTRUE) ||
@@ -420,69 +435,120 @@ static int32_t updateAttitudeINSGPS(bool first_run)
 		return -1;
 	}
 
-	// Get most recent data
-	// TODO: Acquire all data in a queue
-	GyrosGet(&gyrosData);
-	AccelsGet(&accelsData);
-	MagnetometerGet(&magData);
-	BaroAltitudeGet(&baroData);
-	
-	bool mag_updated;
-	bool baro_updated;
-	bool gps_updated;
-	
 	if (inited) {
 		mag_updated = 0;
 		baro_updated = 0;
+		gps_updated = 0;
 	}
 
 	mag_updated |= xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
 	baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
-	gps_updated |= xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
+	gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
 
-	if (!inited && (!mag_updated || !baro_updated || !gps_updated)) {
+	// Get most recent data
+	GyrosGet(&gyrosData);
+	AccelsGet(&accelsData);
+	MagnetometerGet(&magData);
+	BaroAltitudeGet(&baroData);
+	GPSPositionGet(&gpsData);
+
+	// Have a minimum requirement for gps usage
+	gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP >= 3.0f);
+
+	if (!inited)
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
+	else if (outdoor_mode && gpsData.Satellites < 7)
+		AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
+	else
+		AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
+
+	if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) {
 		// Don't initialize until all sensors are read
-		return -1;
+		if (init_stage == 0 && !outdoor_mode) {
-	} else if (!inited ) {
+			float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
-		inited = true;
+			float q[4];
+			float var[3] = {500.0f, 500.0f, 500.0f};
+			float pos[3] = {0.0f, 0.0f, 0.0f};
+			pos[2] = baroData.Altitude * -1.0f;
 
-		float Rbe[3][3], q[4];
+			// Reset the INS algorithm
-		float ge[3]={0.0f,0.0f,-9.81f};
+			INSGPSInit();
-		float zeros[3]={0.0f,0.0f,0.0f};
+			var[0] = var[1] = var[2] = 5e-3f;
-		float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
+			INSSetMagVar(var);
-		float vel[3], NED[3];
+			var[0] = var[1] = var[2] = 1.5e-5f;
+			INSSetAccelVar(var);
+			var[0] = var[1] = var[2] = 2.0e-4f;
+			INSSetGyroVar(var);
 
-		INSGPSInit();
+			// Set initial attitude
+			float rpy[3];
+			rpy[0] = atan2f(accelsData.x, accelsData.z) * 180.0f / F_PI;
+			rpy[1] = atan2f(accelsData.y, accelsData.z) * 180.0f / F_PI;
+			rpy[2] = atan2f(magData.x, -magData.y) * 180.0f / F_PI;
+			RPY2Quaternion(rpy,q);
 
-		HomeLocationData home;
+			INSSetState(pos, zeros, q, zeros, zeros);
-		HomeLocationGet(&home);
+			INSResetP(Pdiag);
+		} else if (init_stage == 0 && outdoor_mode) {
+			float q[4], rpy[3];
+			float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
+			float NED[3];
+			float var[3];
 
-		GPSPositionData gpsPosition;
+			// Reset the INS algorithm
-		GPSPositionGet(&gpsPosition);
+			INSGPSInit();
+			var[0] = var[1] = var[2] = 5e-3f;
+			INSSetMagVar(var);
+			var[0] = var[1] = var[2] = 1.5e-5f;
+			INSSetAccelVar(var);
+			var[0] = var[1] = var[2] = 2.0e-4f;
+			INSSetGyroVar(var);
 
-		vel[0] = gpsPosition.Groundspeed * cosf(gpsPosition.Heading * F_PI / 180.0f);
+			HomeLocationData home;
-		vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
+			HomeLocationGet(&home);
-		vel[2] = 0;
+			INSSetMagNorth(home.Be);
 
-		// convert from cm back to meters
+			GPSPositionData gpsPosition;
-		float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, (float) gpsPosition.Longitude / 1e7f, (float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
+			GPSPositionGet(&gpsPosition);
-		// put in local NED frame
-		float ECEF[3] = {(float) (home.ECEF[0] / 100.0f), (float) (home.ECEF[1] / 100.0f), (float) (home.ECEF[2] / 100.0f)};
-		LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
 
-		RotFrom2Vectors(&accelsData.x, ge, &magData.x, home.Be, Rbe);
+			// convert from cm back to meters
-		R2Quaternion(Rbe,q);
+			float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, 
-		INSSetState(NED, vel, q, &gyrosData.x, zeros);
+				(float) gpsPosition.Longitude / 1e7f, 
-		INSSetGyroBias(&gyrosData.x);
+				(float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
-		INSResetP(Pdiag);
+			// put in local NED frame
+			float ECEF[3] = {(float) (home.ECEF[0] / 100.0f),
+				(float) (home.ECEF[1] / 100.0f),
+				(float) (home.ECEF[2] / 100.0f)};
+			LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
+
+			// Set initial attitude
+			rpy[0] = atan2f(accelsData.x, accelsData.z) * 180.0f / F_PI;
+			rpy[1] = atan2f(accelsData.y, accelsData.z) * 180.0f / F_PI;
+			rpy[2] = atan2f(magData.x, -magData.y) * 180.0f / F_PI;
+			RPY2Quaternion(rpy,q);
+
+			INSSetState(NED, zeros, q, zeros, zeros);
+			INSResetP(Pdiag);
+		} else if (init_stage > 0) {
+			// Run prediction a bit before any corrections
+			GyrosBiasGet(&gyrosBias);
+			float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, 
+				(gyrosData.y + gyrosBias.y) * F_PI / 180.0f, 
+				(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
+			INSStatePrediction(gyros, &accelsData.x, 0.002f);
+		}
+
+		init_stage++;
+		if(init_stage > 500)
+			inited = true;
 
 		ins_last_time = PIOS_DELAY_GetRaw();	
-		return 0;
+
+		return -1;
 	}
 
-	// Perform the update	
+	if (!inited)
-	uint16_t sensors = 0;
+		return -1;
-	float dT;
 
 	dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
 	ins_last_time = PIOS_DELAY_GetRaw();
@@ -493,10 +559,9 @@ static int32_t updateAttitudeINSGPS(bool first_run)
 	else if(dT <= 0.001f)
 		dT = 0.001f;
 
-	
+	// Because the sensor module remove the bias we need to add it
-	GyrosBiasData gyrosBias;
+	// back in here so that the INS algorithm can track it correctly 
 	GyrosBiasGet(&gyrosBias);
-
 	float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, 
 		(gyrosData.y + gyrosBias.y) * F_PI / 180.0f, 
 		(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
@@ -523,17 +588,21 @@ static int32_t updateAttitudeINSGPS(bool first_run)
 	// Advance the covariance estimate
 	INSCovariancePrediction(dT);
 
-	if(mag_updated)
+	//if(mag_updated)
-		sensors |= MAG_SENSORS;
+	//	sensors |= MAG_SENSORS;
+
 	if(baro_updated)
 		sensors |= BARO_SENSOR;
 
-	float NED[3] = {0,0,0};
+	HomeLocationData home;
-	float vel[3] = {0,0,0};
+	HomeLocationGet(&home);
 	
-	if(gps_updated)
+	INSSetMagNorth(home.Be);
+
+	if(gps_updated && outdoor_mode)
 	{
-		sensors = HORIZ_SENSORS | VERT_SENSORS;
+		INSSetPosVelVar(1.0f, 1.0f);
+		sensors = POS_SENSORS; //HORIZ_SENSORS | VERT_SENSORS;
 		GPSPositionData gpsPosition;
 		GPSPositionGet(&gpsPosition);
 
@@ -541,21 +610,30 @@ static int32_t updateAttitudeINSGPS(bool first_run)
 		vel[1] = gpsPosition.Groundspeed * sinf(gpsPosition.Heading * F_PI / 180.0f);
 		vel[2] = 0;
 
-		HomeLocationData home;
-		HomeLocationGet(&home);
-
 		// convert from cm back to meters
-		float LLA[3] = {(float) gpsPosition.Latitude / 1e7f, (float) gpsPosition.Longitude / 1e7f, (float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
+		float LLA[3] = {(float) gpsPosition.Latitude / 1e7f,
+			(float) gpsPosition.Longitude / 1e7f,
+			(float) (gpsPosition.GeoidSeparation + gpsPosition.Altitude)};
 		// put in local NED frame
-		float ECEF[3] = {(float) (home.ECEF[0] / 100.0f), (float) (home.ECEF[1] / 100.0f), (float) (home.ECEF[2] / 100.0f)};
+		float ECEF[3] = {(float) (home.ECEF[0] / 100.0f),
+			(float) (home.ECEF[1] / 100.0f),
+			(float) (home.ECEF[2] / 100.0f)};
 		LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, NED);
+	} else if (!outdoor_mode) {
+		//INSSetPosVelVar(0.1f, 0.1f);
+		vel[0] = vel[1] = vel[2] = 0;
+		NED[0] = NED[1] = 0;
+		NED[2] = baroData.Altitude;
+		sensors |= HORIZ_POS_SENSORS;
+		//sensors |= HORIZ_SENSORS | VERT_SENSORS | POS_SENSORS;
 	}
 
 	/*
 	 * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
 	 * although probably should occur within INS itself
 	 */
-	INSCorrection(&magData.x, NED, vel, baroData.Altitude, sensors);
+	if (sensors)
+		INSCorrection(&magData.x, NED, vel, baroData.Altitude, sensors);
 	
 	// Copy the position and velocity into the UAVO
 	PositionActualData positionActual;
@@ -572,7 +650,6 @@ static int32_t updateAttitudeINSGPS(bool first_run)
 	velocityActual.Down = Nav.Vel[2];
 	VelocityActualSet(&velocityActual);
 	
-
 	if(fabs(Nav.gyro_bias[0]) > 0.1f || fabs(Nav.gyro_bias[1]) > 0.1f || fabs(Nav.gyro_bias[2]) > 0.1f) {
 		float zeros[3] = {0.0f,0.0f,0.0f};
 		INSSetGyroBias(zeros);
@@ -586,7 +663,6 @@ static void settingsUpdatedCb(UAVObjEvent * objEv)
 	AttitudeSettingsData attitudeSettings;
 	AttitudeSettingsGet(&attitudeSettings);
 
-
 	accelKp = attitudeSettings.AccelKp;
 	accelKi = attitudeSettings.AccelKi;
 	yawBiasRate = attitudeSettings.YawBiasRate;
@@ -604,26 +680,8 @@ static void settingsUpdatedCb(UAVObjEvent * objEv)
 	gyrosBias.y = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Y] / 100.0f;
 	gyrosBias.z = attitudeSettings.GyroBias[ATTITUDESETTINGS_GYROBIAS_Z] / 100.0f;
 	GyrosBiasSet(&gyrosBias);
-	
-	// Indicates not to expend cycles on rotation
-	if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
-	   attitudeSettings.BoardRotation[2] == 0) {
-		rotate = 0;
-
-		// Shouldn't be used but to be safe
-		float rotationQuat[4] = {1,0,0,0};
-		Quaternion2R(rotationQuat, R);
-	} else {
-		float rotationQuat[4];
-		const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL],
-			attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH],
-			attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW]};
-		RPY2Quaternion(rpy, rotationQuat);
-		Quaternion2R(rotationQuat, R);
-		rotate = 1;
-	}
 }
 /**
-  * @}
+ * @}
-  * @}
+ * @}
-  */
+ */

matlab/ins/insgps.c

@@ -231,6 +231,17 @@ void MagCorrection(float mag_data[3])
 	INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
 }
 
+void BaroCorrection(float baro)
+{
+    INSCorrection(zeros, zeros, zeros, baro, BARO_SENSOR);
+}
+void GpsCorrection(float Pos[3], float Vel[3])
+{
+    INSCorrection(zeros, Pos, Vel, zeros[0],
+		      POS_SENSORS); // | HORIZ_SENSORS | VERT_SENSORS);
+}
+
+
 void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
 {
 	INSCorrection(mag_data, zeros, Vel, BaroAlt,
@@ -241,7 +252,7 @@ void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
 void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
 {
 	INSCorrection(zeros, Pos, Vel, BaroAlt,
-		      HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
+		      POS_SENSORS | HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
 }
 
 void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],

matlab/ins/insgps.h

@@ -62,6 +62,8 @@ void INSSetMagVar(float scaled_mag_var[3]);
 void INSPosVelReset(float pos[3], float vel[3]);
 
 void MagCorrection(float mag_data[3]);
+void BaroCorrection(float baro);
+void GpsCorrection(float Pos[3], float Vel[3]);
 void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
 void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
 		    float BaroAlt);