From 06b9df2c0f6a4763149a31cd3d563b8596a1c962 Mon Sep 17 00:00:00 2001 From: chiyu1468 Date: Wed, 27 Mar 2024 07:02:55 +0800 Subject: [PATCH] basic command complete MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 使用另一個方式做了 goto 完成了 set height --- mavone/systemHandler.cpp | 71 ++++++++++++++++++++++++---------------- 1 file changed, 43 insertions(+), 28 deletions(-) diff --git a/mavone/systemHandler.cpp b/mavone/systemHandler.cpp index 69634f9..4dd0cf6 100644 --- a/mavone/systemHandler.cpp +++ b/mavone/systemHandler.cpp @@ -74,13 +74,16 @@ void systemHandler(System& system) { bool reset = false; // 程序執行時會用到的變數 - int _counter; mavlink_message_t mav_message; - mavlink_set_position_target_global_int_t set_position_target_global_int; + mavlink_command_int_t mav_command_int; MavlinkPassthrough::CommandLong mav_command_long; MavlinkPassthrough::Result result_mavpass; Action::Result result_action; + int _counter; + double _target_lon; + double _target_lat; + // std::cout << "Dev Test Start" << std::endl; @@ -105,12 +108,10 @@ void systemHandler(System& system) { const Telemetry::FlightMode mode_result = telemetry.flight_mode(); std::cerr << "Vehicle Mode: " << mode_result << '\n'; - - std::cout << " Set Mode Test " << std::endl; sleep_for(milliseconds(1500)); - // sleep_for(milliseconds(1500)); + // action.set_takeoff_altitude(10); // std::cout << "ARM...\n"; // action.arm(); @@ -367,6 +368,8 @@ void systemHandler(System& system) { #ifdef VEHICLE_CONTROL_ACTIVE result_action = action.takeoff(); std::cout << " ACK REL: " << result_action << std::endl; // dev + _target_lat = std::stof(telemetryInfo["vehicle_lat"]); + _target_lon = std::stof(telemetryInfo["vehicle_lon"]); #endif break; @@ -376,7 +379,27 @@ void systemHandler(System& system) { << "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \ << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug #ifdef VEHICLE_CONTROL_ACTIVE - #endif + if (telemetryInfo["drone_air"] == "0") { + action.set_takeoff_altitude(static_cast(oneVehicleCommand.second[0])); // mavsdk show error but still effect well (?) + } else { + mav_command_int.target_system = sysid; // Target system ID + mav_command_int.target_component = MAV_COMP_ID_AUTOPILOT1; // Target component ID + mav_command_int.command = MAV_CMD_DO_REPOSITION; // Command ID + mav_command_int.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // Coordinate system + mav_command_int.x = _target_lat * 1E7; // Target latitude + mav_command_int.y = _target_lon * 1E7; // Target longitude + mav_command_int.z = static_cast(oneVehicleCommand.second[0]); // Target altitude + mavlink_msg_command_int_encode(mavlink_passthrough.get_our_sysid(), + mavlink_passthrough.get_our_compid(), + &mav_message, + &mav_command_int); + // mavlink_passthrough.send_message(mav_message); + result_mavpass = mavlink_passthrough.send_message(mav_message); + std::cout << " ACK REL: " << result_mavpass << std::endl; // dev + std::cout << "x: " << std::to_string(_target_lon) << std::endl; // dev + std::cout << "y: " << std::to_string(_target_lat) << std::endl; // dev + } + #endif break; // Here deal with the function of GOTO @@ -385,29 +408,21 @@ void systemHandler(System& system) { << "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \ << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug #ifdef VEHICLE_CONTROL_ACTIVE - set_position_target_global_int.time_boot_ms = 0; - set_position_target_global_int.target_system = sysid; - set_position_target_global_int.target_component = MAV_COMP_ID_AUTOPILOT1; - set_position_target_global_int.coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // 對於 home 點指定相對高度 - set_position_target_global_int.type_mask = 0b110111111000; // Only latitude, longitude and altitude - set_position_target_global_int.lat_int = oneVehicleCommand.second[0] * 1E7; // Latitude in degrees * 1E7 - set_position_target_global_int.lon_int = oneVehicleCommand.second[1] * 1E7; // Longitude in degrees * 1E7 - set_position_target_global_int.alt = 30; // Altitude in meters - - mavlink_msg_set_position_target_global_int_pack( - mavlink_passthrough.get_our_sysid(), - mavlink_passthrough.get_our_compid(), - &mav_message, - set_position_target_global_int.time_boot_ms, - set_position_target_global_int.target_system, - set_position_target_global_int.target_component, - set_position_target_global_int.coordinate_frame, - set_position_target_global_int.type_mask, - set_position_target_global_int.lat_int, - set_position_target_global_int.lon_int, - set_position_target_global_int.alt, - 0, 0, 0, 0, 0, 0, 0, 0); // vx, vy, vz, afx, afy, afz, yaw, yaw_rate are all 0 + mav_command_int.target_system = sysid; // Target system ID + mav_command_int.target_component = MAV_COMP_ID_AUTOPILOT1; // Target component ID + mav_command_int.command = MAV_CMD_DO_REPOSITION; // Command ID + mav_command_int.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // Coordinate system + mav_command_int.x = oneVehicleCommand.second[0] * 1E7; // Target latitude + mav_command_int.y = oneVehicleCommand.second[1] * 1E7; // Target longitude + mav_command_int.z = std::stof(telemetryInfo["drone_alt"]); // Target altitude + mavlink_msg_command_int_encode(mavlink_passthrough.get_our_sysid(), + mavlink_passthrough.get_our_compid(), + &mav_message, + &mav_command_int); + // mavlink_passthrough.send_message(mav_message); result_mavpass = mavlink_passthrough.send_message(mav_message); + _target_lon = oneVehicleCommand.second[1]; + _target_lat = oneVehicleCommand.second[0]; std::cout << " ACK REL: " << result_mavpass << std::endl; // dev #endif break;