AP_Compass_AK8963: fix sem handling

In case of error or zeroed data, the i2c semaphore wasn't given.
It happened at first startup on Bebop and caused a failure:
"PANIC: failed to take _bus->sem 100 times in a row..."

libraries/AP_Compass/AP_Compass_AK8963.cpp

@@ -237,30 +237,31 @@ void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) co
 
 void AP_Compass_AK8963::_update()
 {
+    struct AP_AK8963_SerialBus::raw_value rv;
+    float mag_x, mag_y, mag_z;
+
     if (hal.scheduler->micros() - _last_update_timestamp < 10000) {
-        return;
+        goto end;
     }
 
     if (!_sem_take_nonblocking()) {
-        return;
+        goto end;
     }
 
-    struct AP_AK8963_SerialBus::raw_value rv;
-
     _bus->read_raw(&rv);
 
     /* Check for overflow. See AK8963's datasheet, section
      * 6.4.3.6 - Magnetic Sensor Overflow. */
     if ((rv.st2 & 0x08)) {
-        return;
+        goto fail;
     }
 
-    float mag_x = (float) rv.val[0];
-    float mag_y = (float) rv.val[1];
-    float mag_z = (float) rv.val[2];
+    mag_x = (float) rv.val[0];
+    mag_y = (float) rv.val[1];
+    mag_z = (float) rv.val[2];
 
     if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) {
-        return;
+        goto fail;
     }
 
     _mag_x_accum += mag_x;
@@ -275,7 +276,10 @@ void AP_Compass_AK8963::_update()
     }
 
     _last_update_timestamp = hal.scheduler->micros();
+fail:
     _sem_give();
+end:
+    return;
 }
 
 bool AP_Compass_AK8963::_check_id()