basic command complete

使用另一個方式做了 goto
完成了 set height

mavone/systemHandler.cpp

@@ -74,13 +74,16 @@ void systemHandler(System& system) {
     bool reset = false;
 
     // 程序執行時會用到的變數
-    int _counter;
     mavlink_message_t mav_message;
-    mavlink_set_position_target_global_int_t set_position_target_global_int;
+    mavlink_command_int_t mav_command_int;
     MavlinkPassthrough::CommandLong mav_command_long;
     MavlinkPassthrough::Result result_mavpass;
     Action::Result result_action;
 
+    int _counter;
+    double _target_lon;
+    double _target_lat;
+
     //
     std::cout << "Dev Test Start" << std::endl;
 
@@ -105,12 +108,10 @@ void systemHandler(System& system) {
     const Telemetry::FlightMode mode_result = telemetry.flight_mode();
     std::cerr << "Vehicle Mode: " << mode_result << '\n';
 
-
-    std::cout << "  Set Mode Test " << std::endl;
     sleep_for(milliseconds(1500));
 
 
-    // sleep_for(milliseconds(1500));
+
     // action.set_takeoff_altitude(10);
     // std::cout << "ARM...\n";
     // action.arm();
@@ -367,6 +368,8 @@ void systemHandler(System& system) {
                     #ifdef VEHICLE_CONTROL_ACTIVE
                     result_action = action.takeoff();
                     std::cout << "   ACK REL: " << result_action << std::endl; // dev
+                    _target_lat = std::stof(telemetryInfo["vehicle_lat"]);
+                    _target_lon = std::stof(telemetryInfo["vehicle_lon"]);
                     #endif
                     break;
 
@@ -376,6 +379,26 @@ void systemHandler(System& system) {
                             << "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
                             << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
                     #ifdef VEHICLE_CONTROL_ACTIVE
+                    if (telemetryInfo["drone_air"] == "0") {
+                        action.set_takeoff_altitude(static_cast<int>(oneVehicleCommand.second[0])); // mavsdk show error but still effect well (?)
+                    } else {
+                        mav_command_int.target_system = sysid;                      // Target system ID
+                        mav_command_int.target_component = MAV_COMP_ID_AUTOPILOT1;  // Target component ID
+                        mav_command_int.command = MAV_CMD_DO_REPOSITION;            // Command ID
+                        mav_command_int.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;      // Coordinate system
+                        mav_command_int.x = _target_lat * 1E7;                      // Target latitude
+                        mav_command_int.y = _target_lon * 1E7;                      // Target longitude
+                        mav_command_int.z = static_cast<float>(oneVehicleCommand.second[0]);  // Target altitude
+                        mavlink_msg_command_int_encode(mavlink_passthrough.get_our_sysid(),
+                                                        mavlink_passthrough.get_our_compid(),
+                                                        &mav_message,
+                                                        &mav_command_int);
+                        // mavlink_passthrough.send_message(mav_message);
+                        result_mavpass = mavlink_passthrough.send_message(mav_message);
+                        std::cout << "   ACK REL: " << result_mavpass << std::endl; // dev
+                        std::cout << "x: " << std::to_string(_target_lon) << std::endl; // dev
+                        std::cout << "y: " << std::to_string(_target_lat) << std::endl; // dev
+                    }
                     #endif
                     break;
 
@@ -385,29 +408,21 @@ void systemHandler(System& system) {
                             << "Para1 : " << std::to_string(oneVehicleCommand.second[0]) \
                             << ", Para2 : " << std::to_string(oneVehicleCommand.second[1]) << std::endl; // debug
                     #ifdef VEHICLE_CONTROL_ACTIVE
-                    set_position_target_global_int.time_boot_ms = 0;
-                    set_position_target_global_int.target_system = sysid;
-                    set_position_target_global_int.target_component = MAV_COMP_ID_AUTOPILOT1;
-                    set_position_target_global_int.coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // 對於 home 點指定相對高度
-                    set_position_target_global_int.type_mask = 0b110111111000; // Only latitude, longitude and altitude
-                    set_position_target_global_int.lat_int = oneVehicleCommand.second[0] * 1E7; // Latitude in degrees * 1E7
-                    set_position_target_global_int.lon_int = oneVehicleCommand.second[1] * 1E7; // Longitude in degrees * 1E7
-                    set_position_target_global_int.alt = 30; // Altitude in meters
-
-                    mavlink_msg_set_position_target_global_int_pack(
-                        mavlink_passthrough.get_our_sysid(),
-                        mavlink_passthrough.get_our_compid(),
-                        &mav_message,
-                        set_position_target_global_int.time_boot_ms,
-                        set_position_target_global_int.target_system,
-                        set_position_target_global_int.target_component,
-                        set_position_target_global_int.coordinate_frame,
-                        set_position_target_global_int.type_mask,
-                        set_position_target_global_int.lat_int,
-                        set_position_target_global_int.lon_int,
-                        set_position_target_global_int.alt,
-                        0, 0, 0, 0, 0, 0, 0, 0);  // vx, vy, vz, afx, afy, afz, yaw, yaw_rate are all 0
+                    mav_command_int.target_system = sysid;                      // Target system ID
+                    mav_command_int.target_component = MAV_COMP_ID_AUTOPILOT1;  // Target component ID
+                    mav_command_int.command = MAV_CMD_DO_REPOSITION;            // Command ID
+                    mav_command_int.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;      // Coordinate system
+                    mav_command_int.x = oneVehicleCommand.second[0] * 1E7;      // Target latitude
+                    mav_command_int.y = oneVehicleCommand.second[1] * 1E7;      // Target longitude
+                    mav_command_int.z = std::stof(telemetryInfo["drone_alt"]);  // Target altitude
+                    mavlink_msg_command_int_encode(mavlink_passthrough.get_our_sysid(),
+                                                    mavlink_passthrough.get_our_compid(),
+                                                    &mav_message,
+                                                    &mav_command_int);
+                    // mavlink_passthrough.send_message(mav_message);
                     result_mavpass = mavlink_passthrough.send_message(mav_message);
+                    _target_lon = oneVehicleCommand.second[1];
+                    _target_lat = oneVehicleCommand.second[0];
                     std::cout << "   ACK REL: " << result_mavpass << std::endl; // dev
                     #endif                    
                     break;