ArduPilot-sitl中的一些操作記錄

ArduPilot 這么優秀的代碼，提供了一套很方便的SITL仿真開發模式，在git clone代碼的時候，已經將相關的東西下載下來了，問題是如何進行使用，
首先要安裝mavproxy 這個軟體，pymavlink --mavlink封裝的python module，后面也可以采用這個腳本給仿真加上自己的程式，這點和ROS/MAVROS 的模式類似，（ROS是Ｃ++編程，多執行緒是沒有問題的，python是偽多執行緒）
其中指令有多點，./sim_vehicle.py --vehicle=ArduCopter --frame=heli --tonealarm --rgbled --console --map --out=172.18.52.221:14550
不過可以采用./sim_vehicle.py --help來進行查詢， 上面的例子是可以將生成的內容通過172.18.52.221的14550埠發送出去，采用的是UDP形式，我這里采用的是在同一個局域網的地面站進行查看的，地面站的操作方法，我在前面的博客中已經提及到了，
我采用的是直升機的仿真，對于直升機而言，在準備好之后需要有一個操作，就是

1. rc 8 1000
2. rc 3 1000
3. arm throttle
4. rc 8 1700
5. rc 3 1700
   這樣就完成了直升機的起飛，當然，也可以采用遠程mission planner來設定航點，讓飛機飛過去，
   每次仿真之后會在當前檔案夾下生成一個log檔案，位于log檔案夾下，這個檔案可以通過地面站轉換成matlab可以識別的.mat檔案，然后繪制相應的曲線圖，方便科研，

仿真可以通過ros完成無人機的控制，只需要roslaunch在啟動的時候給定埠就可以了，如下圖所示，系結14550埠，

程式部分需要注意是guide模式，然后起飛的時候要先takeoff這點和Px4不同，下面給出示例程式：

#include <ros/ros.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandBool.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandTOL.h>
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
    current_state = *msg;
}

geometry_msgs::PoseStamped local_pos;
void pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    local_pos = *msg;
}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"guided");
    ros::NodeHandle nh;
    ros::Rate rate(50);

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state",10,state_cb);
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
    ros::ServiceClient arming_clinet = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    ros::Publisher pos_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local",10);
    ros::ServiceClient takeoff_client = nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/takeoff",10);
    ros::Subscriber pos_cur_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose",10,pos_cb);

    geometry_msgs::PoseStamped pos_des;
    pos_des.pose.position.x = 0;
    pos_des.pose.position.y = 0;
    pos_des.pose.position.z = 20;

    mavros_msgs::SetMode guide_mode;
    guide_mode.request.custom_mode = "GUIDED";

    mavros_msgs::CommandBool arming;
    arming.request.value = true;

    mavros_msgs::CommandTOL takeoff;
    takeoff.request.altitude = 2;
    while(ros::ok())
    {
        if(!current_state.armed )
        {
            arming_clinet.call(arming);
        }
        if(current_state.mode != "GUIDED")
        {
            set_mode_client.call(guide_mode);
        }
        if(current_state.armed && current_state.mode =="GUIDED" && local_pos.pose.position.z<=1.5 )
        {
            takeoff_client.call(takeoff);
        }
        else{
            pos_pub.publish(pos_des);
        }
        ros::spinOnce();
        rate.sleep();
    }
    return 1;
}

之后又采用pymavlink來實作試試，一個簡單的模板如下：

from pymavlink import mavutil

master = mavutil.mavlink_connection('udp:127.0.0.1:{}'.format(14551))
master.wait_heartbeat()
print(master.target_system)

# def change_mode_guided():
    # if master.flightmode != 'GUIDED':
    #     master.set_mode_apm

message = master.recv_match(type='LOCAL_POSITION_NED',blocking=True)

# mavlink（message) is needed!
while True:
    master.mav.command_long_send(master.target_system,master.target_component,
                        176,
                        0,
                        1,
                        4,0,0,0,0,0,force_mavlink1=True)
print(message)

注意這里面的模式切換4, 代表的是guided 模式，1表示的為custom Mode 參考網址