AP_Mount: correct use of disparate altitude frames in AP_Mount

libraries/AP_Mount/AP_Mount.h

@@ -167,6 +167,7 @@ protected:
 
         MAV_MOUNT_MODE  _mode;              // current mode (see MAV_MOUNT_MODE enum)
         struct Location _roi_target;        // roi target location
+        uint32_t _roi_target_set_ms;
     } state[AP_MOUNT_MAX_INSTANCES];
 
 private:

libraries/AP_Mount/AP_Mount_Alexmos.cpp

@@ -52,8 +52,7 @@ void AP_Mount_Alexmos::update()
 
         // point mount to a GPS point given by the mission planner
         case MAV_MOUNT_MODE_GPS_POINT:
-            if(AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
+            if (calc_angle_to_roi_target(_angle_ef_target_rad, true, false)) {
-                calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, false);
                 control_axis(_angle_ef_target_rad, false);
             }
             break;

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -20,6 +20,7 @@ void AP_Mount_Backend::set_roi_target(const struct Location &target_loc)
 {
     // set the target gps location
     _state._roi_target = target_loc;
+    _state._roi_target_set_ms = AP_HAL::millis();
 
     // set the mode to GPS tracking mode
     _frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT);
@@ -131,16 +132,35 @@ float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min,
     return radians(((rc->norm_input() + 1.0f) * 0.5f * (angle_max - angle_min) + angle_min)*0.01f);
 }
 
+bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
+                                                bool calc_tilt,
+                                                bool calc_pan,
+                                                bool relative_pan)
+{
+    if (_state._roi_target_set_ms == 0) {
+        return false;
+    }
+    return calc_angle_to_location(_state._roi_target, angles_to_target_rad, calc_tilt, calc_pan, relative_pan);
+}
+
 // 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, bool relative_pan)
+bool AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan)
 {
     Location current_loc;
     if (!AP::ahrs().get_position(current_loc)) {
-        // completely ignore this error; ahrs will give us its best guess
+        return false;
     }
     const float GPS_vector_x = (target.lng-current_loc.lng)*cosf(ToRad((current_loc.lat+target.lat)*0.00000005f))*0.01113195f;
     const float GPS_vector_y = (target.lat-current_loc.lat)*0.01113195f;
-    const float GPS_vector_z = (target.alt-current_loc.alt);                 // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
+    int32_t target_alt_cm = 0;
+    if (!target.get_alt_cm(Location::AltFrame::ABOVE_HOME, target_alt_cm)) {
+        return false;
+    }
+    int32_t current_alt_cm = 0;
+    if (!current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, current_alt_cm)) {
+        return false;
+    }
+    float GPS_vector_z = target_alt_cm - current_alt_cm;
     float target_distance = 100.0f*norm(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
@@ -159,4 +179,5 @@ void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vec
             angles_to_target_rad.z = wrap_PI(angles_to_target_rad.z - AP::ahrs().yaw);
         }
     }
+    return true;
 }

libraries/AP_Mount/AP_Mount_Backend.h

@@ -52,10 +52,10 @@ public:
     virtual void set_mode(enum MAV_MOUNT_MODE mode) = 0;
 
     // set_angle_targets - sets angle targets in degrees
-    virtual void set_angle_targets(float roll, float tilt, float pan);
+    void set_angle_targets(float roll, float tilt, float pan);
 
     // set_roi_target - sets target location that mount should attempt to point towards
-    virtual void set_roi_target(const struct Location &target_loc);
+    void set_roi_target(const struct Location &target_loc);
 
     // control - control the mount
     virtual void control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, MAV_MOUNT_MODE mount_mode);
@@ -86,8 +86,20 @@ protected:
     // angle_input_rad - convert RC input into an earth-frame target angle
     float angle_input_rad(const RC_Channel* rc, int16_t angle_min, int16_t angle_max);
 
-    // calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
+    // calc_angle_to_location - calculates the earth-frame roll, tilt
-    void calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan = true);
+    // and pan angles (and radians) to point at the given target
+    bool calc_angle_to_location(const struct Location &target,
+                                Vector3f& angles_to_target_rad,
+                                bool calc_tilt,
+                                bool calc_pan,
+                                bool relative_pan = true) WARN_IF_UNUSED;
+    // calc_angle_to_roi_target - calculates the earth-frame roll, tilt
+    // and pan angles (and radians) to point at the ROI-target (as set
+    // by various mavlink messages)
+    bool calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
+                                  bool calc_tilt,
+                                  bool calc_pan,
+                                  bool relative_pan = true) WARN_IF_UNUSED;
 
     // get the mount mode from frontend
     MAV_MOUNT_MODE get_mode(void) const { return _frontend.get_mode(_instance); }

libraries/AP_Mount/AP_Mount_SToRM32.cpp

@@ -59,8 +59,7 @@ void AP_Mount_SToRM32::update()
 
         // point mount to a GPS point given by the mission planner
         case MAV_MOUNT_MODE_GPS_POINT:
-            if(AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
+            if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) {
-                calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
                 resend_now = true;
             }
             break;

libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp

@@ -75,8 +75,7 @@ void AP_Mount_SToRM32_serial::update()
 
         // point mount to a GPS point given by the mission planner
         case MAV_MOUNT_MODE_GPS_POINT:
-            if(AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
+            if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) {
-                calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
                 resend_now = true;
             }
             break;

libraries/AP_Mount/AP_Mount_Servo.cpp

@@ -70,8 +70,7 @@ void AP_Mount_Servo::update()
         // point mount to a GPS point given by the mission planner
         case MAV_MOUNT_MODE_GPS_POINT:
         {
-            if(AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
+            if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, false)) {
-                calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, false);
                 stabilize();
             }
             break;

libraries/AP_Mount/AP_Mount_SoloGimbal.cpp

@@ -70,8 +70,7 @@ void AP_Mount_SoloGimbal::update()
         // point mount to a GPS point given by the mission planner
         case MAV_MOUNT_MODE_GPS_POINT:
             _gimbal.set_lockedToBody(false);
-            if(AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
+            if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) {
-                calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
             }
             break;