PX4 MAVSDK – C++ Programming [7편] 특정 위치로 이동 (goto_location) 및 하버사인 공식의 이해

#include <cmath>
#include <numbers> // C++20 이상의 std::numbers::pi_v 사용

// 각도를 라디안으로 변환하는 헬퍼 함수
constexpr double deg_to_rad(double deg) {
    // M_PI는 비표준이므로, <numbers>의 pi_v를 사용하거나 상수를 직접 정의하는 것을 권장합니다 [5].
    return deg * (std::numbers::pi_v<double> / 180.0);
}

// 두 GPS 좌표 간의 거리를 미터(m) 단위로 반환하는 하버사인 함수 [6]
double calculate_distance(double lat1, double lon1, double lat2, double lon2) {
    // 평균 지구 반지름 세팅 (단위: 미터) [6]
    // 더 정밀한 계산을 원할 경우 WGS-84 적도반경(6378137.0)을 사용할 수도 있습니다 [6].
    const double EARTH_RADIUS_M = 6371000.0; 

    // 1. 위도와 경도 차이를 라디안으로 변환 [5]
    double d_lat = deg_to_rad(lat2 - lat1);
    double d_lon = deg_to_rad(lon2 - lon1);

    // 2. 입력받은 위도도 라디안으로 변환 [5]
    double rad_lat1 = deg_to_rad(lat1);
    double rad_lat2 = deg_to_rad(lat2);

    // 3. 매개변수 a 계산 (sin^2 형태를 사용하여 소수점 상실 방지) [5]
    double a = std::sin(d_lat / 2.0) * std::sin(d_lat / 2.0) +
               std::cos(rad_lat1) * std::cos(rad_lat2) * 
               std::sin(d_lon / 2.0) * std::sin(d_lon / 2.0);

    // 4. 중심각 c 계산 (atan2를 사용하여 안정성 확보) [5]
    double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a));

    // 5. 최종 거리 반환 (R * c) [6]
    return EARTH_RADIUS_M * c;
}

#include <cmath>
#include <numbers> // C++20 이상의 std::numbers::pi_v 사용

// 각도를 라디안으로 변환하는 헬퍼 함수
constexpr double deg_to_rad(double deg) {
    // M_PI는 비표준이므로, <numbers>의 pi_v를 사용하거나 상수를 직접 정의하는 것을 권장합니다 [5].
    return deg * (std::numbers::pi_v<double> / 180.0);
}

// 두 GPS 좌표 간의 거리를 미터(m) 단위로 반환하는 하버사인 함수 [6]
double calculate_distance(double lat1, double lon1, double lat2, double lon2) {
    // 평균 지구 반지름 세팅 (단위: 미터) [6]
    // 더 정밀한 계산을 원할 경우 WGS-84 적도반경(6378137.0)을 사용할 수도 있습니다 [6].
    const double EARTH_RADIUS_M = 6371000.0; 

    // 1. 위도와 경도 차이를 라디안으로 변환 [5]
    double d_lat = deg_to_rad(lat2 - lat1);
    double d_lon = deg_to_rad(lon2 - lon1);

    // 2. 입력받은 위도도 라디안으로 변환 [5]
    double rad_lat1 = deg_to_rad(lat1);
    double rad_lat2 = deg_to_rad(lat2);

    // 3. 매개변수 a 계산 (sin^2 형태를 사용하여 소수점 상실 방지) [5]
    double a = std::sin(d_lat / 2.0) * std::sin(d_lat / 2.0) +
               std::cos(rad_lat1) * std::cos(rad_lat2) * 
               std::sin(d_lon / 2.0) * std::sin(d_lon / 2.0);

    // 4. 중심각 c 계산 (atan2를 사용하여 안정성 확보) [5]
    double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a));

    // 5. 최종 거리 반환 (R * c) [6]
    return EARTH_RADIUS_M * c;
}

#include <iostream>
#include <thread>
#include <chrono>
#include <cmath>
#include <numbers>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>

using namespace mavsdk;
using std::chrono::seconds;
using std::this_thread::sleep_for;

// (앞서 정의한 deg_to_rad 및 calculate_distance 함수가 여기에 포함된다고 가정합니다)

int main() {
    // 1. 초기화 및 SITL 연결
    Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}};
    if (mavsdk.add_any_connection("udpin://0.0.0.0:14540") != ConnectionResult::Success) {
        std::cerr << "연결 실패\n"; return 1;
    }

    auto system = mavsdk.first_autopilot(3.0);
    if (!system) {
        std::cerr << "시스템 감지 시간 초과\n"; return 1;
    }

    auto action = Action{system.value()};
    auto telemetry = Telemetry{system.value()};

    // 2. 비행 전 점검 및 이륙
    while (!telemetry.health_all_ok()) {
        std::cout << "안전 점검 및 GPS Lock 대기 중...\n";
        sleep_for(seconds(1));
    }

    std::cout << "Arming 및 이륙 중...\n";
    action.arm();
    action.set_takeoff_altitude(5.0f);
    action.takeoff();

    // 이륙이 어느 정도 안정화될 때까지 대기
    sleep_for(seconds(8));

    // 3. 현재 위치를 기준으로 목표 지점 생성
    auto current_pos = telemetry.position();
    // 위도를 임의로 살짝 증가시켜 북쪽으로 이동 목표 설정 (약 11m 이동 효과)
    double target_lat = current_pos.latitude_deg + 0.0001; 
    double target_lon = current_pos.longitude_deg;
    // 고도는 현재 기체의 절대 고도(AMSL)를 그대로 유지
    double target_alt = current_pos.absolute_altitude_m;
    // 목표 지점을 향해 북쪽(0도)을 바라보게 요 각도 설정
    float target_yaw = 0.0f; 

    // 4. goto_location 명령 전송
    std::cout << "목표 지점으로 이동(goto_location) 명령 전송...\n";
    Action::Result goto_result = action.goto_location(target_lat, target_lon, target_alt, target_yaw);
    
    if (goto_result != Action::Result::Success) {
        std::cerr << "이동 명령 실패: " << goto_result << '\n';
        return 1;
    }

    // 5. 목표 지점 도달 모니터링 (하버사인 공식 활용)
    bool reached_target = false;
    while (!reached_target) {
        // 최신 텔레메트리 위치 데이터 폴링
        auto pos = telemetry.position();
        
        // 현재 위치와 목표 위치 사이의 거리 계산
        double distance_to_target = calculate_distance(
            pos.latitude_deg, pos.longitude_deg, target_lat, target_lon);
        
        std::cout << "목표까지 남은 거리: " << distance_to_target << " 미터\n";

        // 남은 거리가 1미터 미만이면 도착한 것으로 간주
        if (distance_to_target < 1.0) {
            std::cout << "목표 지점에 도달했습니다!\n";
            reached_target = true;
        }

        sleep_for(seconds(1)); // 1초 간격으로 모니터링
    }

    // 6. 착륙 및 종료
    std::cout << "현재 위치에 착륙(Land)을 시도합니다.\n";
    action.land();

    while (telemetry.in_air()) {
        std::cout << "하강 중...\n";
        sleep_for(seconds(1));
    }
    
    std::cout << "착륙 완료! 프로그램을 종료합니다.\n";
    return 0;
}

#include <iostream>
#include <thread>
#include <chrono>
#include <cmath>
#include <numbers>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>

using namespace mavsdk;
using std::chrono::seconds;
using std::this_thread::sleep_for;

// (앞서 정의한 deg_to_rad 및 calculate_distance 함수가 여기에 포함된다고 가정합니다)

int main() {
    // 1. 초기화 및 SITL 연결
    Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}};
    if (mavsdk.add_any_connection("udpin://0.0.0.0:14540") != ConnectionResult::Success) {
        std::cerr << "연결 실패\n"; return 1;
    }

    auto system = mavsdk.first_autopilot(3.0);
    if (!system) {
        std::cerr << "시스템 감지 시간 초과\n"; return 1;
    }

    auto action = Action{system.value()};
    auto telemetry = Telemetry{system.value()};

    // 2. 비행 전 점검 및 이륙
    while (!telemetry.health_all_ok()) {
        std::cout << "안전 점검 및 GPS Lock 대기 중...\n";
        sleep_for(seconds(1));
    }

    std::cout << "Arming 및 이륙 중...\n";
    action.arm();
    action.set_takeoff_altitude(5.0f);
    action.takeoff();

    // 이륙이 어느 정도 안정화될 때까지 대기
    sleep_for(seconds(8));

    // 3. 현재 위치를 기준으로 목표 지점 생성
    auto current_pos = telemetry.position();
    // 위도를 임의로 살짝 증가시켜 북쪽으로 이동 목표 설정 (약 11m 이동 효과)
    double target_lat = current_pos.latitude_deg + 0.0001; 
    double target_lon = current_pos.longitude_deg;
    // 고도는 현재 기체의 절대 고도(AMSL)를 그대로 유지
    double target_alt = current_pos.absolute_altitude_m;
    // 목표 지점을 향해 북쪽(0도)을 바라보게 요 각도 설정
    float target_yaw = 0.0f; 

    // 4. goto_location 명령 전송
    std::cout << "목표 지점으로 이동(goto_location) 명령 전송...\n";
    Action::Result goto_result = action.goto_location(target_lat, target_lon, target_alt, target_yaw);
    
    if (goto_result != Action::Result::Success) {
        std::cerr << "이동 명령 실패: " << goto_result << '\n';
        return 1;
    }

    // 5. 목표 지점 도달 모니터링 (하버사인 공식 활용)
    bool reached_target = false;
    while (!reached_target) {
        // 최신 텔레메트리 위치 데이터 폴링
        auto pos = telemetry.position();
        
        // 현재 위치와 목표 위치 사이의 거리 계산
        double distance_to_target = calculate_distance(
            pos.latitude_deg, pos.longitude_deg, target_lat, target_lon);
        
        std::cout << "목표까지 남은 거리: " << distance_to_target << " 미터\n";

        // 남은 거리가 1미터 미만이면 도착한 것으로 간주
        if (distance_to_target < 1.0) {
            std::cout << "목표 지점에 도달했습니다!\n";
            reached_target = true;
        }

        sleep_for(seconds(1)); // 1초 간격으로 모니터링
    }

    // 6. 착륙 및 종료
    std::cout << "현재 위치에 착륙(Land)을 시도합니다.\n";
    action.land();

    while (telemetry.in_air()) {
        std::cout << "하강 중...\n";
        sleep_for(seconds(1));
    }
    
    std::cout << "착륙 완료! 프로그램을 종료합니다.\n";
    return 0;
}