[MAVROS] Mission 분석

---
current_seq: 6
waypoints:
- frame: 6
  command: 84
  is_current: false
  autocontinue: true
  param1: 0.0
  param2: 0.0
  param3: 0.0
  param4: .nan
  x_lat: 47.3979937
  y_long: 8.5470447
  z_alt: 20.0

- frame: 2
  command: 189
  is_current: false
  autocontinue: true
  param1: 0.0
  param2: 0.0
  param3: 0.0
  param4: 0.0
  x_lat: 0.0
  y_long: 0.0
  z_alt: 0.0
- frame: 2
  command: 206
  is_current: false
  autocontinue: true
  param1: 0.0
  param2: 0.0
  param3: 0.0
  param4: 0.0
  x_lat: 0.0
  y_long: 0.0
  z_alt: 0.0
- frame: 2
  command: 2001
  is_current: false
  autocontinue: true
  param1: 0.0
  param2: .nan
  param3: .nan
  param4: .nan
  x_lat: -2147483648.0
  y_long: -2147483648.0
  z_alt: .nan
- frame: 2
  command: 2501
  is_current: false
  autocontinue: true
  param1: 0.0
  param2: .nan
  param3: .nan
  param4: .nan
  x_lat: -2147483648.0
  y_long: -2147483648.0
  z_alt: .nan
- frame: 6
  command: 31
  is_current: false
  autocontinue: true
  param1: 1.0
  param2: 75.0
  param3: 0.0
  param4: 1.0
  x_lat: 47.3972096
  y_long: 8.5430996
  z_alt: 30.0
- frame: 6
  command: 85
  is_current: true
  autocontinue: true
  param1: 0.0
  param2: 0.0
  param3: 0.0
  param4: .nan
  x_lat: 47.3979944
  y_long: 8.547012
  z_alt: 0.0
---

param 정리

    // std::array<double, 3> home_alt_global_position = { 10.0, 0.0, 0.0 };
    /* VTOL_TAKEOFF
     * NAV_TAKEOFF
     * 0: param1 : min_pitch
     * 0: param2 : empty
     * 0: param3 : empty
     * 0: param4 : yaw angle (NaN)
     *
     */
    setWaypoint(mavros_msgs::msg::Waypoint::FRAME_GLOBAL_REL_ALT,
                mavros_msgs::msg::CommandCode::NAV_TAKEOFF, true, true, 
                // mavros_msgs::msg::CommandCode::NAV_VTOL_TAKEOFF, true, true, 
                {0, 0, 0, vtol::NaN}, { home_alt_global_position[0],
                                   home_alt_global_position[1],
                                   home_alt_global_position[2]});
    // /* FRAME_GLOBAL_REL_ALT
    //  * param1 : loiter time in seconds
    //  * param2 : empty
    //  * param3 : empty
    //  * param4 : yaw angle
    //  * z_alt  : altitude
    //  * x_lat  : latitude
    //  * y_long : longitude
    //  */
    setWaypoint(mavros_msgs::msg::Waypoint::FRAME_GLOBAL_REL_ALT,
                mavros_msgs::msg::CommandCode::NAV_LOITER_TIME, false, true, 
                {10, 0, 0, vtol::NaN}, { home_alt_global_position[0],
                                   home_alt_global_position[1],
                                   home_alt_global_position[2]});
    // WP2
    /* NAV_WAYPOINT
     * 0: param1 : hold time in seconds
     * 0: param2 : empty
     * 0: param3 : empty
     * 0: param4 : empty
     * 0: z_alt  : altitude
     * 0: x_lat  : latitude
     * 0: y_long : longitude
     */
    setWaypoint(mavros_msgs::msg::Waypoint::FRAME_GLOBAL_REL_ALT,
                mavros_msgs::msg::CommandCode::NAV_WAYPOINT, false, true, 
                {5, 0, 0, vtol::NaN}, { home_alt_global_position[0],
                                home_alt_global_position[1],
                                home_alt_global_position[2] + 0.00009});

    /* DO_VTOL_TRANSITION
     * 0: param1 : empty
     * 0: param2 : empty
     * 0: param3 : empty
     * 0: param4 : empty
     * 0: z_alt  : altitude
     * 0: x_lat  : latitude
     * 0: y_long : longitude
     * 
     */
    // setWaypoint(mavros_msgs::msg::Waypoint::FRAME_GLOBAL_REL_ALT,
    //             mavros_msgs::msg::CommandCode::DO_VTOL_TRANSITION, false, true, 
    //             {mavros_msgs::msg::ExtendedState::VTOL_STATE_FW , 0, 0, 0}, 
    //             {home_alt_global_position[0],
    //              home_alt_global_position[1],
    //              home_alt_global_position[2] + 0.00009});

    //
    // // WP2
    setWaypoint(mavros_msgs::msg::Waypoint::FRAME_GLOBAL_REL_ALT,
                mavros_msgs::msg::CommandCode::NAV_WAYPOINT, false, true, 
                {5, 0, 0, vtol::NaN}, {home_alt_global_position[0],
                                 home_alt_global_position[1] + 0.00027,
                                 home_alt_global_position[2] + 0.00009 });

    // setWaypoint(mavros_msgs::msg::Waypoint::FRAME_GLOBAL_REL_ALT,
    //             mavros_msgs::msg::CommandCode::DO_VTOL_TRANSITION, false, true, 
    //             {3, 0, 0, 0}, { home_alt_global_position[0],
    //                             home_alt_global_position[1] + 0.00027,
    //                             home_alt_global_position[2] + 0.00009 });


    // WP3

    
    //
    // WP4
    /* NAV_LAND
     * 0: param1 : empty
     * 0: param2 : empty
     * 0: param3 : empty
     * 0: param4 : empty
     * 0: z_alt  : altitude
     * 0: x_lat  : latitude
     * 0: y_long : longitude
     */
    setWaypoint(mavros_msgs::msg::Waypoint::FRAME_GLOBAL_REL_ALT,
                mavros_msgs::msg::CommandCode::NAV_LAND, false, true, 
                {0, 0, 0, 1.57}, { 0,
                                home_alt_global_position[1],
                                home_alt_global_position[2]});

}
// void OffboardMavros::initializeWaypoints() {
//     // WP0
//     setWaypoint(mavros_msgs::msg::Waypoint::FRAME_LOCAL_ENU,
//                 mavros_msgs::msg::CommandCode::NAV_TAKEOFF, true, true, 
//                 {0, 0, 0, 0}, { 0, 0, 10.0 });
//      
//     // WP1
//     setWaypoint(mavros_msgs::msg::Waypoint::FRAME_LOCAL_ENU,
//                 mavros_msgs::msg::CommandCode::NAV_LOITER_TIME, false, true, 
//                 {10, 0, 2, 1}, { 0, 0, 10.0 });
//
//     // WP2
//     setWaypoint(mavros_msgs::msg::Waypoint::FRAME_LOCAL_ENU,
//                 mavros_msgs::msg::CommandCode::NAV_WAYPOINT, false, true, 
//                 {0, 0, 0, 0}, { 0, 10.0, 20.0 });
//
//     // WP3
//     setWaypoint(mavros_msgs::msg::Waypoint::FRAME_LOCAL_ENU,
//                 mavros_msgs::msg::CommandCode::NAV_LAND, false, true, 
//                 {0, 0, 0, 0}, { 10.0, 10.0, 20.0 });
//
//     // WP4
//     setWaypoint(mavros_msgs::msg::Waypoint::FRAME_LOCAL_ENU,
//                 mavros_msgs::msg::CommandCode::NAV_LAND, false, true, 
//                 {0, 0, 0, 0}, { 0, 0, 0});
//
// }