MAVSDK_Dev/mavone/systemHandler.cpp

#include <mavsdk/mavsdk.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;

/*
 * 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};
    int sysid = static_cast<int>(system.get_system_id());
    std::cout << "System " << sysid << " get in Thread. (debug)" << std::endl; // debug

    // Store telemetry information
    std::map<std::string,std::string> telemetryInfo; 


    // Put all the subscriber init in this section
    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("");
    });

    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("");
    });

    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"] = num_str_ss.str();
        num_str_ss.str("");
    });

    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";
        }
    });

    // Wait Until telemetryInfo get something.
    while(telemetryInfo.empty()){
        sleep_for(seconds(1));
    }
    
    // Loop
    while(system.is_connected()) {
        
        // Send Telemetry Data 
        gTeleMtx.lock();
        gTelemetryInfo[sysid] = telemetryInfo;
        gTeleMtx.unlock();

        // Deal Command 

        // for purpose
        sleep_for(milliseconds(100));
    }

    // Destroy
    gVehicleCommand[sysid]["is_connected"] = 0;

    // Thread Terminate
    std::cout << "Thread Out " << sysid << std::endl; //debug
}