Make the sensors code only apply the GyroBias from the UAVO which is now coming

consistently only from the attitude module

flight/Modules/Attitude/revolution/attitude.c

@@ -367,7 +367,7 @@ static int32_t updateAttitudeComplimentary(bool first_run)
 
 		// If the mag is producing bad data don't use it (normally bad calibration)
 		if  (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) {
-			rot_mult(Rbe, home.Be, brot);
+			rot_mult(Rbe, homeLocation.Be, brot);
 
 			float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
 			mag.x /= mag_len;
@@ -571,6 +571,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	BaroAltitudeGet(&baroData);
 	GPSPositionGet(&gpsData);
 	GPSVelocityGet(&gpsVelData);
+	GyrosBiasGet(&gyrosBias);
 	HomeLocationGet(&home);
 
 	// Discard mag if it has NAN (normally from bad calibration)
@@ -604,6 +605,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 			INSSetGyroVar(revoCalibration.gyro_var);
 			INSSetBaroVar(revoCalibration.baro_var);
 
+			// Initialize the gyro bias from the settings
+			INSSetGyroBias(&gyrosBias.x);
+
 			xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
 			MagnetometerGet(&magData);
 
@@ -635,6 +639,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 
 			INSSetMagNorth(home.Be);
 
+			// Initialize the gyro bias from the settings
+			INSSetGyroBias(&gyrosBias.x);
+
 			GPSPositionData gpsPosition;
 			GPSPositionGet(&gpsPosition);
 
@@ -703,12 +710,21 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	else if(dT <= 0.001f)
 		dT = 0.001f;
 
+	// If the gyro bias setting was updated we should reset
+	// the state estimate of the EKF
+	if(gyroBiasSettingsUpdated) {
+		INSSetGyroBias(&gyrosBias.x);
+		gyroBiasSettingsUpdated = false;
+	}
+
 	// Because the sensor module remove the bias we need to add it
 	// back in here so that the INS algorithm can track it correctly
-	GyrosBiasGet(&gyrosBias);
+	float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f};
-	float gyros[3] = {(gyrosData.x - gyrosBias.x) * F_PI / 180.0f, 
+	if (revoCalibration.BiasCorrectedRaw) {
-		(gyrosData.y - gyrosBias.y) * F_PI / 180.0f, 
+		gyros[0] -= gyrosBias.x * F_PI / 180.0f;
-		(gyrosData.z - gyrosBias.z) * F_PI / 180.0f};
+		gyros[1] -= gyrosBias.y * F_PI / 180.0f;
+		gyros[2] -= gyrosBias.z * F_PI / 180.0f;
+	}
 
 	// Advance the state estimate
 	INSStatePrediction(gyros, &accelsData.x, dT);
@@ -723,11 +739,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
 	Quaternion2RPY(&attitude.q1,&attitude.Roll);
 	AttitudeActualSet(&attitude);
 
-	// Copy the gyro bias into the UAVO
+	if (revoCalibration.BiasCorrectedRaw && !gyroBiasSettingsUpdated) {
-	gyrosBias.x = -Nav.gyro_bias[0] * 180.0f / F_PI;
+		// Copy the gyro bias into the UAVO except when it was updated
-	gyrosBias.y = -Nav.gyro_bias[1] * 180.0f / F_PI;
+		// from the settings during the calculation, then consume it
-	gyrosBias.z = -Nav.gyro_bias[2] * 180.0f / F_PI;
+		// next cycle
-	GyrosBiasSet(&gyrosBias);
+		gyrosBias.x = -Nav.gyro_bias[0] * 180.0f / F_PI;
+		gyrosBias.y = -Nav.gyro_bias[1] * 180.0f / F_PI;
+		gyrosBias.z = -Nav.gyro_bias[2] * 180.0f / F_PI;
+		GyrosBiasSet(&gyrosBias);
+	}
 
 	// Advance the covariance estimate
 	INSCovariancePrediction(dT);

flight/Modules/Sensors/sensors.c

@@ -88,7 +88,6 @@ static float mag_bias[3] = {0,0,0};
 static float mag_scale[3] = {0,0,0};
 static float accel_bias[3] = {0,0,0};
 static float accel_scale[3] = {0,0,0};
-static float gyro_bias[3] = {0,0,0};
 
 static float R[3][3] = {{0}};
 static int8_t rotate = 0;
@@ -377,12 +376,12 @@ static void SensorsTask(void *parameters)
 		}
 		
 		if (bias_correct_gyro) {
-			// Apply bias correction to the gyros
+			// Apply bias correction to the gyros from the state estimator
 			GyrosBiasData gyrosBias;
 			GyrosBiasGet(&gyrosBias);
-			gyrosData.x += gyrosBias.x - gyro_bias[0];
+			gyrosData.x += gyrosBias.x;
-			gyrosData.y += gyrosBias.y - gyro_bias[1];
+			gyrosData.y += gyrosBias.y;
-			gyrosData.z += gyrosBias.z - gyro_bias[2];
+			gyrosData.z += gyrosBias.z;
 		}
 		GyrosSet(&gyrosData);
 		
@@ -449,9 +448,8 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
 	accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
 	accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
 	accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
-	gyro_bias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
+	// Do not store gyros_bias here as that comes from the state estimator and should be
-	gyro_bias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
+	// used from GyroBias directly
-	gyro_bias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
 
 	AttitudeSettingsData attitudeSettings;
 	AttitudeSettingsGet(&attitudeSettings);