Use copyTo for writing to memory blocks

EKF/ekf.h

@@ -227,26 +227,21 @@ public:
 	// return the amount the local horizontal position changed in the last reset and the number of reset events
 	void get_posNE_reset(float delta[2], uint8_t *counter)
 	{
-		delta[0] = _state_reset_status.posNE_change(0);
-		delta[1] = _state_reset_status.posNE_change(1);
+		_state_reset_status.posNE_change.copyTo(delta);
 		*counter = _state_reset_status.posNE_counter;
 	}
 
 	// return the amount the local horizontal velocity changed in the last reset and the number of reset events
 	void get_velNE_reset(float delta[2], uint8_t *counter)
 	{
-		delta[0] = _state_reset_status.velNE_change(0);
-		delta[1] = _state_reset_status.velNE_change(1);
+		_state_reset_status.velNE_change.copyTo(delta);
 		*counter = _state_reset_status.velNE_counter;
 	}
 
 	// return the amount the quaternion has changed in the last reset and the number of reset events
 	void get_quat_reset(float delta_quat[4], uint8_t *counter)
 	{
-		for (size_t i = 0; i < 4; i++)
-		{
-			delta_quat[i] = _state_reset_status.quat_change(i);
-		}
+		_state_reset_status.quat_change.copyTo(delta_quat);
 		*counter = _state_reset_status.quat_counter;
 	}
 

EKF/swig/ecl_EKF.i

@@ -256,9 +256,7 @@
        return self->get_state_delayed(out);
    }
    void get_quaternion(float out[4]) {
-       for(unsigned int i = 0; i < 4; i++) {
-           out[i] = self->get_quaternion()(i);
-       }
+       self->get_quaternion().copyTo(out);
    }
 }