From 022d61051938289729c1e29793263bc3caf37dbf Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 Aug 2022 19:58:15 +0900 Subject: [PATCH] AP_Mount: separate handling of mount-control and do-mount-control --- libraries/AP_Mount/AP_Mount.cpp | 5 +- libraries/AP_Mount/AP_Mount_Backend.cpp | 103 +++++++++++++++--------- libraries/AP_Mount/AP_Mount_Backend.h | 4 +- 3 files changed, 68 insertions(+), 44 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index a8b7feafd4..6d5c99ffe2 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -635,10 +635,7 @@ MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_long_ return MAV_RESULT_FAILED; } - // send message to backend - _backends[_primary]->control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); - - return MAV_RESULT_ACCEPTED; + return _backends[_primary]->handle_command_do_mount_control(packet); } MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 170bfe32f7..313e329371 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -96,53 +96,80 @@ void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan // process MOUNT_CONTROL messages received from GCS. deprecated. void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packet) { - control((int32_t)packet.input_a, (int32_t)packet.input_b, (int32_t)packet.input_c, _mode); + switch (get_mode()) { + case MAV_MOUNT_MODE_MAVLINK_TARGETING: + // input_a : Pitch in centi-degrees + // input_b : Roll in centi-degrees + // input_c : Yaw in centi-degrees (interpreted as body-frame) + set_angle_target(packet.input_b * 0.01, packet.input_a * 0.01, packet.input_c * 0.01, false); + break; + + case MAV_MOUNT_MODE_GPS_POINT: { + // input_a : lat in degE7 + // input_b : lon in degE7 + // input_c : alt in cm (interpreted as above home) + const Location target_location { + packet.input_a, + packet.input_b, + packet.input_c, + Location::AltFrame::ABOVE_HOME + }; + set_roi_target(target_location); + break; + } + + case MAV_MOUNT_MODE_RETRACT: + case MAV_MOUNT_MODE_NEUTRAL: + case MAV_MOUNT_MODE_RC_TARGETING: + case MAV_MOUNT_MODE_SYSID_TARGET: + case MAV_MOUNT_MODE_HOME_LOCATION: + default: + // no effect in these modes + break; + } } -void AP_Mount_Backend::control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, MAV_MOUNT_MODE mount_mode) +// handle do_mount_control command. Returns MAV_RESULT_ACCEPTED on success +MAV_RESULT AP_Mount_Backend::handle_command_do_mount_control(const mavlink_command_long_t &packet) { - set_mode(mount_mode); + const MAV_MOUNT_MODE new_mode = (MAV_MOUNT_MODE)packet.param7; // interpret message fields based on mode - switch (get_mode()) { - case MAV_MOUNT_MODE_RETRACT: - case MAV_MOUNT_MODE_NEUTRAL: - // do nothing with request if mount is retracted or in neutral position - break; + switch (new_mode) { + case MAV_MOUNT_MODE_RETRACT: + case MAV_MOUNT_MODE_NEUTRAL: + case MAV_MOUNT_MODE_RC_TARGETING: + case MAV_MOUNT_MODE_HOME_LOCATION: + // simply set mode + set_mode(new_mode); + return MAV_RESULT_ACCEPTED; - // set earth frame target angles from mavlink message - case MAV_MOUNT_MODE_MAVLINK_TARGETING: - set_angle_target(roll_or_lon*0.01f, pitch_or_lat*0.01f, yaw_or_alt*0.01f, false); - break; - - // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization - case MAV_MOUNT_MODE_RC_TARGETING: - // do nothing if pilot is controlling the roll, pitch and yaw - break; + case MAV_MOUNT_MODE_MAVLINK_TARGETING: + // set body-frame target angles (in degrees) from mavlink message + // param1: pitch (in degrees) + // param2: roll in degrees + // param3: yaw in degrees + set_angle_target(packet.param2, packet.param1, packet.param3, false); + return MAV_RESULT_ACCEPTED; + case MAV_MOUNT_MODE_GPS_POINT: { // set lat, lon, alt position targets from mavlink message + // param4: altitude in meters + // param5: latitude in degrees * 1E7 + // param6: longitude in degrees * 1E7 + const Location target_location { + (int32_t)packet.param5, // latitude in degrees * 1E7 + (int32_t)packet.param6, // longitude in degrees * 1E7 + (int32_t)packet.param4 * 100, // alt converted from meters to cm + Location::AltFrame::ABOVE_HOME + }; + set_roi_target(target_location); + return MAV_RESULT_ACCEPTED; + } - case MAV_MOUNT_MODE_GPS_POINT: { - const Location target_location{ - pitch_or_lat, - roll_or_lon, - yaw_or_alt, - Location::AltFrame::ABOVE_HOME - }; - set_roi_target(target_location); - break; - } - - case MAV_MOUNT_MODE_HOME_LOCATION: { - // set the target gps location - _roi_target = AP::ahrs().get_home(); - _roi_target_set = true; - break; - } - - default: - // do nothing - break; + default: + // invalid mode + return MAV_RESULT_FAILED; } } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 65c7512977..1b006bbfd9 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -73,8 +73,8 @@ public: // 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); + // handle do_mount_control command. Returns MAV_RESULT_ACCEPTED on success + MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet); // process MOUNT_CONFIGURE messages received from GCS. deprecated. void handle_mount_configure(const mavlink_mount_configure_t &msg);