From 76e66be9cb8ca733ff4b76cdfee8bed1e740e4c1 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 11 Aug 2015 04:25:20 -0700
Subject: [PATCH] Copter: convert fn from body-frame to NE

---
 ArduCopter/Attitude.cpp | 9 +++++++++
 ArduCopter/Copter.h     | 1 +
 2 files changed, 10 insertions(+)

diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
index c3263a21c3..1235ea4285 100644
--- a/ArduCopter/Attitude.cpp
+++ b/ArduCopter/Attitude.cpp
@@ -308,3 +308,12 @@ void Copter::update_poscon_alt_max()
     // pass limit to pos controller
     pos_control.set_alt_max(alt_limit_cm);
 }
+
+// rotate vector from vehicle's perspective to North-East frame
+void Copter::rotate_body_frame_to_NE(float &x, float &y)
+{
+    float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw();
+    float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
+    x = ne_x;
+    y = ne_y;
+}
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 8f130ed0a9..d19ca04bd3 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -563,6 +563,7 @@ private:
     float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt);
     void set_accel_throttle_I_from_pilot_throttle(int16_t pilot_throttle);
     void update_poscon_alt_max();
+    void rotate_body_frame_to_NE(float &x, float &y);
     void gcs_send_heartbeat(void);
     void gcs_send_deferred(void);
     void send_heartbeat(mavlink_channel_t chan);