dev v0.61
為了電池子計畫 硬加了一些東西 不太穩定 懶得詳述了
This commit is contained in:
+1
-1
@@ -299,7 +299,7 @@ int main(int argc, char** argv)
|
||||
}
|
||||
sqlString.pop_back();
|
||||
sqlString += " WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";";
|
||||
std::cout << "MySQL UPDATE Query: " << sqlString << std::endl; //debug
|
||||
// std::cout << "MySQL UPDATE Query: " << sqlString << std::endl; //debug
|
||||
stmt->execute(sqlString);
|
||||
gTelemetryInfo[handlerInfo.systemID] = {};
|
||||
}
|
||||
|
||||
+121
-126
@@ -79,74 +79,39 @@ void systemHandler(System& system) {
|
||||
MavlinkPassthrough::CommandLong mav_command_long;
|
||||
MavlinkPassthrough::Result result_mavpass;
|
||||
Action::Result result_action;
|
||||
int vehicle_type;
|
||||
|
||||
int _counter;
|
||||
double _target_lon;
|
||||
double _target_lat;
|
||||
|
||||
//
|
||||
std::cout << "Dev Test Start" << std::endl;
|
||||
// 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;
|
||||
// // 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;
|
||||
// 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';
|
||||
// const Telemetry::FlightMode mode_result = telemetry.flight_mode();
|
||||
// std::cerr << "Vehicle Mode: " << mode_result << '\n';
|
||||
|
||||
sleep_for(milliseconds(1500));
|
||||
// sleep_for(milliseconds(500));
|
||||
|
||||
|
||||
|
||||
// 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;
|
||||
@@ -189,11 +154,28 @@ 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()) {
|
||||
|
||||
// 用 param 去判斷是什麼種類的 system
|
||||
std::pair< Param::Result, int32_t > param_int_result;
|
||||
|
||||
vehicle_type = 0;
|
||||
|
||||
// 實體機驗證
|
||||
// param_int_result = param.get_param_int("BATT_OPTIONS"); // 都有的
|
||||
param_int_result = param.get_param_int("BRD_SBUS_OUT"); // 只有實體機
|
||||
std::cout << "BRD_SBUS_OUT Para Read: " << param_int_result.first << " : " << param_int_result.second << std::endl; // debug
|
||||
if (param_int_result.first == Param::Result::Success) { vehicle_type = 2; }
|
||||
// 虛擬機驗證
|
||||
param_int_result = param.get_param_int("INS_USE3"); // 只有虛擬機
|
||||
std::cout << "INS_USE3 Para Read: " << param_int_result.first << " : " << param_int_result.second << std::endl; // debug
|
||||
if (param_int_result.first == Param::Result::Success) { vehicle_type = 1; }
|
||||
|
||||
|
||||
// =======================都用的==============================
|
||||
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;
|
||||
@@ -227,28 +209,6 @@ void systemHandler(System& system) {
|
||||
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;
|
||||
@@ -259,9 +219,11 @@ void systemHandler(System& system) {
|
||||
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;
|
||||
if (vehicle_type == 2) {
|
||||
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) {
|
||||
// 轉換數字到字串用的暫存變數
|
||||
@@ -276,18 +238,6 @@ void systemHandler(System& system) {
|
||||
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";
|
||||
@@ -296,42 +246,74 @@ void systemHandler(System& system) {
|
||||
}
|
||||
});
|
||||
|
||||
telemetry.subscribe_flight_mode([&system, &telemetryInfo](Telemetry::FlightMode fightMode) {
|
||||
telemetryInfo["vehicle_mode"] = flightModeMap[fightMode];
|
||||
});
|
||||
|
||||
// 為了分析 statusText 字串的兩個暫存變數
|
||||
std::vector<std::string> statusTextPiece;
|
||||
std::string _token;
|
||||
telemetry.subscribe_status_text([&system, &telemetryInfo, &statusTextPiece, &_token]
|
||||
(Telemetry::StatusText statusText) {
|
||||
// =======================只有無人機==============================
|
||||
if (vehicle_type >= 1) {
|
||||
// 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;
|
||||
// 為了分析 statusText 字串的另一個暫存變數
|
||||
std::istringstream _iss(statusText.text);
|
||||
std::cout << "(systemHandler.cpp:systemHandler) status_text: " << statusText.type << " ; " << statusText.text << std::endl; // debug
|
||||
|
||||
if(statusText.type == Telemetry::StatusTextType::Info) {
|
||||
|
||||
while (std::getline(_iss, _token, ',')) {
|
||||
statusTextPiece.push_back(_token);
|
||||
}
|
||||
|
||||
for (const auto& value : statusTextPiece) {
|
||||
if (value.at(0) == 'T') {
|
||||
num_str_ss << std::fixed << std::setprecision(2) << std::stof(value.substr(1));
|
||||
telemetryInfo["vehicle_bat_Temp"] = num_str_ss.str();
|
||||
num_str_ss.str("");
|
||||
} else if (value.at(0) == 'I') {
|
||||
num_str_ss << std::fixed << std::setprecision(2) << std::stof(value.substr(1));
|
||||
telemetryInfo["vehicle_bat_current"] = num_str_ss.str();
|
||||
num_str_ss.str("");
|
||||
// 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("");
|
||||
});
|
||||
|
||||
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_flight_mode([&system, &telemetryInfo](Telemetry::FlightMode fightMode) {
|
||||
telemetryInfo["vehicle_mode"] = flightModeMap[fightMode];
|
||||
});
|
||||
}
|
||||
|
||||
// =======================只有車子載具==============================
|
||||
|
||||
if (vehicle_type == 0) {
|
||||
telemetry.subscribe_status_text([&system, &telemetryInfo]
|
||||
(Telemetry::StatusText statusText) {
|
||||
// 轉換數字到字串用的暫存變數
|
||||
std::ostringstream num_str_ss;
|
||||
std::istringstream _iss(statusText.text);
|
||||
// 為了分析 statusText 字串的兩個暫存變數
|
||||
std::string _token;
|
||||
|
||||
// std::cout << "(systemHandler.cpp:systemHandler) status_text: " << statusText.type << " ; " << statusText.text << std::endl; // debug
|
||||
if(statusText.type == Telemetry::StatusTextType::Info) {
|
||||
while (std::getline(_iss, _token, ',')) {
|
||||
if (_token.at(0) == 'T') {
|
||||
num_str_ss << std::fixed << std::setprecision(2) << std::stof(_token.substr(1));
|
||||
telemetryInfo["vehicle_bat_Temp"] = num_str_ss.str();
|
||||
num_str_ss.str("");
|
||||
} else if (_token.at(0) == 'I') {
|
||||
num_str_ss << std::fixed << std::setprecision(2) << std::stof(_token.substr(1));
|
||||
telemetryInfo["vehicle_bat_current"] = num_str_ss.str();
|
||||
num_str_ss.str("");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
statusTextPiece.clear();
|
||||
}
|
||||
});
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -348,6 +330,18 @@ void systemHandler(System& system) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
switch (vehicle_type) {
|
||||
case 2:
|
||||
telemetryInfo["vehicle_type"] = "Real";
|
||||
break;
|
||||
case 1:
|
||||
telemetryInfo["vehicle_type"] = "SITL";
|
||||
break;
|
||||
case 0:
|
||||
telemetryInfo["vehicle_type"] = "Self";
|
||||
break;
|
||||
}
|
||||
|
||||
// Loop
|
||||
while(system.is_connected() & !reset) {
|
||||
@@ -367,7 +361,8 @@ void systemHandler(System& system) {
|
||||
reset = (gHandlerMask[sysid]["cutoff"] != 0);
|
||||
|
||||
// Vehicle Command
|
||||
for ( auto& oneVehicleCommand : vehicleCommand) {
|
||||
for ( auto& oneVehicleCommand : vehicleCommand ) {
|
||||
if (vehicle_type == 0) { break; } // 不是飛機不吃指令
|
||||
switch (oneVehicleCommand.first) {
|
||||
case -1 :
|
||||
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Error command_type! " << std::endl;
|
||||
|
||||
Reference in New Issue
Block a user