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

dev v0.61

Browse Source 
為了電池子計畫 硬加了一些東西 不太穩定 懶得詳述了
Dev
chiyu1468 2 years ago
parent f937f89e56
commit 3f9b2ab8e5
4 changed files with 155 additions and 149 deletions
Show Stats Download Patch File Download Diff File

4
Dockerfile
Unescape Escape View File

@ -31,8 +31,8 @@ RUN rm -r /home/mavone/pkg
CMD ["/bin/sh", "-c", "mavone $CONFIG_FILE"] CMD ["/bin/sh", "-c", "mavone $CONFIG_FILE"]
# 測試語句 # 測試語句
# docker run -it --rm -e CONFIG_FILE="/home/mavone/mavone_config.txt" -v C:\_Project\MAVSDK_Dev\configurations:/home/mavone mavone_server:0.42 # docker run -it --rm -e CONFIG_FILE="/home/mavone/mavone_config.txt" -v D:\tryRun:/home/mavone mavone_server:0.60
# docker run -it --rm -e CONFIG_FILE="/home/mavone/mavone_config.txt" -p 14550:14550 -v C:\_Project\MAVSDK_Dev\configurations:/home/mavone mavone_server:0.421 # docker run -it --rm -e CONFIG_FILE="/home/mavone/mavone_config.txt" -p 14550:14550 -v D:\tryRun:/home/mavone mavone_server:0.60
# docker run -it --rm -e CONFIG_FILE="/home/mavone/config1.txt" -p 49304:49304 -v /home/webpage/Mavsdk_DEV/mavone:/home/mavone mavone_server:0.421 # docker run -it --rm -e CONFIG_FILE="/home/mavone/config1.txt" -p 49304:49304 -v /home/webpage/Mavsdk_DEV/mavone:/home/mavone mavone_server:0.421
# 這個方式會是錯的 因為會去執行 host 的環境變數 而不是 container 的環境變數 # 這個方式會是錯的 因為會去執行 host 的環境變數 而不是 container 的環境變數

2
mavone/mavone.cpp
Unescape Escape View File

@ -299,7 +299,7 @@ int main(int argc, char** argv)
} }
sqlString.pop_back(); sqlString.pop_back();
sqlString += " WHERE SerialNO = " + std::to_string(handlerInfo.mysqlSN) + ";"; 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); stmt->execute(sqlString);
gTelemetryInfo[handlerInfo.systemID] = {}; gTelemetryInfo[handlerInfo.systemID] = {};
} }

253
mavone/systemHandler.cpp
Unescape Escape View File

@ -79,73 +79,38 @@ void systemHandler(System& system) {
MavlinkPassthrough::CommandLong mav_command_long; MavlinkPassthrough::CommandLong mav_command_long;
MavlinkPassthrough::Result result_mavpass; MavlinkPassthrough::Result result_mavpass;
Action::Result result_action; Action::Result result_action;
int vehicle_type;
int _counter; int _counter;
double _target_lon; double _target_lon;
double _target_lat; double _target_lat;
// //
std::cout << "Dev Test Start" << std::endl; // std::cout << "Dev Test Start" << std::endl;
// Get the info plugin for this drone // // Get the info plugin for this drone
auto atvData = system.get_autopilot_version_data(); // auto atvData = system.get_autopilot_version_data();
std::cout << "Autopilot Version Data:" << std::endl; // std::cout << "Autopilot Version Data:" << std::endl;
std::cout << " Vendor ID: " << atvData.vendor_id << std::endl; // std::cout << " Vendor ID: " << atvData.vendor_id << std::endl;
std::cout << " Product ID: " << atvData.product_id << std::endl; // std::cout << " Product ID: " << atvData.product_id << std::endl;
std::cout << " Capabilities: " << atvData.capabilities << std::endl; // std::cout << " Capabilities: " << atvData.capabilities << std::endl;
std::cout << " Flight SW Version: " << atvData.flight_sw_version << 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 << " Middleware SW Version: " << atvData.middleware_sw_version << std::endl;
std::cout << " OS SW Version: " << atvData.os_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::cout << " Board Version: " << atvData.board_version << std::endl;
std::array<uint8_t, 18> uid = atvData.uid2; // std::array<uint8_t, 18> uid = atvData.uid2;
std::cout << "UID: "; // std::cout << "UID: ";
for (const auto& byte : uid) { // for (const auto& byte : uid) {
std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " "; // std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(byte) << " ";
} // }
std::cout << std::endl; // std::cout << std::endl;
const Telemetry::FlightMode mode_result = telemetry.flight_mode(); // const Telemetry::FlightMode mode_result = telemetry.flight_mode();
std::cerr << "Vehicle Mode: " << mode_result << '\n'; // 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 // // 演示一下 如何抓到 parameter
// auto get_result_i = param.get_param_int("VISO_TYPE"); // This function is blocking. // auto get_result_i = param.get_param_int("VISO_TYPE"); // This function is blocking.
@ -189,11 +154,28 @@ void systemHandler(System& system) {
// std::cout << "Has AutoPilot : " << system.has_autopilot() << std::endl; // debug // 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 // Put all the subscriber init in this section
if (system.has_autopilot()) { 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); auto set_rate_result = telemetry.set_rate_position(1.0);
// if (set_rate_result != mavsdk::Telemetry::Result::Success) { // if (set_rate_result != mavsdk::Telemetry::Result::Success) {
// std::cout << "(systemHandler.cpp:systemHandler) Setting position rate failed:" << set_rate_result << std::endl; // 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(""); 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) { telemetry.subscribe_heading([&system, &telemetryInfo](Telemetry::Heading heading) {
// 轉換數字到字串用的暫存變數 // 轉換數字到字串用的暫存變數
std::ostringstream num_str_ss; std::ostringstream num_str_ss;
@ -259,9 +219,11 @@ void systemHandler(System& system) {
num_str_ss.str(""); num_str_ss.str("");
}); });
set_rate_result = telemetry.set_rate_battery(1.0); if (vehicle_type == 2) {
if (set_rate_result != mavsdk::Telemetry::Result::Success) { set_rate_result = telemetry.set_rate_battery(1.0);
std::cout << "(systemHandler.cpp:systemHandler) Setting battery rate failed:" << set_rate_result << std::endl; 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) { telemetry.subscribe_battery([&system, &telemetryInfo](Telemetry::Battery battery) {
// 轉換數字到字串用的暫存變數 // 轉換數字到字串用的暫存變數
@ -276,18 +238,6 @@ void systemHandler(System& system) {
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) { telemetry.subscribe_armed([&system, &telemetryInfo](bool is_armed) {
if(is_armed){ if(is_armed){
telemetryInfo["drone_arm"] = "1"; 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]; 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) {
// 為了分析 statusText 字串的兩個暫存變數 // std::cout << "(systemHandler.cpp:systemHandler) Setting euler rate failed:" << set_rate_result << std::endl;
std::vector<std::string> statusTextPiece; // }
std::string _token; telemetry.subscribe_attitude_euler([&system, &telemetryInfo](Telemetry::EulerAngle eulerAngle) {
telemetry.subscribe_status_text([&system, &telemetryInfo, &statusTextPiece, &_token]
(Telemetry::StatusText statusText) {
// 轉換數字到字串用的暫存變數 // 轉換數字到字串用的暫存變數
std::ostringstream num_str_ss; 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) { // 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("");
while (std::getline(_iss, _token, ',')) { num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.pitch_deg;
statusTextPiece.push_back(_token); telemetryInfo["drone_pitch"] = num_str_ss.str();
} num_str_ss.str("");
for (const auto& value : statusTextPiece) { num_str_ss << std::fixed << std::setprecision(4) << eulerAngle.yaw_deg;
if (value.at(0) == 'T') { telemetryInfo["drone_yaw"] = num_str_ss.str();
num_str_ss << std::fixed << std::setprecision(2) << std::stof(value.substr(1)); num_str_ss.str("");
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("");
}
}
statusTextPiece.clear(); 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("");
}
}
}
});
}
} }
@ -349,6 +331,18 @@ void systemHandler(System& system) {
} }
} }
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 // Loop
while(system.is_connected() & !reset) { while(system.is_connected() & !reset) {
@ -367,7 +361,8 @@ void systemHandler(System& system) {
reset = (gHandlerMask[sysid]["cutoff"] != 0); reset = (gHandlerMask[sysid]["cutoff"] != 0);
// Vehicle Command // Vehicle Command
for ( auto& oneVehicleCommand : vehicleCommand) { for ( auto& oneVehicleCommand : vehicleCommand ) {
if (vehicle_type == 0) { break; } // 不是飛機不吃指令
switch (oneVehicleCommand.first) { switch (oneVehicleCommand.first) {
case -1 : case -1 :
std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Error command_type! " << std::endl; std::cout << "(systemHandler.cpp:systemHandler) System " << sysid << " Get Error command_type! " << std::endl;

41
unit function dev/test3_vel_gps_bat_head.py
Unescape Escape View File

@ -14,6 +14,17 @@ async def run():
print("123") print("123")
await drone.telemetry_server.publish_status_text(StatusText(StatusTextType.ALERT,"Check")) await drone.telemetry_server.publish_status_text(StatusText(StatusTextType.ALERT,"Check"))
# await drone.param_server.provide_param_int("CAL_ACC0_ID",1)
# await drone.param_server.provide_param_int("CAL_GYRO0_ID",1)
# await drone.param_server.provide_param_int("CAL_MAG0_ID",1)
# await drone.param_server.provide_param_int("SYS_HITL",0)
# await drone.param_server.provide_param_int("MIS_TAKEOFF_ALT",0)
a1 = drone.param_server.IntParam("CAL_ACC0_ID",1)
a2 = drone.param_server.IntParam("CAL_GYRO0_ID",1)
a3 = drone.param_server.IntParam("CAL_MAG0_ID",1)
a4 = drone.param_server.IntParam("SYS_HITL",0)
a5 = drone.param_server.IntParam("MIS_TAKEOFF_ALT",0)
while True: while True:
@ -30,21 +41,21 @@ async def run():
heading = Heading(heading_deg) heading = Heading(heading_deg)
raw_gps = RawGps( raw_gps = RawGps(
timestamp_us=123456789, # 当前时间戳，以微秒为单位 timestamp_us=123456789, # 当前时间戳，以微秒为单位
latitude_deg=47.3977418, # 纬度 latitude_deg=47.3977418, # 纬度
longitude_deg=8.5455934, # 经度 longitude_deg=8.5455934, # 经度
absolute_altitude_m=500, # 绝对海拔高度（米） absolute_altitude_m=500, # 绝对海拔高度（米）
hdop=1.0, # 水平精度因子 hdop=1.0, # 水平精度因子
vdop=1.0, # 垂直精度因子 vdop=1.0, # 垂直精度因子
velocity_m_s=random.uniform(0.0,50.0), # 速度（米/秒） velocity_m_s=random.uniform(0.0,50.0), # 速度（米/秒）
cog_deg=90.0, # 航向（度） cog_deg=90.0, # 航向（度）
altitude_ellipsoid_m=510.0, # 大地水准面上的高度（米） altitude_ellipsoid_m=510.0, # 大地水准面上的高度（米）
horizontal_uncertainty_m=1.000000000000, # 水平准确性（米） horizontal_uncertainty_m=1.000000000000, # 水平准确性（米）
vertical_uncertainty_m=1.00000000, vertical_uncertainty_m=1.00000000,
velocity_uncertainty_m_s=0.0000000000000000 ,# 垂直准确性（米） velocity_uncertainty_m_s=0.0000000000000000 ,# 垂直准确性（米）
heading_uncertainty_deg=1.0000000, # 速度准确性（米/秒） heading_uncertainty_deg=1.0000000, # 速度准确性（米/秒）
yaw_deg=180.5# 是否已定位的标志 yaw_deg=180.5# 是否已定位的标志
) )
gps_info = GpsInfo( gps_info = GpsInfo(
num_satellites=10, # 卫星数量 num_satellites=10, # 卫星数量
fix_type=FixType.RTK_FIXED # GPS 修复类型 fix_type=FixType.RTK_FIXED # GPS 修复类型

Write Preview
Loading…
Cancel
Save