From 7139c1121f10c9d96778486c53259ec209fa46d7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 4 Nov 2019 13:09:30 +1100 Subject: [PATCH] AP_Mount: correct use of disparate altitude frames in AP_Mount --- libraries/AP_Mount/AP_Mount.h | 1 + libraries/AP_Mount/AP_Mount_Alexmos.cpp | 3 +-- libraries/AP_Mount/AP_Mount_Backend.cpp | 27 ++++++++++++++++--- libraries/AP_Mount/AP_Mount_Backend.h | 20 +++++++++++--- libraries/AP_Mount/AP_Mount_SToRM32.cpp | 3 +-- .../AP_Mount/AP_Mount_SToRM32_serial.cpp | 3 +-- libraries/AP_Mount/AP_Mount_Servo.cpp | 3 +-- libraries/AP_Mount/AP_Mount_SoloGimbal.cpp | 3 +-- 8 files changed, 46 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index b0e2def173..648b2c3582 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/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: diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index e47b3590c0..b23828ee61 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/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) { - calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, false); + if (calc_angle_to_roi_target(_angle_ef_target_rad, true, false)) { control_axis(_angle_ef_target_rad, false); } break; diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 6afd742cd6..8b8daf08c8 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/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; } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 217949e01a..2808a550fe 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/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 - void calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan = true); + // calc_angle_to_location - calculates the earth-frame roll, tilt + // 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); } diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index d1e7649e51..f34bb10c89 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/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) { - calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true); + if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) { resend_now = true; } break; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 3d0871327b..f5a845ea58 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/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) { - calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true); + if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) { resend_now = true; } break; diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 34824a169d..0347c4f703 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/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) { - calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, false); + if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, false)) { stabilize(); } break; diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 54a2b4b5fe..f2cd7c5e06 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/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) { - calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true); + if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) { } break;