sensors: Get data from attitude filter, while disarmed, to warm up bias.

Zero the gyros in sensors.c when BiasCorrectGyro is disabled and then freeze.

flight/Modules/Stabilization/sensors.c

@@ -409,19 +409,23 @@ static void update_gyros(struct pios_sensor_gyro_data *gyros)
 	if (bias_correct_gyro || zero_during_arming) {
 		// Apply bias correction to the gyros from the state estimator
 		static GyrosBiasData gyrosBias = { 0 };
-		if (bias_correct_gyro) {
+		uint8_t status;
+		FlightStatusArmedGet(&status);
+
+		if (bias_correct_gyro || status == FLIGHTSTATUS_ARMED_DISARMED) {
 			GyrosBiasGet(&gyrosBias);
-		} else {
-			uint8_t status;
-			FlightStatusArmedGet(&status);
-			if (status == FLIGHTSTATUS_ARMED_ARMING) {
-				/* We're ignoring the complementary filter and are taking the readings here.
-				   If you jostle it during arming, ya done goofed. */
-				gyrosBias.x = (gyrosBias.x + gyrosData.x) * 0.5f;
-				gyrosBias.y = (gyrosBias.y + gyrosData.y) * 0.5f;
-				gyrosBias.z = (gyrosBias.z + gyrosData.z) * 0.5f;
-			}
+		} else if (status == FLIGHTSTATUS_ARMED_ARMING) {
+			/* We're ignoring the complementary filter from now on and are
+			   fixing up the existing data, then freeze the bias.
+
+			   If you jostle the craft during arming, ya done goofed. */
+			gyrosBias.x += 0.5f * (gyrosData.x - gyrosBias.x);
+			gyrosBias.y += 0.5f * (gyrosData.y - gyrosBias.y);
+			gyrosBias.z += 0.5f * (gyrosData.z - gyrosBias.z);
+
+			GyrosBiasSet(&gyrosBias);
 		}
+
 		gyrosData.x -= gyrosBias.x;
 		gyrosData.y -= gyrosBias.y;
 		gyrosData.z -= gyrosBias.z;