chiyu1468
/
MAVSDK_Dev
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

nothing change

Browse Source 
只是整理一下排版跟刪掉多餘註解
Dev
chiyu1468 2 years ago
parent 06b9df2c0f
commit 0f532701f2
1 changed files with 6 additions and 76 deletions
Show Stats Download Patch File Download Diff File

82
mavone/systemHandler.cpp
Unescape Escape View File

@ -84,69 +84,7 @@ void systemHandler(System& system) {
double _target_lon;
double _target_lat;
//
std::cout << "Dev Test Start" << std::endl;
// Get the info plugin for this drone
auto atvData = system.get_autopilot_version_data();
std::cout << "Autopilot Version Data:" << std::endl;
std::cout << " Vendor ID: " << atvData.vendor_id << std::endl;
std::cout << " Product ID: " << atvData.product_id << std::endl;
std::cout << " Capabilities: " << atvData.capabilities << std::endl;
std::cout << " Flight SW Version: " << atvData.flight_sw_version << std::endl;
std::cout << " Middleware SW Version: " << atvData.middleware_sw_version << std::endl;
std::cout << " OS SW Version: " << atvData.os_sw_version << std::endl;
std::cout << " Board Version: " << atvData.board_version << std::endl;
std::array<uint8_t, 18> uid = atvData.uid2;
std::cout << "UID: ";
for (const auto& byte : uid) {
std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
}
std::cout << std::endl;
const Telemetry::FlightMode mode_result = telemetry.flight_mode();
std::cerr << "Vehicle Mode: " << mode_result << '\n';
sleep_for(milliseconds(1500));
// action.set_takeoff_altitude(10);
// std::cout << "ARM...\n";
// action.arm();
// sleep_for(seconds(1));
// std::cout << "Taking off...\n";
// action.takeoff();
// sleep_for(seconds(10));
// std::cout << "GoTo...\n";
// // auto result_ackFromAction = action.goto_location(24.119289235158828, 120.67216137689361,30,180);
// 測試GOTO
// mavlink_message_t message;
// MavlinkPassthrough::CommandLong command;
// command.target_sysid = sysid;
// command.target_compid = MAV_COMP_ID_AUTOPILOT1; // 這行指 component id = 1
// command.command = MAV_CMD_DO_REPOSITION;
// command.param1 = 5; // Speed
// command.param2 = 0; // IF change to GUILDED mode ?
// command.param3 = 0; // Radius
// command.param4 = 0; // Yaw
// command.param5 = 24.119289235158828; // Latitude
// command.param6 = 120.67216137689361; // Longitude
// command.param5 = 24.11928923; // Latitude
// command.param6 = 120.67216137; // Longitude
// command.param7 = 25; // Altitude
// mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
// mavlink_passthrough.get_our_compid(),
// &message,
// &command);
// mavlink_passthrough.send_command_long(command);
// auto result_ackFromSendMessage = mavlink_passthrough.send_message(message);
// std::cout << " ACK REL: " << result_ackFromSendMessage << std::endl;
// std::cout << "Dev Test Start" << std::endl;
// // 演示一下 如何抓到 parameter
// auto get_result_i = param.get_param_int("VISO_TYPE"); // This function is blocking.
// std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
@ -188,8 +126,7 @@ void systemHandler(System& system) {
// // 看看 has_autopilot() 的值
// std::cout << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug
std::cout << "Dev Test End" << std::endl;
// std::cout << "Dev Test End" << std::endl;
// Put all the subscriber init in this section
if (system.has_autopilot()) {
@ -339,10 +276,10 @@ void systemHandler(System& system) {
<< ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
#ifdef VEHICLE_CONTROL_ACTIVE
mav_command_long.target_sysid = sysid;
mav_command_long.target_compid = MAV_COMP_ID_AUTOPILOT1; // 這行指 component id = 1
mav_command_long.command = 176; // MAV_CMD_DO_SET_MODE
mav_command_long.param1 = 1; // 固定是 1
mav_command_long.param2 = static_cast<int>(oneVehicleCommand.second[0]); // mode
mav_command_long.target_compid = MAV_COMP_ID_AUTOPILOT1; // 這行指 component id = 1
mav_command_long.command = 176; // MAV_CMD_DO_SET_MODE
mav_command_long.param1 = 1; // 固定是 1
mav_command_long.param2 = static_cast<int>(oneVehicleCommand.second[0]); // mode
result_mavpass = mavlink_passthrough.send_command_long(mav_command_long); // 送出指令
std::cout << " ACK REL: " << result_mavpass << std::endl; // dev
@ -393,11 +330,8 @@ void systemHandler(System& system) {
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;
@ -419,7 +353,6 @@ void systemHandler(System& system) {
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];
@ -429,14 +362,11 @@ void systemHandler(System& system) {
}
}
// for purpose
sleep_for(milliseconds(100));
}
// Thread Terminate
std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug

Write Preview
Loading…
Cancel
Save