diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index b672e512e8..5f35ade558 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -441,13 +441,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id) #endif break; - case MSG_MOUNT_STATUS: -#if MOUNT == ENABLED - CHECK_PAYLOAD_SIZE(MOUNT_STATUS); - sub.camera_mount.status_msg(chan); -#endif // MOUNT == ENABLED - break; - case MSG_OPTICAL_FLOW: #if OPTFLOW == ENABLED CHECK_PAYLOAD_SIZE(OPTICAL_FLOW); @@ -455,13 +448,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id) #endif break; - case MSG_GIMBAL_REPORT: -#if MOUNT == ENABLED - CHECK_PAYLOAD_SIZE(GIMBAL_REPORT); - sub.camera_mount.send_gimbal_report(chan); -#endif - break; - case MSG_PID_TUNING: CHECK_PAYLOAD_SIZE(PID_TUNING); sub.send_pid_tuning(chan); @@ -661,6 +647,15 @@ MAV_RESULT GCS_MAVLINK_Sub::_handle_command_preflight_calibration(const mavlink_ return GCS_MAVLINK::_handle_command_preflight_calibration(packet); } +MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc) +{ + if (!check_latlng(roi_loc)) { + return MAV_RESULT_FAILED; + } + sub.set_auto_yaw_roi(roi_loc); + return MAV_RESULT_ACCEPTED; +} + MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet) { switch (packet.command) { @@ -707,25 +702,6 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_ return MAV_RESULT_FAILED; } - case MAV_CMD_DO_SET_ROI: { - // param1 : /* Region of interest mode (not used)*/ - // param2 : /* MISSION index/ target ID (not used)*/ - // param3 : /* ROI index (not used)*/ - // param4 : /* empty */ - // x : lat - // y : lon - // z : alt - // sanity check location - if (!check_latlng(packet.x, packet.y)) { - return MAV_RESULT_FAILED; - } - Location roi_loc; - roi_loc.lat = packet.x; - roi_loc.lng = packet.y; - roi_loc.alt = (int32_t)(packet.z * 100.0f); - sub.set_auto_yaw_roi(roi_loc); - return MAV_RESULT_ACCEPTED; - } default: return GCS_MAVLINK::handle_command_int_packet(packet); } @@ -801,30 +777,6 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon } return MAV_RESULT_FAILED; - case MAV_CMD_DO_SET_ROI: - // param1 : regional of interest mode (not supported) - // param2 : mission index/ target id (not supported) - // param3 : ROI index (not supported) - // param5 : x / lat - // param6 : y / lon - // param7 : z / alt - // sanity check location - if (!check_latlng(packet.param5, packet.param6)) { - return MAV_RESULT_FAILED; - } - Location roi_loc; - roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); - roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); - roi_loc.alt = (int32_t)(packet.param7 * 100.0f); - sub.set_auto_yaw_roi(roi_loc); - return MAV_RESULT_ACCEPTED; - -#if MOUNT == ENABLED - case MAV_CMD_DO_MOUNT_CONTROL: - sub.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); - return MAV_RESULT_ACCEPTED; -#endif - case MAV_CMD_MISSION_START: if (sub.motors.armed() && sub.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) { return MAV_RESULT_ACCEPTED; @@ -892,20 +844,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) break; } - case MAVLINK_MSG_ID_PARAM_VALUE: { -#if MOUNT == ENABLED - sub.camera_mount.handle_param_value(msg); -#endif - break; - } - - case MAVLINK_MSG_ID_GIMBAL_REPORT: { -#if MOUNT == ENABLED - handle_gimbal_report(sub.camera_mount, msg); -#endif - break; - } - case MAVLINK_MSG_ID_MANUAL_CONTROL: { // MAV ID: 69 if (msg->sysid != sub.g.sysid_my_gcs) { break; // Only accept control from our gcs @@ -1143,17 +1081,6 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) break; #endif // AC_FENCE == ENABLED -#if MOUNT == ENABLED - //deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE - case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // MAV ID: 204 - sub.camera_mount.configure_msg(msg); - break; - //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL - case MAVLINK_MSG_ID_MOUNT_CONTROL: - sub.camera_mount.control_msg(msg); - break; -#endif // MOUNT == ENABLED - case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: #if AP_TERRAIN_AVAILABLE && AC_TERRAIN diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 53d79d99f7..5b0dfed0e0 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -22,6 +22,7 @@ protected: bool set_mode(uint8_t mode) override; bool should_zero_rc_outputs_on_reboot() const override { return true; } + MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; MAV_RESULT _handle_command_preflight_calibration_baro() override; MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index a49985c015..2d26e9d509 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -408,7 +408,7 @@ private: // Camera/Antenna mount tracking and stabilisation stuff #if MOUNT == ENABLED // current_loc uses the baro/gps soloution for altitude rather than gps only. - AP_Mount camera_mount{ahrs, current_loc}; + AP_Mount camera_mount{current_loc}; #endif // AC_Fence library to reduce fly-aways