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

first command online

Browse Source 
第一個版本
This commit is contained in:
chiyu1468
2024-03-26 23:20:43 +08:00
parent 49752d4626
commit 55d92d4614
12 changed files with 2284 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mavone/globals.cpp
+3 -2
View File
@@ -34,6 +34,7 @@ gHandlerMask
std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
std::map<int, std::map<std::string, std::array<double, 2>>> gVehicleCommand;
std::map<int, std::map<int, std::array<double, 2>>> gVehicleCommand;
std::map<int, std::map<std::string, int>> gHandlerMask;
std::mutex gTeleMtx;
std::mutex gTeleMtx;
std::mutex gComMtx;
mavone/globals.h
+2 -1
View File
@@ -7,8 +7,9 @@
#include <mutex>
extern std::map<int, std::map<std::string, std::string>> gTelemetryInfo;
extern std::map<int, std::map<std::string, std::array<double, 2>>> gVehicleCommand;
extern std::map<int, std::map<int, std::array<double, 2>>> gVehicleCommand;
extern std::map<int, std::map<std::string, int>> gHandlerMask;
extern std::mutex gTeleMtx;
extern std::mutex gComMtx;
#endif
mavone/mavone.cpp
+56 -2
View File
@@ -52,6 +52,44 @@ int initializeParameters(const std::string& filePath, MavInitParameter& initSett
return 0;
}
// 這裡會對 mysql 上的指令 做第一次轉換，再放給 systemHandler 做第二次轉換
std::map<std::string, int> commandTypeMap = {
{"aGuided", 101},
{"aLand", 102},
{"aArm", 103},
{"aSetHeight", 104},
{"aTakeoff", 105},
{"aGoto", 106},
};
std::map<int, std::array<double, 2>> decodeMysqlCommand(std::string commandType, std::string Para1, std::string Para2) {
// std::cout << commandType << ", " << Para1 << ", " << Para2 <<std::endl; // for Dev
std::map<std::string, std::function<void()>> commandMapFunc;
std::map<int, std::array<double, 2>> result;
switch (commandTypeMap[commandType]) {
case 101: // aGuided
result[1] = {4,0}; // means change mode to 4
break;
case 102: // aLand
result[1] = {9,0}; // means change mode to 9
break;
case 103: // aArm
result[11] = {0,0}; // means action arm
break;
case 104: // aSetHeight
result[13] = {std::stof(Para1),0}; // means change height to Para1
break;
case 105: // aTakeoff
result[12] = {0,0}; // means action takeoff
break;
case 106: // aGoto
result[21] = {std::stof(Para1),std::stof(Para2)}; // means goto
break;
}
return result;
}
int main(int argc, char** argv)
{
@@ -226,8 +264,24 @@ int main(int argc, char** argv)
break;
}
// 把 MAVSDK 從無人機接到的 telemetry 放到 mysql
aTelemetryInfo = gTelemetryInfo[handlerInfo.systemID];
// TODO: 把 mysql 的指令放給 MAVSDK 準備傳到無人機
sqlString = "SELECT command_type,command_para1,command_para2 FROM NodeRed_one WHERE SerialNo = " \
+ std::to_string(handlerInfo.mysqlSN) + " AND command_type IS NOT NULL;";
res = stmt->executeQuery(sqlString);
if (res->next()) {
// 有讀到資料的話 就直接更新 gVehicleCommand
gComMtx.lock();
gVehicleCommand[handlerInfo.systemID] = decodeMysqlCommand(res->getString(1), res->getString(2), res->getString(3));
gComMtx.unlock();
// 本來想要把清空 mysql 的 command 欄位跟下面更新 telemetry 放一起 但是這樣會把程式複雜度弄上去 所以算了
sqlString = "UPDATE NodeRed_one SET command_type = NULL, command_para1 = NULL, command_para2 = NULL WHERE SerialNo = " \
+ std::to_string(handlerInfo.mysqlSN) + ";";
stmt->execute(sqlString);
}
// 把 MAVSDK 從無人機接到的 telemetry 放到 mysql
if(aTelemetryInfo.size() != 0) {
sqlString = "UPDATE NodeRed_one SET ";
for(const auto& pair : aTelemetryInfo) {
@@ -242,7 +296,7 @@ int main(int argc, char** argv)
}
// TODO: 把 mysql 的指令放給 MAVSDK 準備傳到無人機
// reset = true; //debug
mavone/mavone.h
+3
View File
@@ -45,7 +45,10 @@ struct MavInitParameter {
enum class systemHandlerState {
Init, // 剛剛交給 systemHandler 去處理
Prologue, // 建立了 mysql row 並取得 serialNo
Transmit, // 正在傳送指令到載具
Ready, // 收到第一筆並放到 telemetryInfo 後
Busy, // 載具正在執行上一次指令
Disconnected, // mavsdk 判斷該系統斷線了
Terminate, // 準備要從管理列表中刪除
// Void
mavone/systemHandler.cpp
+169 -16
View File
@@ -26,30 +26,60 @@ 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) {
// Initialization
auto telemetry = Telemetry{system};
auto action = Action{system};
auto param = Param{system};
int sysid = static_cast<int>(system.get_system_id());
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " get in Thread. (debug)" << std::endl; // debug
// Store telemetry information
std::map<std::string,std::string> telemetryInfo;
// 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;
@@ -75,18 +105,46 @@ void systemHandler(System& system) {
const Telemetry::FlightMode mode_result = telemetry.flight_mode();
std::cerr << "Vehicle Mode: " << mode_result << '\n';
// auto mavlink_passthrough = MavlinkPassthrough{system};
// mavlink_command_long_t command;
// command.target_system = sysid;
// command.target_component = MAV_COMP_ID_AUTOPILOT1;
// command.command = MAV_CMD_REQUEST_MESSAGE;
// command.param1 = 1;
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.
@@ -130,7 +188,7 @@ void systemHandler(System& system) {
// 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()) {
@@ -236,6 +294,10 @@ void systemHandler(System& system) {
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.
@@ -257,10 +319,101 @@ void systemHandler(System& system) {
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));