@ -2,43 +2,69 @@
// Simple example to demonstrate how takeoff and land using MAVSDK.
// Simple example to demonstrate how takeoff and land using MAVSDK.
//
//
# include <chrono>
# include <cstdint>
# include <mavsdk/mavsdk.h>
# include <mavsdk/mavsdk.h>
# include <mavsdk/plugins/action/action.h>
# include <mavsdk/plugins/action/action.h>
# include <mavsdk/plugins/telemetry/telemetry.h>
# include <mavsdk/plugins/telemetry/telemetry.h>
# include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
# include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
# include <cstdint>
# include <iostream>
# include <iostream>
# include <fstream>
# include <fstream>
# include <sstream>
# include <sstream>
# include <iomanip>
# include <future>
# include <future>
# include <memory>
# include <thread>
# include <thread>
# include <chrono>
# include <vector>
# include <vector>
# include <mutex>
# include <map>
# include <map>
# include <memory>
# include <mysql_driver.h>
# include <mysql_connection.h>
# include <cppconn/resultset.h>
# include <cppconn/statement.h>
using namespace mavsdk ;
using namespace mavsdk ;
using std : : chrono : : seconds ;
using std : : chrono : : seconds ;
using std : : chrono : : milliseconds ;
using std : : this_thread : : sleep_for ;
using std : : this_thread : : sleep_for ;
struct MavInitParameter {
struct MavInitParameter {
std : : string connectPort ;
std : : string connectPort ;
std : : string connectPort2 ;
std : : string connectPort2 ;
int B ;
float C ;
float C ;
std : : string mysqlHost ;
std : : string mysqlPort ;
std : : string mysqlUser ;
std : : string mysqlPW ;
std : : string mysqlDatabase ;
} ;
enum class systemHandlerState {
Init , // 剛剛交給 systemHandler 去處理
Prologue , // 建立了 mysql row 並取得 serialNo
Ready // 收到第一筆並放到 telemetryInfo 後
} ;
} ;
struct systemHandlerInfo {
struct systemHandlerInfo {
int systemID ;
int systemID ;
std : : thread systemThread ;
std : : thread systemThread ;
systemHandlerState handlerState ;
int mysqlSN ;
} ;
} ;
// 我無法解決跨執行緒的 promise 變數生命週期問題
// 我無法解決跨執行緒的 promise 變數生命週期問題
// 用最智障的全域變數 (砍掉 promise 結構簡單好多)
// 用最智障的全域變數 (砍掉 promise 結構簡單好多)
std : : map < int , std : : map < std : : string , float > > gTelemetryInfo ;
std : : map < int , std : : map < std : : string , std : : string > > gTelemetryInfo ;
std : : mutex gTeleMtx ;
bool reset ;
int initializeParameters ( const std : : string & filePath , MavInitParameter & initSetting ) {
int initializeParameters ( const std : : string & filePath , MavInitParameter & initSetting ) {
@ -64,8 +90,16 @@ int initializeParameters(const std::string& filePath, MavInitParameter& initSett
initSetting . connectPort = value ;
initSetting . connectPort = value ;
} else if ( key = = " connectPort2 " ) {
} else if ( key = = " connectPort2 " ) {
initSetting . connectPort2 = value ;
initSetting . connectPort2 = value ;
} else if ( key = = " B " ) {
} else if ( key = = " mysqlHost " ) {
initSetting . B = std : : stoi ( value ) ;
initSetting . mysqlHost = value ;
} else if ( key = = " mysqlPort " ) {
initSetting . mysqlPort = value ;
} else if ( key = = " mysqlUser " ) {
initSetting . mysqlUser = value ;
} else if ( key = = " mysqlPW " ) {
initSetting . mysqlPW = value ;
} else if ( key = = " mysqlDatabase " ) {
initSetting . mysqlDatabase = value ;
} else if ( key = = " C " ) {
} else if ( key = = " C " ) {
initSetting . C = std : : stof ( value ) ;
initSetting . C = std : : stof ( value ) ;
}
}
@ -92,79 +126,133 @@ void systemHandler(System& system) {
std : : cout < < " System " < < static_cast < int > ( system . get_system_id ( ) ) < < " get in Thread. (debug) " < < std : : endl ; // debug
std : : cout < < " System " < < static_cast < int > ( system . get_system_id ( ) ) < < " get in Thread. (debug) " < < std : : endl ; // debug
// Store telemetry information
// Store telemetry information
std : : map < std : : string , float > telemetryInfo ;
std : : map < std : : string , std : : string > telemetryInfo ;
// Put all the subscriber init in this section
// Put all the subscriber init in this section
telemetry . subscribe_position ( [ & system , & telemetryInfo ] ( Telemetry : : Position position ) {
telemetry . subscribe_position ( [ & system , & telemetryInfo ] ( Telemetry : : Position position ) {
std : : cout < < " System ID: " < < static_cast < int > ( system . get_system_id ( ) ) < < " Altitude: " < < position . relative_altitude_m < < " m \n " ; // debug
// 轉換數字到字串用的暫存變數
std : : ostringstream num_str_ss ;
// std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
// Store telemetry information
// Store telemetry information
telemetryInfo [ " altitude " ] = position . relative_altitude_m ;
num_str_ss < < std : : fixed < < std : : setprecision ( 4 ) < < position . relative_altitude_m ;
gTelemetryInfo [ static_cast < int > ( system . get_system_id ( ) ) ] = telemetryInfo ;
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 ( " " ) ;
} ) ;
} ) ;
// Loop
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 ( " " ) ;
} ) ;
// Destroy
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 ( " " ) ;
sleep_for ( seconds ( 5 ) ) ;
num_str_ss < < std : : fixed < < std : : setprecision ( 4 ) < < eulerAngle . pitch_deg ;
// Thread Terminate
telemetryInfo [ " drone_pitch " ] = num_str_ss . str ( ) ;
std : : cout < < " Thread Out " < < static_cast < int > ( system . get_system_id ( ) ) < < std : : endl ;
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 ( " " ) ;
} ) ;
// class systemHandler {
telemetry . subscribe_heading ( [ & system , & telemetryInfo ] ( Telemetry : : Heading heading ) {
// 轉換數字到字串用的暫存變數
std : : ostringstream num_str_ss ;
// private:
// Store telemetry information
// Telemetry telemetry;
num_str_ss < < std : : fixed < < std : : setprecision ( 4 ) < < heading . heading_deg ;
// Action action;
telemetryInfo [ " vehicle_head " ] = num_str_ss . str ( ) ;
num_str_ss . str ( " " ) ;
} ) ;
telemetry . subscribe_battery ( [ & system , & telemetryInfo ] ( Telemetry : : Battery battery ) {
// 轉換數字到字串用的暫存變數
std : : ostringstream num_str_ss ;
// public:
// Store telemetry information
// std::map<std::string,float> telemetryInfo;
num_str_ss < < std : : fixed < < std : : setprecision ( 4 ) < < battery . remaining_percent ;
// std::map<std::string,float> actionCommand;
telemetryInfo [ " vehicle_bat " ] = num_str_ss . str ( ) ;
num_str_ss . str ( " " ) ;
} ) ;
// // Constructor
telemetry . subscribe_in_air ( [ & system , & telemetryInfo ] ( bool in_air ) {
// systemHandler(System& system) {
if ( in_air ) {
// // Initialization
telemetryInfo [ " drone_air " ] = " 1 " ;
// telemetry = Telemetry{system};
} else {
// action = Action{system};
telemetryInfo [ " drone_air " ] = " 0 " ;
// std::cout << "System " << static_cast<int>(system.get_system_id()) << " captured. (debug)" << std::endl; // debug
}
} ) ;
telemetry . subscribe_armed ( [ & system , & telemetryInfo ] ( bool is_armed ) {
if ( is_armed ) {
telemetryInfo [ " drone_arm " ] = " 1 " ;
} else {
telemetryInfo [ " drone_arm " ] = " 0 " ;
}
} ) ;
// Wait Until telemetryInfo get something.
while ( telemetryInfo . empty ( ) ) {
sleep_for ( seconds ( 1 ) ) ;
}
// telemetry.subscribe_position([&system, &telemetryInfo](Telemetry::Position position) {
// Loop
// std::cout << "System ID: " << static_cast<int>(system.get_system_id()) << " Altitude: " << position.relative_altitude_m << " m\n"; // debug
while ( system . is_connected ( ) & & ! reset ) {
// // Store telemetry information
// Send Telemetry Data
// telemetryInfo["altitude"] = position.relative_altitude_m;
gTeleMtx . lock ( ) ;
// });
gTelemetryInfo [ static_cast < int > ( system . get_system_id ( ) ) ] = telemetryInfo ;
gTeleMtx . unlock ( ) ;
// }
// Deal Command
// for purpose
sleep_for ( milliseconds ( 100 ) ) ;
}
// // Loop
// Destroy
// void operator()() {
// std::cout << "Thread is running" << std::endl;
// }
// }
// Thread Terminate
std : : cout < < " Thread Out " < < static_cast < int > ( system . get_system_id ( ) ) < < std : : endl ; //debug
}
int main ( int argc , char * * argv )
int main ( int argc , char * * argv )
{
{
reset = false ;
// if (argc != 2) {
if ( argc ! = 2 ) {
// std::cout << "Config File Needed! ex. config.txt" << std::endl;
std : : cout < < " Config File Needed! ex. config.txt " < < std : : endl ;
// return 1;
return 1 ;
// }
}
// Program Initial Read Setting from config file
// Program Initial Read Setting from config file
struct MavInitParameter initSetting ;
struct MavInitParameter initSetting ;
// auto i = initializeParameters(argv[1], initSetting);
auto i = initializeParameters ( argv [ 1 ] , initSetting ) ;
auto i = initializeParameters ( " config.txt " , initSetting ) ;
// auto i = initializeParameters("config.txt", initSetting);
if ( i = = 1 ) {
if ( i = = 1 ) {
std : : cerr < < " Failed to open config file: " < < argv [ 1 ] < < std : : endl ;
std : : cerr < < " Failed to open config file: " < < argv [ 1 ] < < std : : endl ;
}
}
@ -199,241 +287,156 @@ int main(int argc, char** argv)
}
}
// make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
// make std::shared_ptr<MAVSDK::System> back to MAVSDK::System
System & new_system = * sys ;
System & new_system = * sys ;
std : : cout < < " system detect : " < < static_cast < int > ( new_system . get_system_id ( ) ) < < " (Debug) " < < std : : endl ; // debug
// std::cout << "system detect : " << static_cast<int>(new_system.get_system_id()) << "(Debug)"<< std::endl; // debug
// Let handler progrem deal with System
// Let handler progrem deal with System
std : : thread systemHandleThread ( [ & new_system ] ( ) { systemHandler ( new_system ) ; } ) ;
std : : thread systemHandleThread ( [ & new_system ] ( ) { systemHandler ( new_system ) ; } ) ;
// Need some extra time fpr complete init
sleep_for ( milliseconds ( 500 ) ) ;
systemHandlerInfos . push_back ( systemHandlerInfo {
systemHandlerInfos . push_back ( systemHandlerInfo {
static_cast < int > ( new_system . get_system_id ( ) ) ,
static_cast < int > ( new_system . get_system_id ( ) ) ,
std : : move ( systemHandleThread ) ,
std : : move ( systemHandleThread ) ,
systemHandlerState : : Init ,
- 1
} ) ;
} ) ;
} ) ;
} ) ;
// MySQL connection is here
// MySQL connection is here
sql : : mysql : : MySQL_Driver * driver ;
sql : : Connection * con ;
sql : : Statement * stmt ;
sql : : ResultSet * res ;
try {
driver = sql : : mysql : : get_mysql_driver_instance ( ) ;
con = driver - > connect ( " tcp:// " + initSetting . mysqlHost + " : " + initSetting . mysqlPort , initSetting . mysqlUser , initSetting . mysqlPW ) ;
con - > setSchema ( initSetting . mysqlDatabase ) ;
stmt = con - > createStatement ( ) ;
} catch ( sql : : SQLException & e ) {
std : : cout < < " MySQL Connection Error: " < < e . what ( ) < < std : : endl ;
return 1 ;
}
// Main Loop is here
// Main Loop is here
// -- 將這個服務目前的狀態 更新到 mysql 中
// -- 將這個服務目前的狀態 更新到 mysql 中
// -- 從 systemHandlerInfos 中 輪巡所有 system 檢查並處理是否有需要丟到 mysql 的資料
// -- 從 systemHandlerInfos 中 輪巡所有 system 檢查並處理是否有需要丟到 mysql 的資料
// -- 從 mysql 抓到指令 丟到 systemHandlerInfo
// -- 從 mysql 抓到指令 丟到 systemHandlerInfo
int randomValue ;
std : : string sqlString ;
std : : map < std : : string , std : : string > aTelemetryInfo ;
while ( ! reset ) {
sleep_for ( seconds ( 1 ) ) ; // debug
// 每個 System 輪詢過去
for ( auto & handlerInfo : systemHandlerInfos ) {
switch ( handlerInfo . handlerState ) {
case systemHandlerState : : Init :
std : : cout < < " SystemHandler: " < < std : : to_string ( handlerInfo . systemID ) < < " at Init " < < std : : endl ; //debug
// 用系統時間生成的隨機數 標記目前欄位
std : : srand ( std : : time ( 0 ) ) ;
randomValue = std : : rand ( ) % 10000 ;
sqlString = " INSERT INTO NodeRed_one (system_id, connect_state) VALUES ( " + std : : to_string ( handlerInfo . systemID ) + " , " + std : : to_string ( randomValue ) + " ); " ;
// std::cout << "MySQL Insert Query: " << sqlString << std::endl; //debug
stmt - > execute ( sqlString ) ;
// 取得對應的欄位序號 SerialNO
sqlString = " SELECT SerialNO FROM NodeRed_one WHERE system_id = " + std : : to_string ( handlerInfo . systemID ) + " AND connect_state = " + std : : to_string ( randomValue ) + " ; " ;
// std::cout << "MySQL SELECT Query: " << sqlString << std::endl; //debug
res = stmt - > executeQuery ( sqlString ) ;
if ( res - > next ( ) ) {
// std::cout << "Query Reselt: " << res->getString(1) << std::endl; //debug
handlerInfo . mysqlSN = std : : stof ( res - > getString ( 1 ) ) ;
}
// 切換到下一個狀態
handlerInfo . handlerState = systemHandlerState : : Prologue ;
break ;
case systemHandlerState : : Prologue :
std : : cout < < " SystemHandler: " < < std : : to_string ( handlerInfo . systemID ) < < " at Prologue " < < std : : endl ; //debug
// 確認之前的 handlerInfo.mysqlSN 有正常被取得 若沒有整個程式即將關閉
if ( handlerInfo . mysqlSN = = - 1 ) {
reset = true ;
std : : cout < < " handlerInfo.mysqlSN not aquired. Something goes wrong. Shutdown all service. " < < std : : endl ;
}
// MAVSDK 還沒收到第一筆 telemetry 就持續在這個狀態
aTelemetryInfo = gTelemetryInfo [ handlerInfo . systemID ] ;
if ( aTelemetryInfo . size ( ) = = 0 ) {
break ;
}
// 切換到下一個狀態
handlerInfo . handlerState = systemHandlerState : : Ready ;
case systemHandlerState : : Ready :
std : : cout < < " SystemHandler: " < < std : : to_string ( handlerInfo . systemID ) < < " at Ready " < < std : : endl ; //debug
// 把 MAVSDK 從無人機接到的 telemetry 放到 mysql
aTelemetryInfo = gTelemetryInfo [ handlerInfo . systemID ] ;
if ( aTelemetryInfo . size ( ) ! = 0 ) {
sqlString = " UPDATE NodeRed_one SET " ;
for ( const auto & pair : aTelemetryInfo ) {
// std::cout << "Key: " << pair.first << ", Value: " << pair.second << std::endl; //debug
sqlString + = pair . first + " = ' " + pair . second + " ', " ;
}
sqlString . pop_back ( ) ;
sqlString + = " WHERE SerialNO = " + std : : to_string ( handlerInfo . mysqlSN ) + " ; " ;
// std::cout << "MySQL UPDATE Query: " << sqlString << std::endl; //debug
stmt - > execute ( sqlString ) ;
gTelemetryInfo [ handlerInfo . systemID ] = { } ;
}
// TODO: 把 mysql 的指令放給 MAVSDK 準備傳到無人機
// reset = true; //debug
break ;
}
sleep_for ( seconds ( 3 ) ) ;
}
std : : cout < < " mark A " < < gTelemetryInfo [ 1 ] [ " altitude " ] < < std : : endl ;
// make all thread join
for ( auto & info : systemHandlerInfos ) {
info . systemThread . join ( ) ;
}
}
// ==============================================================================
// // dev del S
// mavsdk.subscribe_on_new_system([&mavsdk]() {
// auto system = mavsdk.systems().back();
// if (system->has_autopilot()) {
// std::cout << "Discovered autopilot\n";
// }
// });
// if (connection_result != ConnectionResult::Success) {
// std::cerr << "Connection failed: " << connection_result << '\n';
// return 1;
// }
// auto system = get_system(mavsdk);
// if (!system) {
// return 1;
// }
// for (auto system : mavsdk.systems()) {
// sleep_for(seconds(3));
// std::cout << "(mavsdk.systems) get id : " << static_cast<int>(system->get_system_id()) << std::endl;
// std::cout << "connect system number: " << gTelemetryInfo.size() << std::endl;
// for (auto systemHandler : systemHandlers) {
// for(int t=0;t<10;t++){
// std::cout << systemHandler.systemID << " Mark B" << std::endl;
// for(int i = 0; i < gTelemetryInfo.size()-1 ; i++){
// if (systemHandler.systemID == static_cast<int>(system->get_system_id())) {
// auto& info = gTelemetryInfo[i];
// std::cout << "Mark C" << std::endl;
// std::cout << "Sysid : " << i+1 << " ,Alt : " << info["drone_alt"] << " ,Lon : " << info["vehicle_lon"] << " ,Lat : " << info["vehicle_lat"] << std::endl;
// // telemetry = Telemetry{system};
// const auto set_rate_result = telemetry.set_rate_position(1);
// if (set_rate_result != Telemetry::Result::Success) {
// std::cerr << "Setting rate failed: " << set_rate_result << '\n';
// return 1;
// }
// telemetry.subscribe_position([systemHandler](Telemetry::Position position) {
// std::cout << "System ID: " << systemHandler.systemID << " Altitude: " << position.relative_altitude_m << " m\n";
// });
// // // Check until vehicle is ready to arm
// // while (telemetry.health_all_ok() != true) {
// // std::cout << "System ID: " << systemHandler.systemID << " Vehicle is getting ready to arm\n";
// // sleep_for(seconds(1));
// // }
// // // Arm vehicle
// // std::cout << "System ID: " << systemHandler.systemID << " Arming...\n";
// // const Action::Result arm_result = action.arm();
// // if (arm_result != Action::Result::Success) {
// // std::cerr << "System ID: " << systemHandler.systemID << " Arming failed: " << arm_result << '\n';
// // return 1;
// // }
// // telemetrys.push_back(telemetry);
// sleep_for(seconds(5));
// } else {
// std::cout << " Mark D " << std::endl;
// }
// }
// sleep_for(seconds(1));
// }
// }
// }
// std::cout << typeid().name() << std::endl;
// return 1;
// Instantiate plugins.
// auto telemetry = Telemetry{systemsList[0]};
// auto telemetry1 = Telemetry{systemsList[1]};
// // Instantiate plugins.
// auto telemetry = Telemetry{system};
// auto action = Action{system};
// // Instantiate mavlink_passthrough plugin.
// auto mavlink_passthrough = MavlinkPassthrough{system};
// auto x = static_cast<int>(system->get_system_id());
// std::cout << "System ID : " << x << '\n';
// System::AutopilotVersion y = system->get_autopilot_version_data();
// // dev del E
// std::cout << "MAV_PROTOCOL_CAPABILITY : " << y.capabilities << '\n';
// std::cout << "Flight Stack Version : " << y.flight_sw_version << '\n';
// // Check vehicle mode
// const Telemetry::FlightMode mode_result = telemetry.flight_mode();
// std::cerr << "Vehicle Mode: " << mode_result << '\n';
// // return 1; // debug
// // Set the flight mode to "AUTO". use SET_MODE (#11) in MAVLink Messages.
// mavlink_message_t message;
// // mavlink_msg_set_mode_pack(mavlink_passthrough.get_our_sysid(),
// // mavlink_passthrough.get_our_compid(),
// // &message,
// // system->get_system_id(),
// // MAV_COMP_ID_AUTOPILOT1,
// // 4);
// mavlink_command_long_t command;
// When Progrem Terminate
// command.target_system = system->get_system_id();
reset = true ;
// command.target_component = MAV_COMP_ID_AUTOPILOT1;
// command.command = MAV_CMD_DO_SET_MODE;
// command.param1 = MAV_MODE_AUTO_ARMED;
// mavlink_msg_command_long_encode(mavlink_passthrough.get_our_sysid(),
// mavlink_passthrough.get_our_compid(),
// &message,
// &command);
// mavlink_passthrough.send_message(message);
// make all thread join
for ( auto & handlerInfo : systemHandlerInfos ) {
handlerInfo . systemThread . join ( ) ;
// return 1; // debug
}
// We want to listen to the altitude of the drone at 1 Hz.
// const auto set_rate_result = telemetry0.set_rate_position(1);
// if (set_rate_result != Telemetry::Result::Success) {
// std::cerr << "Setting rate failed: " << set_rate_result << '\n';
// return 1;
// }
// const auto set_rate_result = telemetry1.set_rate_position(1);
// if (set_rate_result != Telemetry::Result::Success) {
// std::cerr << "Setting rate failed: " << set_rate_result << '\n';
// return 1;
// }
// Set up callback to monitor altitude while the vehicle is in flight
// auto x = systemsList[0]->get_system_id();
// telemetry.subscribe_position([x](Telemetry::Position position) {
// std::cout << "System ID: " << x << "Altitude: " << position.relative_altitude_m << " m\n";
// });
// std::cout << "Mark C" << std::endl;
// for(int i = 0; i < 5; ++i) {
// sleep_for(seconds(5));
// std::cout << "Number of discovered systems: " << systemsList.size() << std::endl;
// for (auto system : mavsdk.systems()) {
// std::cout << "Found system with system ID: " << static_cast<int>(system->get_system_id())
// << ", connected: " << (system->is_connected() ? "yes" : "no") << '\n';
// }
// std::cout << "link check " << i << " time. \n";
// }
// // Check until vehicle is ready to arm
// while (telemetry.health_all_ok() != true) {
// std::cout << "Vehicle is getting ready to arm\n";
// sleep_for(seconds(1));
// }
// // Arm vehicle
// std::cout << "Arming...\n";
// const Action::Result arm_result = action.arm();
// if (arm_result != Action::Result::Success) {
// std::cerr << "Arming failed: " << arm_result << '\n';
// return 1;
// }
// return 1; // debug
// // Take off
// std::cout << "Taking off...\n";
// const Action::Result takeoff_result = action.takeoff();
// if (takeoff_result != Action::Result::Success) {
// std::cerr << "Takeoff failed: " << takeoff_result << '\n';
// return 1;
// }
// Let it hover for a bit before landing again.
sleep_for ( seconds ( 5 ) ) ;
// std::cout << "Landing...\n";
// const Action::Result land_result = action.land();
// if (land_result != Action::Result::Success) {
// std::cerr << "Land failed: " << land_result << '\n';
// return 1;
// }
// // Check if vehicle is still in air
// while (telemetry.in_air()) {
// std::cout << "Vehicle is landing...\n";
// sleep_for(seconds(1));
// }
// std::cout << "Landed!\n";
// // We are relying on auto-disarming but let's keep watching the telemetry for a bit longer.
// MySQL Indicator
// sleep_for(seconds(3));
delete res ;
// std::cout << "Finished...\n";
delete stmt ;
delete con ;
return 0 ;
return 0 ;
}
}