Mount_Backend: move calc_angle_to_lcoation to backend

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -0,0 +1,25 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_Mount_Backend.h>
+
+// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
+void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan)
+{
+    float GPS_vector_x = (target.lng-_frontend._current_loc.lng)*cosf(ToRad((_frontend._current_loc.lat+target.lat)*0.00000005f))*0.01113195f;
+    float GPS_vector_y = (target.lat-_frontend._current_loc.lat)*0.01113195f;
+    float GPS_vector_z = (target.alt-_frontend._current_loc.alt);                 // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
+    float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y);      // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
+
+    // initialise all angles to zero
+    angles_to_target_rad.zero();
+
+    // tilt calcs
+    if (calc_tilt) {
+        angles_to_target_rad.y = atan2f(GPS_vector_z, target_distance);
+    }
+
+    // pan calcs
+    if (calc_pan) {
+        angles_to_target_rad.z = atan2f(GPS_vector_x, GPS_vector_y);
+    }
+}

libraries/AP_Mount/AP_Mount_Backend.h

@@ -62,6 +62,10 @@ public:
     virtual void status_msg(mavlink_channel_t chan) {};
 
 protected:
+
+    // calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
+    void calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan);
+
     AP_Mount    &_frontend; // reference to the front end which holds parameters
     uint8_t     _instance;  // this instance's number
 };

libraries/AP_Mount/AP_Mount_Servo.cpp

@@ -101,7 +101,7 @@ void AP_Mount_Servo::update()
         case MAV_MOUNT_MODE_GPS_POINT:
         {
             if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
-                calc_angle_to_location(_frontend.state[_instance]._roi_target, _angle_ef_target_rad);
+                calc_angle_to_location(_frontend.state[_instance]._roi_target, _angle_ef_target_rad, _flags.pan_control, _flags.tilt_control);
                 stabilize();
             }
             break;
@@ -153,28 +153,6 @@ void AP_Mount_Servo::check_servo_map()
     _flags.pan_control = RC_Channel_aux::function_assigned(_pan_idx);
 }
 
-// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
-void AP_Mount_Servo::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad)
-{
-    float GPS_vector_x = (target.lng-_frontend._current_loc.lng)*cosf(ToRad((_frontend._current_loc.lat+target.lat)*0.00000005f))*0.01113195f;
-    float GPS_vector_y = (target.lat-_frontend._current_loc.lat)*0.01113195f;
-    float GPS_vector_z = (target.alt-_frontend._current_loc.alt);                 // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
-    float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y);      // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
-
-    // initialise all angles to zero
-    angles_to_target_rad.zero();
-
-    // tilt calcs
-    if (_flags.tilt_control) {
-        angles_to_target_rad.y = atan2f(GPS_vector_z, target_distance);
-    }
-
-    // pan calcs
-    if (_flags.pan_control) {
-        angles_to_target_rad.z = atan2f(GPS_vector_x, GPS_vector_y);
-    }
-}
-
 // configure_msg - process MOUNT_CONFIGURE messages received from GCS
 void AP_Mount_Servo::configure_msg(mavlink_message_t* msg)
 {

libraries/AP_Mount/AP_Mount_Servo.h

@@ -70,9 +70,6 @@ private:
     //  should be called periodically (i.e. 1hz or less)
     void    check_servo_map();
 
-    // calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
-    void calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad);
-
     // stabilize - stabilizes the mount relative to the Earth's frame
     void stabilize();