|
|
|
|
|
#include <mavsdk/mavsdk.h>
|
|
|
#include <mavsdk/plugins/info/info.h>
|
|
|
#include <mavsdk/plugins/param/param.h>
|
|
|
#include <mavsdk/plugins/action/action.h>
|
|
|
#include <mavsdk/plugins/telemetry/telemetry.h>
|
|
|
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
|
|
|
#include <cstdint>
|
|
|
#include <iostream>
|
|
|
#include <fstream>
|
|
|
#include <sstream>
|
|
|
#include <iomanip>
|
|
|
#include <future>
|
|
|
#include <thread>
|
|
|
#include <chrono>
|
|
|
#include <vector>
|
|
|
#include <mutex>
|
|
|
#include <map>
|
|
|
#include <memory>
|
|
|
|
|
|
|
|
|
#include "globals.h"
|
|
|
|
|
|
using namespace mavsdk;
|
|
|
using std::chrono::seconds;
|
|
|
using std::chrono::milliseconds;
|
|
|
using std::this_thread::sleep_for;
|
|
|
|
|
|
std::map<Telemetry::FlightMode, std::string> flightModeMap = {
|
|
|
{Telemetry::FlightMode::Unknown, "Unknown"},
|
|
|
{Telemetry::FlightMode::Takeoff, "Takeoff"},
|
|
|
{Telemetry::FlightMode::Ready, "Ready"},
|
|
|
{Telemetry::FlightMode::Hold, "Hold"},
|
|
|
{Telemetry::FlightMode::ReturnToLaunch, "Return To Launch"},
|
|
|
{Telemetry::FlightMode::Land, "Land"},
|
|
|
{Telemetry::FlightMode::Offboard, "Offboard"},
|
|
|
{Telemetry::FlightMode::FollowMe, "FollowMe"},
|
|
|
{Telemetry::FlightMode::Manual, "Manual"},
|
|
|
{Telemetry::FlightMode::Altctl, "Altctl"},
|
|
|
{Telemetry::FlightMode::Posctl, "Posctl"},
|
|
|
{Telemetry::FlightMode::Acro, "Acro"},
|
|
|
{Telemetry::FlightMode::Stabilized, "Stabilized"},
|
|
|
{Telemetry::FlightMode::Rattitude, "Rattitude"}
|
|
|
};
|
|
|
|
|
|
|
|
|
#define VEHICLE_CONTROL_ACTIVE
|
|
|
|
|
|
|
|
|
/*
|
|
|
* Important multi-threaded functions,
|
|
|
* handling system initialization, receiving messages and sending messages.
|
|
|
*/
|
|
|
void systemHandler(System& system) {
|
|
|
int sysid = static_cast<int>(system.get_system_id());
|
|
|
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " get in Thread. (debug)" << std::endl; // debug
|
|
|
|
|
|
// Initialization
|
|
|
auto mavlink_passthrough = MavlinkPassthrough{system};
|
|
|
auto telemetry = Telemetry{system};
|
|
|
auto action = Action{system};
|
|
|
auto param = Param{system};
|
|
|
|
|
|
|
|
|
// Store telemetry information that will pass to main thread.
|
|
|
std::map<std::string,std::string> telemetryInfo;
|
|
|
|
|
|
// 控制指令 已經被主迴圈轉換過一次
|
|
|
std::map<int, std::array<double, 2>> vehicleCommand;
|
|
|
|
|
|
// 這些變數監控 handler 的行為
|
|
|
gHandlerMask[sysid]["cutoff"] = 0;
|
|
|
gHandlerMask[sysid]["is_connected"] = 1;
|
|
|
bool reset = false;
|
|
|
|
|
|
// 程序執行時會用到的變數
|
|
|
int _counter;
|
|
|
mavlink_message_t mav_message;
|
|
|
mavlink_set_position_target_global_int_t set_position_target_global_int;
|
|
|
MavlinkPassthrough::CommandLong mav_command_long;
|
|
|
MavlinkPassthrough::Result result_mavpass;
|
|
|
Action::Result result_action;
|
|
|
|
|
|
//
|
|
|
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';
|
|
|
|
|
|
|
|
|
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();
|
|
|
// 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;
|
|
|
|
|
|
// // 演示一下 如何抓到 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;
|
|
|
|
|
|
// std::pair<Param::Result, float> get_result_f = {Param::Result::Unknown , 0};
|
|
|
// get_result_f = param.get_param_float("RNGFND_FILT"); // This function is blocking.
|
|
|
// std::cout << "Para Read: " << get_result_f.first << " : " << get_result_f.second << std::endl;
|
|
|
|
|
|
// auto get_result = param.get_all_params(); // This function is blocking.
|
|
|
// auto int_p = get_result.int_params;
|
|
|
// for(const auto& item : int_p) {
|
|
|
// std::cout << item << std::endl;
|
|
|
// }
|
|
|
|
|
|
// // 抓一下 health
|
|
|
// auto health = telemetry.health();
|
|
|
// std::cout << "GYRO: " << (health.is_gyrometer_calibration_ok ? "OK" : "NG") << std::endl;
|
|
|
// std::cout << "Accelerometer: " << (health.is_accelerometer_calibration_ok ? "OK" : "NG") << std::endl;
|
|
|
// std::cout << "Magnetometer: " << (health.is_magnetometer_calibration_ok ? "OK" : "NG") << std::endl;
|
|
|
// std::cout << "Local position NED: " << (health.is_local_position_ok ? "OK" : "NG") << std::endl;
|
|
|
// std::cout << "Global position (Int): " << (health.is_global_position_ok ? "OK" : "NG") << std::endl;
|
|
|
// std::cout << "Home position: " << (health.is_home_position_ok ? "OK" : "NG") << std::endl;
|
|
|
|
|
|
|
|
|
// // 如果對接另一邊也是 mavsdk 這幾個 Parameter 要 provide，這邊讀看看
|
|
|
// get_result_i = {Param::Result::Unknown , 0};
|
|
|
// get_result_i = param.get_param_int("SYS_HITL"); // This function is blocking.
|
|
|
// std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
|
|
|
// get_result_i = {Param::Result::Unknown , 0};
|
|
|
// get_result_i = param.get_param_int("CAL_GYRO0_ID"); // This function is blocking.
|
|
|
// std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
|
|
|
// get_result_i = {Param::Result::Unknown , 0};
|
|
|
// get_result_i = param.get_param_int("CAL_ACC0_ID"); // This function is blocking.
|
|
|
// std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
|
|
|
// get_result_i = {Param::Result::Unknown , 0};
|
|
|
// get_result_i = param.get_param_int("CAL_MAG0_ID"); // This function is blocking.
|
|
|
// std::cout << "Para Read: " << get_result_i.first << " : " << get_result_i.second << std::endl;
|
|
|
|
|
|
// // 看看 has_autopilot() 的值
|
|
|
// std::cout << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug
|
|
|
|
|
|
|
|
|
std::cout << "Dev Test End" << std::endl;
|
|
|
|
|
|
// Put all the subscriber init in this section
|
|
|
if (system.has_autopilot()) {
|
|
|
|
|
|
auto set_rate_result = telemetry.set_rate_position(1.0);
|
|
|
// if (set_rate_result != mavsdk::Telemetry::Result::Success) {
|
|
|
// std::cout << "(systemHandler.cpp:systemHandler) Setting position rate failed:" << set_rate_result << std::endl;
|
|
|
// }
|
|
|
telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
|
|
|
// 轉換數字到字串用的暫存變數
|
|
|
std::ostringstream num_str_ss;
|
|
|
|
|
|
// std::cout << "System ID: " << sysid << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
|
|
|
// Store telemetry information
|
|
|
num_str_ss << std::fixed << std::setprecision(4) << position.relative_altitude_m;
|
|
|
telemetryInfo["drone_alt"] = num_str_ss.str();
|
|
|
num_str_ss.str("");
|
|
|
|
|
|
num_str_ss << std::fixed << std::setprecision(8) << position.latitude_deg;
|
|
|
telemetryInfo["vehicle_lat"] = num_str_ss.str();
|
|
|
num_str_ss.str("");
|
|
|
|
|
|
num_str_ss << std::fixed << std::setprecision(8) << position.longitude_deg;
|
|
|
telemetryInfo["vehicle_lon"] = num_str_ss.str();
|
|
|
num_str_ss.str("");
|
|
|
});
|
|
|
|
|
|
telemetry.subscribe_raw_gps([&system, &telemetryInfo](Telemetry::RawGps rawGps) {
|
|
|
// 轉換數字到字串用的暫存變數
|
|
|
std::ostringstream num_str_ss;
|
|
|
|
|
|
// Store telemetry information
|
|
|
num_str_ss << std::fixed << std::setprecision(4) << rawGps.velocity_m_s;
|
|
|
telemetryInfo["vehicle_speed"] = num_str_ss.str();
|
|
|
num_str_ss.str("");
|
|
|
});
|
|
|
|
|
|
// set_rate_result = telemetry.set_rate_attitude_euler(1.0); // only in v2.0
|
|
|
// if (set_rate_result != mavsdk::Telemetry::Result::Success) {
|
|
|
// std::cout << "(systemHandler.cpp:systemHandler) Setting euler rate failed:" << set_rate_result << std::endl;
|
|
|
// }
|
|
|
telemetry.subscribe_attitude_euler([&system, &telemetryInfo](Telemetry::EulerAngle eulerAngle) {
|
|
|
// 轉換數字到字串用的暫存變數
|
|
|
std::ostringstream num_str_ss;
|
|
|
|
|
|
// Store telemetry information
|
|
|
num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.roll_deg;
|
|
|
telemetryInfo["drone_roll"] = num_str_ss.str();
|
|
|
num_str_ss.str("");
|
|
|
|
|
|
num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.pitch_deg;
|
|
|
telemetryInfo["drone_pitch"] = num_str_ss.str();
|
|
|
num_str_ss.str("");
|
|
|
|
|
|
num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.yaw_deg;
|
|
|
telemetryInfo["drone_yaw"] = num_str_ss.str();
|
|
|
num_str_ss.str("");
|
|
|
});
|
|
|
|
|
|
telemetry.subscribe_heading([&system, &telemetryInfo](Telemetry::Heading heading) {
|
|
|
// 轉換數字到字串用的暫存變數
|
|
|
std::ostringstream num_str_ss;
|
|
|
|
|
|
// Store telemetry information
|
|
|
num_str_ss << std::fixed << std::setprecision(4) << heading.heading_deg;
|
|
|
telemetryInfo["vehicle_head"] = num_str_ss.str();
|
|
|
num_str_ss.str("");
|
|
|
});
|
|
|
|
|
|
set_rate_result = telemetry.set_rate_battery(1.0);
|
|
|
if (set_rate_result != mavsdk::Telemetry::Result::Success) {
|
|
|
std::cout << "(systemHandler.cpp:systemHandler) Setting battery rate failed:" << set_rate_result << std::endl;
|
|
|
}
|
|
|
telemetry.subscribe_battery([&system, &telemetryInfo](Telemetry::Battery battery) {
|
|
|
// 轉換數字到字串用的暫存變數
|
|
|
std::ostringstream num_str_ss;
|
|
|
|
|
|
// Store telemetry information
|
|
|
num_str_ss << std::fixed << std::setprecision(4) << battery.remaining_percent;
|
|
|
telemetryInfo["vehicle_bat_perc"] = num_str_ss.str();
|
|
|
num_str_ss.str("");
|
|
|
num_str_ss << std::fixed << std::setprecision(4) << battery.voltage_v;
|
|
|
telemetryInfo["vehicle_bat_volt"] = num_str_ss.str();
|
|
|
num_str_ss.str("");
|
|
|
});
|
|
|
|
|
|
set_rate_result = telemetry.set_rate_in_air(1.0);
|
|
|
if (set_rate_result != mavsdk::Telemetry::Result::Success) {
|
|
|
std::cout << "(systemHandler.cpp:systemHandler) Setting in_air rate failed:" << set_rate_result << std::endl;
|
|
|
}
|
|
|
telemetry.subscribe_in_air([&system, &telemetryInfo](bool in_air) {
|
|
|
if(in_air){
|
|
|
telemetryInfo["drone_air"] = "1";
|
|
|
}else{
|
|
|
telemetryInfo["drone_air"] = "0";
|
|
|
}
|
|
|
});
|
|
|
|
|
|
telemetry.subscribe_armed([&system, &telemetryInfo](bool is_armed) {
|
|
|
if(is_armed){
|
|
|
telemetryInfo["drone_arm"] = "1";
|
|
|
}else{
|
|
|
telemetryInfo["drone_arm"] = "0";
|
|
|
}
|
|
|
});
|
|
|
|
|
|
telemetry.subscribe_flight_mode([&system, &telemetryInfo](Telemetry::FlightMode fightMode) {
|
|
|
telemetryInfo["vehicle_mode"] = flightModeMap[fightMode];
|
|
|
});
|
|
|
}
|
|
|
|
|
|
// Wait Until telemetryInfo get something. and if nothing get from System for 10 sec will cut off this connection.
|
|
|
_counter = 0;
|
|
|
while(telemetryInfo.empty()){
|
|
|
sleep_for(seconds(1));
|
|
|
if(_counter++ > 10) {
|
|
|
reset = true;
|
|
|
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " waiting too long without any coming info." << std::endl;
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// Loop
|
|
|
while(system.is_connected() & !reset) {
|
|
|
|
|
|
// Send Telemetry Data
|
|
|
gTeleMtx.lock();
|
|
|
gTelemetryInfo[sysid] = telemetryInfo;
|
|
|
gTeleMtx.unlock();
|
|
|
|
|
|
// 取得來自主迴圈的指令 並清空
|
|
|
vehicleCommand = gVehicleCommand[sysid];
|
|
|
gComMtx.lock();
|
|
|
gVehicleCommand[sysid].clear();
|
|
|
gComMtx.unlock();
|
|
|
|
|
|
// Handler Command
|
|
|
reset = (gHandlerMask[sysid]["cutoff"] != 0);
|
|
|
|
|
|
// Vehicle Command
|
|
|
for ( auto& oneVehicleCommand : vehicleCommand) {
|
|
|
switch (oneVehicleCommand.first) {
|
|
|
// Here deal with the function of Change Mode
|
|
|
case 1 :
|
|
|
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command Change Mode , " \
|
|
|
<< "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
|
|
|
<< ", 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
|
|
|
|
|
|
result_mavpass = mavlink_passthrough.send_command_long(mav_command_long); // 送出指令
|
|
|
std::cout << " ACK REL: " << result_mavpass << std::endl; // dev
|
|
|
#endif
|
|
|
break;
|
|
|
|
|
|
// Here deal with the function of Action Arm
|
|
|
case 11 :
|
|
|
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command Arm, " \
|
|
|
<< "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
|
|
|
<< ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
|
|
|
#ifdef VEHICLE_CONTROL_ACTIVE
|
|
|
result_action = action.arm();
|
|
|
std::cout << " ACK REL: " << result_action << std::endl; // dev
|
|
|
#endif
|
|
|
break;
|
|
|
|
|
|
// Here deal with the function of Action Takeoff
|
|
|
case 12 :
|
|
|
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command Takeoff, " \
|
|
|
<< "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
|
|
|
<< ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
|
|
|
#ifdef VEHICLE_CONTROL_ACTIVE
|
|
|
result_action = action.takeoff();
|
|
|
std::cout << " ACK REL: " << result_action << std::endl; // dev
|
|
|
#endif
|
|
|
break;
|
|
|
|
|
|
// Here deal with the function of change height
|
|
|
case 13 :
|
|
|
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command Change Height, " \
|
|
|
<< "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
|
|
|
<< ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
|
|
|
#ifdef VEHICLE_CONTROL_ACTIVE
|
|
|
#endif
|
|
|
break;
|
|
|
|
|
|
// Here deal with the function of GOTO
|
|
|
case 21 :
|
|
|
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Command GoTo, " \
|
|
|
<< "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
|
|
|
result_mavpass = mavlink_passthrough.send_message(mav_message);
|
|
|
std::cout << " ACK REL: " << result_mavpass << std::endl; // dev
|
|
|
#endif
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
|
|
|
// for purpose
|
|
|
sleep_for(milliseconds(100));
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// Thread Terminate
|
|
|
std::cout << "(systemHandler.cpp:systemHandler) Thread Out " << sysid << std::endl; //debug
|
|
|
|
|
|
// Destroy
|
|
|
gHandlerMask[sysid]["is_connected"] = 0;
|
|
|
}
|