From c649fd14680fabfb76ad35bd7346b39650086d7d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 16 Mar 2019 18:06:02 +1100 Subject: [PATCH] AP_Mount: support sysid targetting --- libraries/AP_Mount/AP_Mount.cpp | 40 +++++++++++++++++++ libraries/AP_Mount/AP_Mount.h | 11 ++++- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 8 ++++ libraries/AP_Mount/AP_Mount_Backend.cpp | 31 +++++++++++++- libraries/AP_Mount/AP_Mount_Backend.h | 16 +++++++- libraries/AP_Mount/AP_Mount_SToRM32.cpp | 6 +++ .../AP_Mount/AP_Mount_SToRM32_serial.cpp | 8 ++++ libraries/AP_Mount/AP_Mount_Servo.cpp | 9 +++++ libraries/AP_Mount/AP_Mount_SoloGimbal.cpp | 5 +++ 9 files changed, 129 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 53fb5631c2..62d94b0113 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -7,6 +7,7 @@ #include "AP_Mount_Alexmos.h" #include "AP_Mount_SToRM32.h" #include "AP_Mount_SToRM32_serial.h" +#include const AP_Param::GroupInfo AP_Mount::var_info[] = { // @Param: _DEFLT_MODE @@ -596,6 +597,33 @@ MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet) } } +/// Change the configuration of the mount +void AP_Mount::handle_global_position_int(const mavlink_message_t &msg) +{ + mavlink_global_position_int_t packet; + mavlink_msg_global_position_int_decode(&msg, &packet); + + if (!check_latlng(packet.lat, packet.lon)) { + return; + } + + for (uint8_t instance=0; instanceset_target_sysid(sysid); + } +} + // set_roi_target - sets target location that mount should attempt to point towards void AP_Mount::set_roi_target(uint8_t instance, const struct Location &target_loc) { @@ -666,6 +703,9 @@ void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &m case MAVLINK_MSG_ID_MOUNT_CONTROL: handle_mount_control(msg); break; + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + handle_global_position_int(msg); + break; default: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL AP_HAL::panic("Unhandled mount case"); diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 648b2c3582..0fd07d8713 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -111,6 +111,10 @@ public: void set_roi_target(const struct Location &target_loc) { set_roi_target(_primary,target_loc); } void set_roi_target(uint8_t instance, const struct Location &target_loc); + // point at system ID sysid + void set_target_sysid(uint8_t instance, const uint8_t sysid); + void set_target_sysid(const uint8_t sysid) { set_target_sysid(_primary, sysid); } + // mavlink message handling: MAV_RESULT handle_command_long(const mavlink_command_long_t &packet); void handle_param_value(const mavlink_message_t &msg); @@ -168,6 +172,11 @@ 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; + + uint8_t _target_sysid; // sysid to track + Location _target_sysid_location; // sysid target location + bool _target_sysid_location_set; + } state[AP_MOUNT_MAX_INSTANCES]; private: @@ -178,7 +187,7 @@ private: MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet); - + void handle_global_position_int(const mavlink_message_t &msg); }; namespace AP { diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index b23828ee61..ab3af2c903 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -57,6 +57,14 @@ void AP_Mount_Alexmos::update() } break; + case MAV_MOUNT_MODE_SYSID_TARGET: + if (calc_angle_to_sysid_target(_angle_ef_target_rad, + true, + false)) { + control_axis(_angle_ef_target_rad, false); + } + break; + default: // we do not know this mode so do nothing break; diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 8b8daf08c8..8662bc79f8 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -26,6 +26,15 @@ void AP_Mount_Backend::set_roi_target(const struct Location &target_loc) _frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT); } +// set_sys_target - sets system that mount should attempt to point towards +void AP_Mount_Backend::set_target_sysid(uint8_t sysid) +{ + _state._target_sysid = sysid; + + // set the mode to sysid tracking mode + _frontend.set_mode(_instance, MAV_MOUNT_MODE_SYSID_TARGET); +} + // process MOUNT_CONFIGURE messages received from GCS. deprecated. void AP_Mount_Backend::handle_mount_configure(const mavlink_mount_configure_t &packet) { @@ -135,7 +144,7 @@ float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min, bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, - bool relative_pan) + bool relative_pan) const { if (_state._roi_target_set_ms == 0) { return false; @@ -143,8 +152,26 @@ bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad, return calc_angle_to_location(_state._roi_target, angles_to_target_rad, calc_tilt, calc_pan, relative_pan); } +bool AP_Mount_Backend::calc_angle_to_sysid_target(Vector3f& angles_to_target_rad, + bool calc_tilt, + bool calc_pan, + bool relative_pan) const +{ + if (!_state._target_sysid_location_set) { + return false; + } + if (!_state._target_sysid) { + return false; + } + return calc_angle_to_location(_state._target_sysid_location, + 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 -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) +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) const { Location current_loc; if (!AP::ahrs().get_position(current_loc)) { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 2808a550fe..24a26d6e78 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -57,6 +57,9 @@ public: // set_roi_target - sets target location that mount should attempt to point towards void set_roi_target(const struct Location &target_loc); + // set_sys_target - sets system that mount should attempt to point towards + void set_target_sysid(uint8_t sysid); + // 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); @@ -92,14 +95,23 @@ protected: Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, - bool relative_pan = true) WARN_IF_UNUSED; + bool relative_pan = true) const 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; + bool relative_pan = true) const WARN_IF_UNUSED; + + // calc_angle_to_sysid_target - calculates the earth-frame roll, tilt + // and pan angles (and radians) to point at the sysid-target (as set + // by various mavlink messages) + bool calc_angle_to_sysid_target(Vector3f& angles_to_target_rad, + bool calc_tilt, + bool calc_pan, + bool relative_pan = true) const 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 f34bb10c89..75414abac4 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -64,6 +64,12 @@ void AP_Mount_SToRM32::update() } break; + case MAV_MOUNT_MODE_SYSID_TARGET: + if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true)) { + resend_now = true; + } + break; + default: // we do not know this mode so do nothing break; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index f5a845ea58..777b8764e6 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -80,6 +80,14 @@ void AP_Mount_SToRM32_serial::update() } break; + case MAV_MOUNT_MODE_SYSID_TARGET: + if (calc_angle_to_sysid_target(_angle_ef_target_rad, + true, + true)) { + resend_now = true; + } + break; + default: // we do not know this mode so do nothing break; diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 0347c4f703..14542d1c74 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -76,6 +76,15 @@ void AP_Mount_Servo::update() break; } + case MAV_MOUNT_MODE_SYSID_TARGET: + if (calc_angle_to_sysid_target(_angle_ef_target_rad, + _flags.tilt_control, + _flags.pan_control, + false)) { + stabilize(); + } + break; + default: //do nothing break; diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index f2cd7c5e06..f959dda443 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -74,6 +74,11 @@ void AP_Mount_SoloGimbal::update() } break; + case MAV_MOUNT_MODE_SYSID_TARGET: + if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true)) { + } + break; + default: // we do not know this mode so do nothing break;