From 4f60ae4bf178ad1e6fb7441937785836222c22ec Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Fri, 21 Jul 2023 12:05:42 +0530 Subject: [PATCH 01/12] AP_Camera: add MNT log msg for mount's actual and target angles logging --- libraries/AP_Camera/AP_Camera_Logging.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera_Logging.cpp b/libraries/AP_Camera/AP_Camera_Logging.cpp index f6a5e26541fe4..bad765067e36f 100644 --- a/libraries/AP_Camera/AP_Camera_Logging.cpp +++ b/libraries/AP_Camera/AP_Camera_Logging.cpp @@ -1,11 +1,12 @@ #include "AP_Camera_Backend.h" +#include #if AP_CAMERA_ENABLED #include #include -// Write a Camera packet +// Write a Camera packet. Also writes a Mount packet if available void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us) { // exit immediately if no logger @@ -41,9 +42,14 @@ void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestam altitude_gps = 0; } + // if timestamp is zero set to current system time + if (timestamp_us == 0) { + timestamp_us = AP_HAL::micros64(); + } + const struct log_Camera pkt{ LOG_PACKET_HEADER_INIT(static_cast(msg)), - time_us : timestamp_us ? timestamp_us : AP_HAL::micros64(), + time_us : timestamp_us, instance : _instance, image_number: image_index, gps_time : gps.time_week_ms(), @@ -58,6 +64,14 @@ void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestam yaw : (uint16_t)ahrs.yaw_sensor }; AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt)); + +#if HAL_MOUNT_ENABLED + auto *mount = AP_Mount::get_singleton(); + if (mount!= nullptr) { + // assuming camera instance matches mount instance + mount->write_log(_instance, timestamp_us); + } +#endif } // Write a Camera packet From d8e2209c79c44f05e386411e88adc9eddda0c17b Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Fri, 21 Jul 2023 12:05:42 +0530 Subject: [PATCH 02/12] AP_Logger: add MNT log msg for mount's actual and target angles logging --- libraries/AP_Logger/LogStructure.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 9ec9e5c15aa1f..7254072b7ff94 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -131,6 +131,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include #include #include #include @@ -1260,6 +1261,7 @@ LOG_STRUCTURE_FROM_PRECLAND \ { LOG_RADIO_MSG, sizeof(log_Radio), \ "RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed", "s-------", "F-------", true }, \ LOG_STRUCTURE_FROM_CAMERA \ +LOG_STRUCTURE_FROM_MOUNT \ { LOG_ARSP_MSG, sizeof(log_ARSP), "ARSP", "QBffcffBBffB", "TimeUS,I,Airspeed,DiffPress,Temp,RawPress,Offset,U,H,Hp,TR,Pri", "s#nPOPP-----", "F-00B00-----", true }, \ LOG_STRUCTURE_FROM_BATTMONITOR \ { LOG_MAG_MSG, sizeof(log_MAG), \ @@ -1370,6 +1372,7 @@ enum LogMessages : uint8_t { LOG_RADIO_MSG, LOG_ATRP_MSG, LOG_IDS_FROM_CAMERA, + LOG_IDS_FROM_MOUNT, LOG_TERRAIN_MSG, LOG_CSRV_MSG, LOG_IDS_FROM_ESC_TELEM, From 01ae266b33eb26ff91a550c7dd329565ee492a43 Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Fri, 21 Jul 2023 12:05:42 +0530 Subject: [PATCH 03/12] AP_Mount: add MNT log msg for mount's actual and target angles logging --- libraries/AP_Mount/AP_Mount.cpp | 24 ++- libraries/AP_Mount/AP_Mount.h | 12 +- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 61 ++++--- libraries/AP_Mount/AP_Mount_Alexmos.h | 2 - libraries/AP_Mount/AP_Mount_Backend.cpp | 168 ++++++++++++------ libraries/AP_Mount/AP_Mount_Backend.h | 33 ++-- libraries/AP_Mount/AP_Mount_Gremsy.cpp | 73 ++++---- libraries/AP_Mount/AP_Mount_SToRM32.cpp | 53 +++--- libraries/AP_Mount/AP_Mount_SToRM32.h | 1 - .../AP_Mount/AP_Mount_SToRM32_serial.cpp | 49 ++--- libraries/AP_Mount/AP_Mount_SToRM32_serial.h | 1 - libraries/AP_Mount/AP_Mount_Scripting.cpp | 126 +++---------- libraries/AP_Mount/AP_Mount_Scripting.h | 9 +- libraries/AP_Mount/AP_Mount_Servo.cpp | 86 +++++---- libraries/AP_Mount/AP_Mount_Servo.h | 1 - libraries/AP_Mount/AP_Mount_Siyi.cpp | 74 ++++---- libraries/AP_Mount/AP_Mount_SoloGimbal.cpp | 59 +++--- libraries/AP_Mount/AP_Mount_SoloGimbal.h | 1 - libraries/AP_Mount/AP_Mount_Viewpro.cpp | 67 +++---- libraries/AP_Mount/AP_Mount_Xacti.cpp | 71 ++++---- libraries/AP_Mount/LogStructure.h | 38 ++++ 21 files changed, 549 insertions(+), 460 deletions(-) create mode 100644 libraries/AP_Mount/LogStructure.h diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 3e1ac030fc96f..5b7cdf6c3edfb 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -646,7 +646,7 @@ bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) return true; } -// accessors for scripting backends +// get target rate in deg/sec. returns true on success bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { auto *backend = get_instance(instance); @@ -656,6 +656,7 @@ bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_ return backend->get_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_is_earth_frame); } +// get target angle in deg. returns true on success bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) { auto *backend = get_instance(instance); @@ -665,6 +666,7 @@ bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_ return backend->get_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); } +// accessors for scripting backends and logging bool AP_Mount::get_location_target(uint8_t instance, Location& target_loc) { auto *backend = get_instance(instance); @@ -683,6 +685,26 @@ void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_ backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg); } +// write mount log packet for all backends +void AP_Mount::write_log() +{ + // each instance writes log + for (uint8_t instance=0; instancewrite_log(0); + } + } +} + +void AP_Mount::write_log(uint8_t instance, uint64_t timestamp_us) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + backend->write_log(timestamp_us); +} + // point at system ID sysid void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid) { diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index b4187eabe46de..6067f7073258e 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -199,12 +199,22 @@ class AP_Mount // any failure_msg returned will not include a prefix bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len); - // accessors for scripting backends + // get target rate in deg/sec. returns true on success bool get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame); + + // get target angle in deg. returns true on success bool get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame); + + // accessors for scripting backends and logging bool get_location_target(uint8_t instance, Location& target_loc); void set_attitude_euler(uint8_t instance, float roll_deg, float pitch_deg, float yaw_bf_deg); + // write mount log packet for all backends + void write_log(); + + // write mount log packet for a single backend (called by camera library) + void write_log(uint8_t instance, uint64_t timestamp_us); + // // camera controls for gimbals that include a camera // diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 1b831edc01c3d..f7f39f6119be4 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -30,58 +30,59 @@ void AP_Mount_Alexmos::update() // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism case MAV_MOUNT_MODE_RETRACT: { const Vector3f &target = _params.retract_angles.get(); - _angle_rad.roll = radians(target.x); - _angle_rad.pitch = radians(target.y); - _angle_rad.yaw = radians(target.z); - _angle_rad.yaw_is_ef = false; + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(target*DEG_TO_RAD, false); break; } // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { const Vector3f &target = _params.neutral_angles.get(); - _angle_rad.roll = radians(target.x); - _angle_rad.pitch = radians(target.y); - _angle_rad.yaw = radians(target.z); - _angle_rad.yaw_is_ef = false; + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(target*DEG_TO_RAD, false); break; } // point to the angles given by a mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: - switch (mavt_target.target_type) { - case MountTargetType::ANGLE: - _angle_rad = mavt_target.angle_rad; - break; - case MountTargetType::RATE: - update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad); - break; - } + // mavlink targets are stored while handling the incoming message break; // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { // update targets using pilot's RC inputs - MountTarget rc_target {}; - if (get_rc_rate_target(rc_target)) { - update_angle_target_from_rate(rc_target, _angle_rad); - } else if (get_rc_angle_target(rc_target)) { - _angle_rad = rc_target; + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; } break; } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: - IGNORE_RETURN(get_angle_target_to_roi(_angle_rad)); + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; + // point mount to Home location case MAV_MOUNT_MODE_HOME_LOCATION: - IGNORE_RETURN(get_angle_target_to_home(_angle_rad)); + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; + // point mount to another vehicle case MAV_MOUNT_MODE_SYSID_TARGET: - IGNORE_RETURN(get_angle_target_to_sysid(_angle_rad)); + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; default: @@ -89,8 +90,16 @@ void AP_Mount_Alexmos::update() break; } - // send latest targets to gimbal - control_axis(_angle_rad); + // send target angles or rates depending on the target type + switch (mnt_target.target_type) { + case MountTargetType::RATE: + update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); + FALLTHROUGH; + case MountTargetType::ANGLE: + // send latest angle targets to gimbal + control_axis(mnt_target.angle_rad); + break; + } } // has_pan_control - returns true if this mount can control its pan (required for multicopters) diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.h b/libraries/AP_Mount/AP_Mount_Alexmos.h index 1547d4a2fa884..6485073dd4f5c 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.h +++ b/libraries/AP_Mount/AP_Mount_Alexmos.h @@ -112,8 +112,6 @@ class AP_Mount_Alexmos : public AP_Mount_Backend // read_incoming - detect and read the header of the incoming message from the gimbal void read_incoming(); - MountTarget _angle_rad; // latest angle target - // structure for the Serial Protocol // CMD_BOARD_INFO diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index d4bc9b3a15085..ac4d2878eb7e9 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -2,6 +2,7 @@ #if HAL_MOUNT_ENABLED #include #include +#include extern const AP_HAL::HAL& hal; @@ -49,11 +50,11 @@ void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float y } // set angle targets - mavt_target.target_type = MountTargetType::ANGLE; - mavt_target.angle_rad.roll = radians(roll_deg); - mavt_target.angle_rad.pitch = radians(pitch_deg); - mavt_target.angle_rad.yaw = radians(yaw_deg); - mavt_target.angle_rad.yaw_is_ef = yaw_is_earth_frame; + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.roll = radians(roll_deg); + mnt_target.angle_rad.pitch = radians(pitch_deg); + mnt_target.angle_rad.yaw = radians(yaw_deg); + mnt_target.angle_rad.yaw_is_ef = yaw_is_earth_frame; // set the mode to mavlink targeting set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING); @@ -64,11 +65,11 @@ void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float y void AP_Mount_Backend::set_rate_target(float roll_degs, float pitch_degs, float yaw_degs, bool yaw_is_earth_frame) { // set rate targets - mavt_target.target_type = MountTargetType::RATE; - mavt_target.rate_rads.roll = radians(roll_degs); - mavt_target.rate_rads.pitch = radians(pitch_degs); - mavt_target.rate_rads.yaw = radians(yaw_degs); - mavt_target.rate_rads.yaw_is_ef = yaw_is_earth_frame; + mnt_target.target_type = MountTargetType::RATE; + mnt_target.rate_rads.roll = radians(roll_degs); + mnt_target.rate_rads.pitch = radians(pitch_degs); + mnt_target.rate_rads.yaw = radians(yaw_degs); + mnt_target.rate_rads.yaw_is_ef = yaw_is_earth_frame; // set the mode to mavlink targeting set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING); @@ -356,6 +357,49 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli return true; } +// write mount log packet +void AP_Mount_Backend::write_log(uint64_t timestamp_us) +{ + // return immediately if no yaw estimate + float ahrs_yaw = AP::ahrs().yaw; + if (isnan(ahrs_yaw)) { + return; + } + + const auto nanf = AP::logger().quiet_nanf(); + + // get_attitude_quaternion and convert to Euler angles + float roll = nanf; + float pitch = nanf; + float yaw_bf = nanf; + float yaw_ef = nanf; + if (_frontend.get_attitude_euler(_instance, roll, pitch, yaw_bf)) { + yaw_ef = wrap_180(yaw_bf + degrees(ahrs_yaw)); + } + + // get mount's target (desired) angles and convert yaw to earth frame + float target_roll = nanf; + float target_pitch = nanf; + float target_yaw = nanf; + bool target_yaw_is_ef = false; + IGNORE_RETURN(get_angle_target(target_roll, target_pitch, target_yaw, target_yaw_is_ef)); + + const struct log_Mount pkt { + LOG_PACKET_HEADER_INIT(static_cast(LOG_MOUNT_MSG)), + time_us : (timestamp_us > 0) ? timestamp_us : AP_HAL::micros64(), + instance : _instance, + desired_roll : target_roll, + actual_roll : roll, + desired_pitch : target_pitch, + actual_pitch : pitch, + desired_yaw_bf: target_yaw_is_ef ? nanf : target_yaw, + actual_yaw_bf : yaw_bf, + desired_yaw_ef: target_yaw_is_ef ? target_yaw : nanf, + actual_yaw_ef : yaw_ef, + }; + AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt)); +} + // get pilot input (in the range -1 to +1) received through RC void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const { @@ -379,61 +423,44 @@ void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_ } } -// get rate targets (in rad/s) from pilot RC -// returns true on success (RC is providing rate targets), false on failure (RC is providing angle targets) -bool AP_Mount_Backend::get_rc_rate_target(MountTarget& rate_rads) const +// get angle or rate targets from pilot RC +// target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s +void AP_Mount_Backend::get_rc_target(MountTargetType& target_type, MountTarget& target_rpy) const { - // exit immediately if RC is not providing rate targets - if (_params.rc_rate_max <= 0) { - return false; - } - // get RC input from pilot float roll_in, pitch_in, yaw_in; get_rc_input(roll_in, pitch_in, yaw_in); - // calculate rates - const float rc_rate_max_rads = radians(_params.rc_rate_max.get()); - rate_rads.roll = roll_in * rc_rate_max_rads; - rate_rads.pitch = pitch_in * rc_rate_max_rads; - rate_rads.yaw = yaw_in * rc_rate_max_rads; - // yaw frame - rate_rads.yaw_is_ef = _yaw_lock; - - return true; -} + target_rpy.yaw_is_ef = _yaw_lock; -// get angle targets (in radians) from pilot RC -// returns true on success (RC is providing angle targets), false on failure (RC is providing rate targets) -bool AP_Mount_Backend::get_rc_angle_target(MountTarget& angle_rad) const -{ - // exit immediately if RC is not providing angle targets - if (_params.rc_rate_max > 0) { - return false; - } - - // get RC input from pilot - float roll_in, pitch_in, yaw_in; - get_rc_input(roll_in, pitch_in, yaw_in); + // if RC_RATE is zero, targets are angle + if (_params.rc_rate_max <= 0) { + target_type = MountTargetType::ANGLE; - // roll angle - angle_rad.roll = radians(((roll_in + 1.0f) * 0.5f * (_params.roll_angle_max - _params.roll_angle_min) + _params.roll_angle_min)); + // roll angle + target_rpy.roll = radians(((roll_in + 1.0f) * 0.5f * (_params.roll_angle_max - _params.roll_angle_min) + _params.roll_angle_min)); - // pitch angle - angle_rad.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_params.pitch_angle_max - _params.pitch_angle_min) + _params.pitch_angle_min)); + // pitch angle + target_rpy.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_params.pitch_angle_max - _params.pitch_angle_min) + _params.pitch_angle_min)); - // yaw angle - angle_rad.yaw_is_ef = _yaw_lock; - if (angle_rad.yaw_is_ef) { - // if yaw is earth-frame pilot yaw input control angle from -180 to +180 deg - angle_rad.yaw = yaw_in * M_PI; - } else { - // yaw target in body frame so apply body frame limits - angle_rad.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min)); + // yaw angle + if (target_rpy.yaw_is_ef) { + // if yaw is earth-frame pilot yaw input control angle from -180 to +180 deg + target_rpy.yaw = yaw_in * M_PI; + } else { + // yaw target in body frame so apply body frame limits + target_rpy.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min)); + } + return; } - return true; + // calculate rate targets + target_type = MountTargetType::RATE; + const float rc_rate_max_rads = radians(_params.rc_rate_max.get()); + target_rpy.roll = roll_in * rc_rate_max_rads; + target_rpy.pitch = pitch_in * rc_rate_max_rads; + target_rpy.yaw = yaw_in * rc_rate_max_rads; } // get angle targets (in radians) to a Location @@ -507,6 +534,15 @@ float AP_Mount_Backend::MountTarget::get_ef_yaw() const return wrap_PI(yaw + AP::ahrs().yaw); } +// sets roll, pitch, yaw and yaw_is_ef +void AP_Mount_Backend::MountTarget::set(const Vector3f& rpy, bool yaw_is_ef_in) +{ + roll = rpy.x; + pitch = rpy.y; + yaw = rpy.z; + yaw_is_ef = yaw_is_ef_in; +} + // update angle targets using a given rate target // the resulting angle_rad yaw frame will match the rate_rad yaw frame // assumes a 50hz update rate @@ -572,6 +608,32 @@ bool AP_Mount_Backend::get_angle_target_to_sysid(MountTarget& angle_rad) const return get_angle_target_to_location(_target_sysid_location, angle_rad); } +// get target rate in deg/sec. returns true on success +bool AP_Mount_Backend::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) +{ + if (mnt_target.target_type == MountTargetType::RATE) { + roll_degs = degrees(mnt_target.rate_rads.roll); + pitch_degs = degrees(mnt_target.rate_rads.pitch); + yaw_degs = degrees(mnt_target.rate_rads.yaw); + yaw_is_earth_frame = mnt_target.rate_rads.yaw_is_ef; + return true; + } + return false; +} + +// get target angle in deg. returns true on success +bool AP_Mount_Backend::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) +{ + if (mnt_target.target_type == MountTargetType::ANGLE) { + roll_deg = degrees(mnt_target.angle_rad.roll); + pitch_deg = degrees(mnt_target.angle_rad.pitch); + yaw_deg = degrees(mnt_target.angle_rad.yaw); + yaw_is_earth_frame = mnt_target.angle_rad.yaw_is_ef; + return true; + } + return false; +} + // sent warning to GCS. Warnings are throttled to at most once every 30 seconds void AP_Mount_Backend::send_warning_to_GCS(const char* warning_str) { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 827c7db133f4c..c663d0dadd781 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -28,7 +28,7 @@ #include #include #include -#include "AP_Mount_Params.h" +#include "AP_Mount.h" class AP_Mount_Backend { @@ -133,12 +133,19 @@ class AP_Mount_Backend // handle GIMBAL_DEVICE_ATTITUDE_STATUS message virtual void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) {} + // get target rate in deg/sec. returns true on success + bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame); + + // get target angle in deg. returns true on success + bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame); + // accessors for scripting backends - virtual bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { return false; } - virtual bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) { return false; } virtual bool get_location_target(Location &target_loc) { return false; } virtual void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) {}; + // write mount log packet + void write_log(uint64_t timestamp_us); + // // camera controls for gimbals that include a camera // @@ -175,8 +182,9 @@ class AP_Mount_Backend RATE, }; - // structure for a single angle or rate target - struct MountTarget { + // class for a single angle or rate target + class MountTarget { + public: float roll; float pitch; float yaw; @@ -187,6 +195,9 @@ class AP_Mount_Backend // return earth-frame yaw angle from a mount target (in radians) float get_ef_yaw() const; + + // set roll, pitch, yaw and yaw_is_ef from Vector3f + void set(const Vector3f& rpy, bool yaw_is_ef_in); }; // returns true if user has configured a valid yaw angle range @@ -199,13 +210,9 @@ class AP_Mount_Backend // get pilot input (in the range -1 to +1) received through RC void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const; - // get rate targets (in rad/s) from pilot RC - // returns true on success (RC is providing rate targets), false on failure (RC is providing angle targets) - bool get_rc_rate_target(MountTarget& rate_rads) const WARN_IF_UNUSED; - - // get angle targets (in radians) from pilot RC - // returns true on success (RC is providing angle targets), false on failure (RC is providing rate targets) - bool get_rc_angle_target(MountTarget& angle_rad) const WARN_IF_UNUSED; + // get angle or rate targets from pilot RC + // target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s + void get_rc_target(MountTargetType& target_type, MountTarget& rpy) const; // get angle targets (in radians) to a Location // returns true on success, false on failure @@ -246,7 +253,7 @@ class AP_Mount_Backend MountTargetType target_type;// MAVLink targeting mode's current target type (e.g. angle or rate) MountTarget angle_rad; // angle target in radians MountTarget rate_rads; // rate target in rad/s - } mavt_target; + } mnt_target; Location _roi_target; // roi target location bool _roi_target_set; // true if the roi target has been set diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index 43e2bbb381755..5c96daa7f17cf 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -26,70 +26,75 @@ void AP_Mount_Gremsy::update() // move mount to a "retracted" position. We disable motors case MAV_MOUNT_MODE_RETRACT: // handled below + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(Vector3f{0,0,0}, false); send_gimbal_device_retract(); break; // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { const Vector3f &angle_bf_target = _params.neutral_angles.get(); - send_gimbal_device_set_attitude(ToRad(angle_bf_target.x), ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false); - } + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); break; + } - // use angle or rate targets provided by a mavlink message or mission command - case MAV_MOUNT_MODE_MAVLINK_TARGETING: - switch (mavt_target.target_type) { - case MountTargetType::ANGLE: - send_gimbal_device_set_attitude(mavt_target.angle_rad.roll, mavt_target.angle_rad.pitch, mavt_target.angle_rad.yaw, mavt_target.angle_rad.yaw_is_ef); - break; - case MountTargetType::RATE: - send_gimbal_device_set_rate(mavt_target.rate_rads.roll, mavt_target.rate_rads.pitch, mavt_target.rate_rads.yaw, mavt_target.rate_rads.yaw_is_ef); - break; - } + case MAV_MOUNT_MODE_MAVLINK_TARGETING: { + // mavlink targets are stored while handling the incoming message set_angle_target() or set_rate_target() break; + } // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's rc inputs - MountTarget rc_target {}; - if (get_rc_rate_target(rc_target)) { - send_gimbal_device_set_rate(rc_target.roll, rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef); - } else if (get_rc_angle_target(rc_target)) { - send_gimbal_device_set_attitude(rc_target.roll, rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef); + // update targets using pilot's RC inputs + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; } break; } // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: { - MountTarget angle_target_rad {}; - if (get_angle_target_to_roi(angle_target_rad)) { - send_gimbal_device_set_attitude(angle_target_rad.roll, angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - // point mount to home - case MAV_MOUNT_MODE_HOME_LOCATION: { - MountTarget angle_target_rad {}; - if (get_angle_target_to_home(angle_target_rad)) { - send_gimbal_device_set_attitude(angle_target_rad.roll, angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + // point mount to Home location + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - case MAV_MOUNT_MODE_SYSID_TARGET: { - MountTarget angle_target_rad {}; - if (get_angle_target_to_sysid(angle_target_rad)) { - send_gimbal_device_set_attitude(angle_target_rad.roll, angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + // point mount to another vehicle + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } default: // unknown mode so do nothing break; } + + // send target angles or rates depending on the target type + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + send_gimbal_device_set_attitude(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); + break; + case MountTargetType::RATE: + send_gimbal_device_set_rate(mnt_target.rate_rads.roll, mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef); + break; + } } // return true if healthy diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index bdea46bf1491c..1c1e5da3eadc1 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -26,32 +26,24 @@ void AP_Mount_SToRM32::update() // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? case MAV_MOUNT_MODE_RETRACT: { const Vector3f &target = _params.retract_angles.get(); - _angle_rad.roll = radians(target.x); - _angle_rad.pitch = radians(target.y); - _angle_rad.yaw = radians(target.z); - _angle_rad.yaw_is_ef = false; + mnt_target.angle_rad.set(target*DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::ANGLE; break; } // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { const Vector3f &target = _params.neutral_angles.get(); - _angle_rad.roll = radians(target.x); - _angle_rad.pitch = radians(target.y); - _angle_rad.yaw = radians(target.z); - _angle_rad.yaw_is_ef = false; + mnt_target.angle_rad.set(target*DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::ANGLE; break; } // point to the angles given by a mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: - switch (mavt_target.target_type) { - case MountTargetType::ANGLE: - _angle_rad = mavt_target.angle_rad; - break; - case MountTargetType::RATE: - update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad); - break; + // mnt_target should have already been populated by set_angle_target() or set_rate_target(). Update target angle from rate if necessary + if (mnt_target.target_type == MountTargetType::RATE) { + update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); } resend_now = true; break; @@ -59,31 +51,40 @@ void AP_Mount_SToRM32::update() // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { // update targets using pilot's RC inputs - MountTarget rc_target {}; - if (get_rc_rate_target(rc_target)) { - update_angle_target_from_rate(rc_target, _angle_rad); - } else if (get_rc_angle_target(rc_target)) { - _angle_rad = rc_target; + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; } resend_now = true; break; } - // point mount to a GPS location + // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: - if (get_angle_target_to_roi(_angle_rad)) { + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; resend_now = true; } break; + // point mount to Home location case MAV_MOUNT_MODE_HOME_LOCATION: - if (get_angle_target_to_home(_angle_rad)) { + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; resend_now = true; } break; + // point mount to another vehicle case MAV_MOUNT_MODE_SYSID_TARGET: - if (get_angle_target_to_sysid(_angle_rad)) { + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; resend_now = true; } break; @@ -95,14 +96,14 @@ void AP_Mount_SToRM32::update() // resend target angles at least once per second if (resend_now || ((AP_HAL::millis() - _last_send) > AP_MOUNT_STORM32_RESEND_MS)) { - send_do_mount_control(_angle_rad); + send_do_mount_control(mnt_target.angle_rad); } } // get attitude as a quaternion. returns true on success bool AP_Mount_SToRM32::get_attitude_quaternion(Quaternion& att_quat) { - att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, _angle_rad.get_bf_yaw()); + att_quat.from_euler(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw()); return true; } diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.h b/libraries/AP_Mount/AP_Mount_SToRM32.h index d850869f9fd15..33aef375c410f 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32.h @@ -42,6 +42,5 @@ class AP_Mount_SToRM32 : public AP_Mount_Backend uint8_t _compid; // component id of gimbal mavlink_channel_t _chan = MAVLINK_COMM_0; // mavlink channel used to communicate with gimbal uint32_t _last_send; // system time of last do_mount_control sent to gimbal - MountTarget _angle_rad; // latest angle target }; #endif // HAL_MOUNT_STORM32MAVLINK_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 9f94ab0a7078e..0fdcdd53c70fb 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -37,32 +37,24 @@ void AP_Mount_SToRM32_serial::update() // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? case MAV_MOUNT_MODE_RETRACT: { const Vector3f &target = _params.retract_angles.get(); - _angle_rad.roll = ToRad(target.x); - _angle_rad.pitch = ToRad(target.y); - _angle_rad.yaw = ToRad(target.z); - _angle_rad.yaw_is_ef = false; + mnt_target.angle_rad.set(target*DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::ANGLE; break; } // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { const Vector3f &target = _params.neutral_angles.get(); - _angle_rad.roll = ToRad(target.x); - _angle_rad.pitch = ToRad(target.y); - _angle_rad.yaw = ToRad(target.z); - _angle_rad.yaw_is_ef = false; + mnt_target.angle_rad.set(target*DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::ANGLE; break; } // point to the angles given by a mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: - switch (mavt_target.target_type) { - case MountTargetType::ANGLE: - _angle_rad = mavt_target.angle_rad; - break; - case MountTargetType::RATE: - update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad); - break; + // mnt_target should have already been filled in by set_angle_target() or set_rate_target() + if (mnt_target.target_type == MountTargetType::RATE) { + update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); } resend_now = true; break; @@ -70,11 +62,15 @@ void AP_Mount_SToRM32_serial::update() // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { // update targets using pilot's RC inputs - MountTarget rc_target {}; - if (get_rc_rate_target(rc_target)) { - update_angle_target_from_rate(rc_target, _angle_rad); - } else if (get_rc_angle_target(rc_target)) { - _angle_rad = rc_target; + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; } resend_now = true; break; @@ -82,19 +78,24 @@ void AP_Mount_SToRM32_serial::update() // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: - if (get_angle_target_to_roi(_angle_rad)) { + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; resend_now = true; } break; + // point mount to Home location case MAV_MOUNT_MODE_HOME_LOCATION: - if (get_angle_target_to_home(_angle_rad)) { + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; resend_now = true; } break; + // point mount to another vehicle case MAV_MOUNT_MODE_SYSID_TARGET: - if (get_angle_target_to_sysid(_angle_rad)) { + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; resend_now = true; } break; @@ -112,7 +113,7 @@ void AP_Mount_SToRM32_serial::update() } if (can_send(resend_now)) { if (resend_now) { - send_target_angles(_angle_rad); + send_target_angles(mnt_target.angle_rad); get_angles(); _reply_type = ReplyType_ACK; _reply_counter = 0; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h index 5ae4fe773aa1f..e6632ca10eb8b 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h @@ -130,7 +130,6 @@ class AP_Mount_SToRM32_serial : public AP_Mount_Backend AP_HAL::UARTDriver *_port; bool _initialised; // true once the driver has been initialised - MountTarget _angle_rad; // latest angle target uint32_t _last_send; // system time of last do_mount_control sent to gimbal uint8_t _reply_length; diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index e1809c4a844cf..9eb738503919b 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -20,14 +20,8 @@ void AP_Mount_Scripting::update() // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? case MAV_MOUNT_MODE_RETRACT: { const Vector3f &angle_bf_target = _params.retract_angles.get(); - target_angle_rad.roll = ToRad(angle_bf_target.x); - target_angle_rad.pitch = ToRad(angle_bf_target.y); - target_angle_rad.yaw = ToRad(angle_bf_target.z); - target_angle_rad.yaw_is_ef = false; - target_angle_rad_valid = true; - - // mark other targets as invalid - target_rate_rads_valid = false; + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::ANGLE; target_loc_valid = false; break; } @@ -35,92 +29,55 @@ void AP_Mount_Scripting::update() // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { const Vector3f &angle_bf_target = _params.neutral_angles.get(); - target_angle_rad.roll = ToRad(angle_bf_target.x); - target_angle_rad.pitch = ToRad(angle_bf_target.y); - target_angle_rad.yaw = ToRad(angle_bf_target.z); - target_angle_rad.yaw_is_ef = false; - target_angle_rad_valid = true; - - // mark other targets as invalid - target_rate_rads_valid = false; + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::ANGLE; target_loc_valid = false; break; } // point to the angles given by a mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: - switch (mavt_target.target_type) { - case MountTargetType::ANGLE: - target_angle_rad = mavt_target.angle_rad; - target_angle_rad_valid = true; - target_rate_rads_valid = false; - target_loc_valid = false; - break; - case MountTargetType::RATE: - target_rate_rads = mavt_target.rate_rads; - target_rate_rads_valid = true; - target_angle_rad_valid = false; - target_loc_valid = false; - break; - } + // mavlink targets should have been already stored while handling the message + target_loc_valid = false; break; // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's rc inputs - MountTarget rc_target {}; - if (get_rc_rate_target(rc_target)) { - target_rate_rads = rc_target; - target_rate_rads_valid = true; - target_angle_rad_valid = false; - target_loc_valid = false; - } else if (get_rc_angle_target(rc_target)) { - target_angle_rad = rc_target; - target_angle_rad_valid = true; - target_rate_rads_valid = false; - target_loc_valid = false; + // update targets using pilot's RC inputs + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; } + target_loc_valid = false; break; } - // point mount towards a GPS point - case MAV_MOUNT_MODE_GPS_POINT: { - target_loc_valid = _roi_target_set; - if (target_loc_valid) { - target_loc = _roi_target; - target_angle_rad_valid = get_angle_target_to_location(target_loc, target_angle_rad); - } else { - target_angle_rad_valid = false; + // point mount to a GPS point given by the mission planner + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } - target_rate_rads_valid = false; break; - } - // point mount towards home - case MAV_MOUNT_MODE_HOME_LOCATION: { - target_loc_valid = AP::ahrs().home_is_set(); - if (target_loc_valid) { - target_loc = AP::ahrs().get_home(); - target_angle_rad_valid = get_angle_target_to_home(target_angle_rad); - } else { - target_angle_rad_valid = false; + // point mount to Home location + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } - target_rate_rads_valid = false; break; - } - // point mount towards another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: { - target_loc_valid = _target_sysid_location_set; - if (target_loc_valid) { - target_loc = _target_sysid_location; - target_angle_rad_valid = get_angle_target_to_location(target_loc, target_angle_rad); - } else { - target_angle_rad_valid = false; + // point mount to another vehicle + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } - target_rate_rads_valid = false; break; - } default: // we do not know this mode so raise internal error @@ -136,31 +93,6 @@ bool AP_Mount_Scripting::healthy() const return (AP_HAL::millis() - last_update_ms <= AP_MOUNT_SCRIPTING_TIMEOUT_MS); } -// accessors for scripting backends -bool AP_Mount_Scripting::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) -{ - if (target_rate_rads_valid) { - roll_degs = degrees(target_rate_rads.roll); - pitch_degs = degrees(target_rate_rads.pitch); - yaw_degs = degrees(target_rate_rads.yaw); - yaw_is_earth_frame = target_rate_rads.yaw_is_ef; - return true; - } - return false; -} - -bool AP_Mount_Scripting::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) -{ - if (target_angle_rad_valid) { - roll_deg = degrees(target_angle_rad.roll); - pitch_deg = degrees(target_angle_rad.pitch); - yaw_deg = degrees(target_angle_rad.yaw); - yaw_is_earth_frame = target_angle_rad.yaw_is_ef; - return true; - } - return false; -} - // return target location if available // returns true if a target location is available and fills in target_loc argument bool AP_Mount_Scripting::get_location_target(Location &_target_loc) diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index d5f945fa120f2..356f9229b87e2 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -32,8 +32,6 @@ class AP_Mount_Scripting : public AP_Mount_Backend bool has_pan_control() const override { return yaw_range_valid(); }; // accessors for scripting backends - bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) override; - bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) override; bool get_location_target(Location& _target_loc) override; void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) override; @@ -48,11 +46,8 @@ class AP_Mount_Scripting : public AP_Mount_Backend uint32_t last_update_ms; // system time of last call to one of the get_ methods. Used for health reporting Vector3f current_angle_deg; // current gimbal angles in degrees (x=roll, y=pitch, z=yaw) - MountTarget target_rate_rads; // rate target in rad/s - bool target_rate_rads_valid; // true if _target_rate_degs holds a valid rate target - - MountTarget target_angle_rad; // angle target in radians - bool target_angle_rad_valid; // true if _target_rate_degs holds a valid rate target + bool target_rate_rads_valid; // true if mnt_target holds a valid rate target + bool target_angle_rad_valid; // true if mnt_target holds a valid angle target Location target_loc; // target location bool target_loc_valid; // true if target_loc holds a valid target location diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 840d19803e0c4..6d1d45a7668a0 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -27,89 +27,87 @@ void AP_Mount_Servo::init() // update mount position - should be called periodically void AP_Mount_Servo::update() { - switch (get_mode()) { + auto mount_mode = get_mode(); + switch (mount_mode) { // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage case MAV_MOUNT_MODE_RETRACT: { _angle_bf_output_rad = _params.retract_angles.get() * DEG_TO_RAD; - - // initialise _angle_rad to smooth transition if user changes to RC_TARGETTING - _angle_rad.roll = _angle_bf_output_rad.x; - _angle_rad.pitch = _angle_bf_output_rad.y; - _angle_rad.yaw = _angle_bf_output_rad.z; - _angle_rad.yaw_is_ef = false; + mnt_target.angle_rad.set(_angle_bf_output_rad, false); + mnt_target.target_type = MountTargetType::ANGLE; break; } // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { _angle_bf_output_rad = _params.neutral_angles.get() * DEG_TO_RAD; - - // initialise _angle_rad to smooth transition if user changes to RC_TARGETTING - _angle_rad.roll = _angle_bf_output_rad.x; - _angle_rad.pitch = _angle_bf_output_rad.y; - _angle_rad.yaw = _angle_bf_output_rad.z; - _angle_rad.yaw_is_ef = false; + mnt_target.angle_rad.set(_angle_bf_output_rad, false); + mnt_target.target_type = MountTargetType::ANGLE; break; } // point to the angles given by a mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: { - switch (mavt_target.target_type) { - case MountTargetType::ANGLE: - _angle_rad = mavt_target.angle_rad; - break; - case MountTargetType::RATE: - update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad); - break; - } - // update _angle_bf_output_rad based on angle target - update_angle_outputs(_angle_rad); + // mavlink targets are stored while handling the incoming message and considered valid break; } // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { // update targets using pilot's RC inputs - MountTarget rc_target {}; - if (get_rc_rate_target(rc_target)) { - update_angle_target_from_rate(rc_target, _angle_rad); - } else if (get_rc_angle_target(rc_target)) { - _angle_rad = rc_target; + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; } - // update _angle_bf_output_rad based on angle target - update_angle_outputs(_angle_rad); break; } - // point mount to a GPS location - case MAV_MOUNT_MODE_GPS_POINT: { - if (get_angle_target_to_roi(_angle_rad)) { - update_angle_outputs(_angle_rad); + // point mount to a GPS point given by the mission planner + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - case MAV_MOUNT_MODE_HOME_LOCATION: { - if (get_angle_target_to_home(_angle_rad)) { - update_angle_outputs(_angle_rad); + // point mount to Home location + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - case MAV_MOUNT_MODE_SYSID_TARGET: { - if (get_angle_target_to_sysid(_angle_rad)) { - update_angle_outputs(_angle_rad); + // point mount to another vehicle + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } default: //do nothing break; } + // send target angles or rates depending on the target type + switch (mnt_target.target_type) { + case MountTargetType::RATE: + update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); + FALLTHROUGH; + case MountTargetType::ANGLE: + // update _angle_bf_output_rad based on angle target + if ((mount_mode != MAV_MOUNT_MODE_RETRACT) & (mount_mode != MAV_MOUNT_MODE_NEUTRAL)) { + update_angle_outputs(mnt_target.angle_rad); + } + break; + } + // move mount to a "retracted position" into the fuselage with a fourth servo - const bool mount_open = (get_mode() == MAV_MOUNT_MODE_RETRACT) ? 0 : 1; + const bool mount_open = (mount_mode == MAV_MOUNT_MODE_RETRACT) ? 0 : 1; move_servo(_open_idx, mount_open, 0, 1); // write the results to the servos diff --git a/libraries/AP_Mount/AP_Mount_Servo.h b/libraries/AP_Mount/AP_Mount_Servo.h index 9016fd2af6d96..529a97bd7341c 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.h +++ b/libraries/AP_Mount/AP_Mount_Servo.h @@ -56,7 +56,6 @@ class AP_Mount_Servo : public AP_Mount_Backend SRV_Channel::Aux_servo_function_t _pan_idx; // SRV_Channel mount pan function index SRV_Channel::Aux_servo_function_t _open_idx; // SRV_Channel mount open function index - MountTarget _angle_rad; // angle target Vector3f _angle_bf_output_rad; // final body frame output angle in radians }; #endif // HAL_MOUNT_SERVO_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index ee60bbf00cd76..f1dc4a1bfa6c8 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -68,76 +68,78 @@ void AP_Mount_Siyi::update() // run zoom control update_zoom_control(); - // update based on mount mode + // Get the target angles or rates first depending on the current mount mode switch (get_mode()) { - // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? case MAV_MOUNT_MODE_RETRACT: { const Vector3f &angle_bf_target = _params.retract_angles.get(); - send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false); + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); break; } - // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { const Vector3f &angle_bf_target = _params.neutral_angles.get(); - send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false); + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); break; } - // point to the angles given by a mavlink message - case MAV_MOUNT_MODE_MAVLINK_TARGETING: - switch (mavt_target.target_type) { + case MAV_MOUNT_MODE_MAVLINK_TARGETING: { + // mavlink targets are stored while handling the incoming message + break; + } + + case MAV_MOUNT_MODE_RC_TARGETING: { + // update targets using pilot's RC inputs + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { case MountTargetType::ANGLE: - send_target_angles(mavt_target.angle_rad.pitch, mavt_target.angle_rad.yaw, mavt_target.angle_rad.yaw_is_ef); + mnt_target.angle_rad = rc_target; break; case MountTargetType::RATE: - send_target_rates(mavt_target.rate_rads.pitch, mavt_target.rate_rads.yaw, mavt_target.rate_rads.yaw_is_ef); + mnt_target.rate_rads = rc_target; break; } break; - - // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's rc inputs - MountTarget rc_target {}; - if (get_rc_rate_target(rc_target)) { - send_target_rates(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef); - } else if (get_rc_angle_target(rc_target)) { - send_target_angles(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef); - } - break; } // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: { - MountTarget angle_target_rad {}; - if (get_angle_target_to_roi(angle_target_rad)) { - send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - case MAV_MOUNT_MODE_HOME_LOCATION: { - MountTarget angle_target_rad {}; - if (get_angle_target_to_home(angle_target_rad)) { - send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + // point mount to Home location + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - case MAV_MOUNT_MODE_SYSID_TARGET:{ - MountTarget angle_target_rad {}; - if (get_angle_target_to_sysid(angle_target_rad)) { - send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + // point mount to another vehicle + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } default: // we do not know this mode so raise internal error INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); break; } + + // send target angles or rates depending on the target type + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); + break; + case MountTargetType::RATE: + send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef); + break; + } } // return true if healthy diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 2643eb36947a2..907b09a0d4574 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -36,61 +36,66 @@ void AP_Mount_SoloGimbal::update() // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism case MAV_MOUNT_MODE_RETRACT: _gimbal.set_lockedToBody(true); - // initialise _angle_rad to smooth transition if user changes to RC_TARGETTINg - _angle_rad = {0, 0, 0, false}; + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(Vector3f{0,0,0}, false); break; // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { _gimbal.set_lockedToBody(false); const Vector3f &target = _params.neutral_angles.get(); - _angle_rad.roll = radians(target.x); - _angle_rad.pitch = radians(target.y); - _angle_rad.yaw = radians(target.z); - _angle_rad.yaw_is_ef = false; + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(target*DEG_TO_RAD, false); break; } // point to the angles given by a mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: + // targets are stored while handling the incoming mavlink message _gimbal.set_lockedToBody(false); - switch (mavt_target.target_type) { - case MountTargetType::ANGLE: - _angle_rad = mavt_target.angle_rad; - break; - case MountTargetType::RATE: - update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad); - break; + if (mnt_target.target_type == MountTargetType::RATE) { + update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); } break; // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { _gimbal.set_lockedToBody(false); - // update targets using pilot's RC inputs - MountTarget rc_target {}; - if (get_rc_rate_target(rc_target)) { - update_angle_target_from_rate(rc_target, _angle_rad); - } else if (get_rc_angle_target(rc_target)) { - _angle_rad = rc_target; + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; } break; } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: - _gimbal.set_lockedToBody(false); - IGNORE_RETURN(get_angle_target_to_roi(_angle_rad)); + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + _gimbal.set_lockedToBody(false); + } break; + // point mount to Home location case MAV_MOUNT_MODE_HOME_LOCATION: - _gimbal.set_lockedToBody(false); - IGNORE_RETURN(get_angle_target_to_home(_angle_rad)); + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + _gimbal.set_lockedToBody(false); + } break; + // point mount to another vehicle case MAV_MOUNT_MODE_SYSID_TARGET: - _gimbal.set_lockedToBody(false); - IGNORE_RETURN(get_angle_target_to_sysid(_angle_rad)); + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + _gimbal.set_lockedToBody(false); + } break; default: @@ -105,7 +110,7 @@ bool AP_Mount_SoloGimbal::get_attitude_quaternion(Quaternion& att_quat) if (!_gimbal.aligned()) { return false; } - att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, _angle_rad.get_bf_yaw()); + att_quat.from_euler(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw()); return true; } @@ -114,7 +119,7 @@ bool AP_Mount_SoloGimbal::get_attitude_quaternion(Quaternion& att_quat) */ void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) { - _gimbal.update_target(Vector3f{_angle_rad.roll, _angle_rad.pitch, _angle_rad.get_bf_yaw()}); + _gimbal.update_target(Vector3f{mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw()}); _gimbal.receive_feedback(chan,msg); AP_Logger *logger = AP_Logger::get_singleton(); diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.h b/libraries/AP_Mount/AP_Mount_SoloGimbal.h index 87503e5f39d8d..4e64233284a9f 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.h +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.h @@ -54,7 +54,6 @@ class AP_Mount_SoloGimbal : public AP_Mount_Backend void Log_Write_Gimbal(SoloGimbal &gimbal); bool _params_saved; - MountTarget _angle_rad; // angle target SoloGimbal _gimbal; }; diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index 5b199d7af3e93..42749f74605c3 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -76,71 +76,76 @@ void AP_Mount_Viewpro::update() // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? case MAV_MOUNT_MODE_RETRACT: { const Vector3f &angle_bf_target = _params.retract_angles.get(); - send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false); + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); break; } // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { const Vector3f &angle_bf_target = _params.neutral_angles.get(); - send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false); + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); break; } // point to the angles given by a mavlink message case MAV_MOUNT_MODE_MAVLINK_TARGETING: - switch (mavt_target.target_type) { - case MountTargetType::ANGLE: - send_target_angles(mavt_target.angle_rad.pitch, mavt_target.angle_rad.yaw, mavt_target.angle_rad.yaw_is_ef); - break; - case MountTargetType::RATE: - send_target_rates(mavt_target.rate_rads.pitch, mavt_target.rate_rads.yaw, mavt_target.rate_rads.yaw_is_ef); - break; - } + // mavlink targets are stored while handling the incoming message break; // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's rc inputs - MountTarget rc_target {}; - if (get_rc_rate_target(rc_target)) { - send_target_rates(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef); - } else if (get_rc_angle_target(rc_target)) { - send_target_angles(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef); + // update targets using pilot's RC inputs + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; } break; } // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: { - MountTarget angle_target_rad {}; - if (get_angle_target_to_roi(angle_target_rad)) { - send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - case MAV_MOUNT_MODE_HOME_LOCATION: { - MountTarget angle_target_rad {}; - if (get_angle_target_to_home(angle_target_rad)) { - send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + // point mount to Home location + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - case MAV_MOUNT_MODE_SYSID_TARGET:{ - MountTarget angle_target_rad {}; - if (get_angle_target_to_sysid(angle_target_rad)) { - send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + // point mount to another vehicle + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } default: // we do not know this mode so raise internal error INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); break; } + + // send target angles or rates depending on the target type + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); + break; + case MountTargetType::RATE: + send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef); + break; + } } // return true if healthy diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index cb08a54fb4cff..62ec3396a3e4a 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -54,71 +54,74 @@ void AP_Mount_Xacti::update() // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? case MAV_MOUNT_MODE_RETRACT: { const Vector3f &angle_bf_target = _params.retract_angles.get(); - send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false); + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); break; } - // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: { const Vector3f &angle_bf_target = _params.neutral_angles.get(); - send_target_rates(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false); + mnt_target.target_type = MountTargetType::ANGLE; + mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); break; } - // point to the angles given by a mavlink message - case MAV_MOUNT_MODE_MAVLINK_TARGETING: - switch (mavt_target.target_type) { + case MAV_MOUNT_MODE_MAVLINK_TARGETING: { + // mavlink targets are set while handling the incoming message + break; + } + + case MAV_MOUNT_MODE_RC_TARGETING: { + // update targets using pilot's RC inputs + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { case MountTargetType::ANGLE: - send_target_angles(mavt_target.angle_rad.pitch, mavt_target.angle_rad.yaw, mavt_target.angle_rad.yaw_is_ef); + mnt_target.angle_rad = rc_target; break; case MountTargetType::RATE: - send_target_rates(mavt_target.rate_rads.pitch, mavt_target.rate_rads.yaw, mavt_target.rate_rads.yaw_is_ef); + mnt_target.rate_rads = rc_target; break; } break; - - // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's rc inputs - MountTarget rc_target {}; - if (get_rc_rate_target(rc_target)) { - send_target_rates(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef); - } else if (get_rc_angle_target(rc_target)) { - send_target_angles(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef); - } - break; } // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: { - MountTarget angle_target_rad {}; - if (get_angle_target_to_roi(angle_target_rad)) { - send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - case MAV_MOUNT_MODE_HOME_LOCATION: { - MountTarget angle_target_rad {}; - if (get_angle_target_to_home(angle_target_rad)) { - send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + // point mount to Home location + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - case MAV_MOUNT_MODE_SYSID_TARGET:{ - MountTarget angle_target_rad {}; - if (get_angle_target_to_sysid(angle_target_rad)) { - send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef); + // point mount to another vehicle + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } default: // we do not know this mode so raise internal error INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); break; } + + // send target angles or rates depending on the target type + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); + break; + case MountTargetType::RATE: + send_target_rates(mnt_target.rate_rads.pitch, mnt_target.rate_rads.yaw, mnt_target.rate_rads.yaw_is_ef); + break; + } } // return true if healthy diff --git a/libraries/AP_Mount/LogStructure.h b/libraries/AP_Mount/LogStructure.h new file mode 100644 index 0000000000000..ab66620cf7bc1 --- /dev/null +++ b/libraries/AP_Mount/LogStructure.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#define LOG_IDS_FROM_MOUNT \ + LOG_MOUNT_MSG + +// @LoggerMessage: MNT +// @Description: Mount's actual and Target/Desired RPY information +// @Field: TimeUS: Time since system startup +// @Field: I: Instance number +// @Field: DesRoll: mount's desired roll +// @Field: Roll: mount's actual roll +// @Field: DesPitch: mount's desired pitch +// @Field: Pitch: mount's actual pitch +// @Field: DesYawB: mount's desired yaw in body frame +// @Field: YawB: mount's actual yaw in body frame +// @Field: DesYawE: mount's desired yaw in earth frame +// @Field: YawE: mount's actual yaw in earth frame + +struct PACKED log_Mount { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + float desired_roll; + float actual_roll; + float desired_pitch; + float actual_pitch; + float desired_yaw_bf; + float actual_yaw_bf; + float desired_yaw_ef; + float actual_yaw_ef; +}; + +#define LOG_STRUCTURE_FROM_MOUNT \ + { LOG_MOUNT_MSG, sizeof(log_Mount), \ + "MNT", "QBffffffff","TimeUS,I,DesRoll,Roll,DesPitch,Pitch,DesYawB,YawB,DesYawE,YawE", "s#dddddddd", "F---------" }, + From 60555a2d990adfdba244023150c6ad585e0244a5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 18 Jul 2023 20:27:13 +0900 Subject: [PATCH 04/12] Copter: log MNT at 10hz --- ArduCopter/Copter.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index f7a0f448712d7..605532d44439a 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -583,6 +583,11 @@ void Copter::ten_hz_logging_loop() g2.winch.write_log(); } #endif +#if HAL_MOUNT_ENABLED + if (should_log(MASK_LOG_CAMERA)) { + camera_mount.write_log(); + } +#endif } // twentyfive_hz_logging - should be run at 25hz From 73c632b18423d6ca1395948b778c76b7e6ebc259 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 18 Jul 2023 20:27:20 +0900 Subject: [PATCH 05/12] Plane: log MNT at 10hz --- ArduPlane/ArduPlane.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 47f379a2e1f65..61d7642533327 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -245,7 +245,12 @@ void Plane::update_logging10(void) ahrs.Write_AOA_SSA(); } else if (log_faster) { ahrs.Write_AOA_SSA(); - } + } +#if HAL_MOUNT_ENABLED + if (should_log(MASK_LOG_CAMERA)) { + camera_mount.write_log(); + } +#endif } /* From 954f99521ac8a7c135fe93ff4ab817885aac2b4d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 18 Jul 2023 20:27:34 +0900 Subject: [PATCH 06/12] Rover: log MNT at 10hz --- Rover/Rover.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index 2a429dd5556f3..837e95cc5ac31 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -421,6 +421,11 @@ void Rover::update_logging2(void) gyro_fft.write_log_messages(); #endif } +#if HAL_MOUNT_ENABLED + if (should_log(MASK_LOG_CAMERA)) { + camera_mount.write_log(); + } +#endif } From 3b7abc46739c06097fa62c0563f3b4c7a083b0e9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 18 Jul 2023 20:27:26 +0900 Subject: [PATCH 07/12] Sub: log MNT at 10hz --- ArduSub/ArduSub.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 9869ed29b80ad..b8ded91fddc34 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -210,6 +210,11 @@ void Sub::ten_hz_logging_loop() if (should_log(MASK_LOG_CTUN)) { attitude_control.control_monitor_log(); } +#if HAL_MOUNT_ENABLED + if (should_log(MASK_LOG_CAMERA)) { + camera_mount.write_log(); + } +#endif } // twentyfive_hz_logging_loop From f9d7457386e23451bfb1f79c9c6b2d45626a9972 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 20 Jul 2023 10:08:32 +0900 Subject: [PATCH 08/12] AP_Mount: viewpro fix for pitch angle reporting --- libraries/AP_Mount/AP_Mount_Viewpro.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index 42749f74605c3..36965cb6911f3 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -315,7 +315,7 @@ void AP_Mount_Viewpro::process_packet() _last_current_angle_rad_ms = AP_HAL::millis(); _current_angle_rad.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+23] & 0x0F, _msg_buff[_msg_buff_data_start+24]) * (180.0/4095.0) - 90.0); // roll angle _current_angle_rad.z = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+25], _msg_buff[_msg_buff_data_start+26]) * AP_MOUNT_VIEWPRO_OUTPUT_TO_DEG); // yaw angle - _current_angle_rad.y = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+27], _msg_buff[_msg_buff_data_start+28]) * AP_MOUNT_VIEWPRO_OUTPUT_TO_DEG); // pitch angle + _current_angle_rad.y = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+27], _msg_buff[_msg_buff_data_start+28]) * AP_MOUNT_VIEWPRO_OUTPUT_TO_DEG); // pitch angle debug("r:%4.1f p:%4.1f y:%4.1f", (double)degrees(_current_angle_rad.x), (double)degrees(_current_angle_rad.y), (double)degrees(_current_angle_rad.z)); break; } From b7124557c7bc9abe589d83a4143927310d743792 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 20 Jul 2023 10:26:49 +0900 Subject: [PATCH 09/12] AP_Scripting: viewpro driver fix for pitch angle reporting --- libraries/AP_Scripting/drivers/mount-viewpro-driver.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua b/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua index dcbf158561fbc..9786e6327bba3 100644 --- a/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua +++ b/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua @@ -379,7 +379,7 @@ function parse_byte(b) local servo_status = (parse_data_buff[24] & 0xF0 >> 4) local roll_deg = int16_value(parse_data_buff[24] & 0x0F, parse_data_buff[25]) * (180.0/4095.0) - 90.0 local yaw_deg = int16_value(parse_data_buff[26], parse_data_buff[27]) * (360.0 / 65536.0) - local pitch_deg = int16_value(parse_data_buff[28], parse_data_buff[29]) * (360.0 / 65536.0) + local pitch_deg = -int16_value(parse_data_buff[28], parse_data_buff[29]) * (360.0 / 65536.0) mount:set_attitude_euler(MOUNT_INSTANCE, roll_deg, pitch_deg, yaw_deg) if VIEP_DEBUG:get() > 0 then From 9fae2774833221b7f6454ff4b8184da7057be256 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 20 Jul 2023 20:18:54 +0900 Subject: [PATCH 10/12] AP_Mount: Gremsy fix for attitude reporting --- libraries/AP_Mount/AP_Mount_Gremsy.cpp | 6 ------ libraries/AP_Mount/AP_Mount_Gremsy.h | 1 - 2 files changed, 7 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index 5c96daa7f17cf..b9bf372a68064 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -128,12 +128,6 @@ bool AP_Mount_Gremsy::healthy() const // get attitude as a quaternion. returns true on success bool AP_Mount_Gremsy::get_attitude_quaternion(Quaternion& att_quat) { - // check we have received an updated message - if (_gimbal_device_attitude_status.time_boot_ms == _sent_gimbal_device_attitude_status_ms) { - return false; - } - _sent_gimbal_device_attitude_status_ms = _gimbal_device_attitude_status.time_boot_ms; - att_quat = _gimbal_device_attitude_status.q; return true; } diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.h b/libraries/AP_Mount/AP_Mount_Gremsy.h index 84670366e55e9..79bd42af77670 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.h +++ b/libraries/AP_Mount/AP_Mount_Gremsy.h @@ -70,6 +70,5 @@ class AP_Mount_Gremsy : public AP_Mount_Backend uint8_t _compid; // component id of gimbal mavlink_gimbal_device_attitude_status_t _gimbal_device_attitude_status; // copy of most recently received gimbal status uint32_t _last_attitude_status_ms; // system time last attitude status was received (used for health reporting) - uint32_t _sent_gimbal_device_attitude_status_ms; // time_boot_ms field of gimbal_device_status message last forwarded to the GCS (used to prevent sending duplicates) }; #endif // HAL_MOUNT_GREMSY_ENABLED From 0bf3bf2a2589634a2417395fe8f175b2f75d1b9a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 21 Jul 2023 10:25:37 +0900 Subject: [PATCH 11/12] AP_Scripting: mount-djirs2 driver angle reporting fix --- libraries/AP_Scripting/drivers/mount-djirs2-driver.lua | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua b/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua index 7f94df7b4e581..ac050cabc6633 100644 --- a/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua +++ b/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua @@ -462,7 +462,7 @@ function send_target_angles(roll_angle_deg, pitch_angle_deg, yaw_angle_deg, time end -- ensure angles are integers - roll_angle_deg = -math.floor(roll_angle_deg + 0.5) + roll_angle_deg = math.floor(roll_angle_deg + 0.5) pitch_angle_deg = math.floor(pitch_angle_deg + 0.5) yaw_angle_deg = math.floor(yaw_angle_deg + 0.5) time_sec = math.floor(time_sec + 0.5) @@ -657,8 +657,8 @@ function parse_byte(b) local ret_code = parse_buff[13] if ret_code == RETURN_CODE.SUCCESS then local yaw_deg = int16_value(parse_buff[16],parse_buff[15]) * 0.1 - local roll_deg = -int16_value(parse_buff[18],parse_buff[17]) * 0.1 - local pitch_deg = int16_value(parse_buff[20],parse_buff[19]) * 0.1 + local pitch_deg = int16_value(parse_buff[18],parse_buff[17]) * 0.1 + local roll_deg = int16_value(parse_buff[20],parse_buff[19]) * 0.1 mount:set_attitude_euler(MOUNT_INSTANCE, roll_deg, pitch_deg, yaw_deg) if DJIR_DEBUG:get() > 1 then gcs:send_text(MAV_SEVERITY.INFO, string.format("DJIR: roll:%4.1f pitch:%4.1f yaw:%4.1f", roll_deg, pitch_deg, yaw_deg)) From bcbe35892f2b976d3e5aabda16afaf77b69a3a59 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 21 Jul 2023 10:26:09 +0900 Subject: [PATCH 12/12] AP_Camera: TYPE param desc gets None value --- libraries/AP_Camera/AP_Camera_Params.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 8f64550bfd27e..ca7e34ae3e228 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Camera shutter (trigger) type // @Description: how to trigger the camera to take a picture - // @Values: 1:Servo,2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting + // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE),