甘肃龙网

  1. 首页 > 生活 > >

ardupilot之mavlink消息--飞控接收--单向

生活百科脚本

由于项目需要，完成一个测试demo, 本次从中发送消息给飞控，飞控接收发来的wp信息，然后进行修改供程序使用 。首先祭出测试视频 est
首先从.cpp中找到到对应的函数，如下图所示：
跳转进去
再次套娃，可以得到下图：
该函数中的最后一个函数 ();

ardupilot之mavlink消息--飞控接收--单向

文章插图
涉及的文件主要是.cpp , 找到其中 () 即可 。里面是通过语句来判断对应的消息ID的, 如下面代码所示：
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:// MAV ID: 86{// decode packetmavlink_set_position_target_global_int_t packet;mavlink_msg_set_position_target_global_int_decode(&msg, &packet);// exit if vehicle is not in Guided mode or Auto-Guided modeif (!copter.flightmode->in_guided_mode()) {break;}bool pos_ignore= packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;bool vel_ignore= packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;bool acc_ignore= packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;bool yaw_ignore= packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;// exit immediately if acceleration providedif (!acc_ignore) {break;}// extract location from messageLocation loc;if (!pos_ignore) {// sanity check locationif (!check_latlng(packet.lat_int, packet.lon_int)) {break;}Location::AltFrame frame;if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) {// unknown coordinate framebreak;}loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};}// prepare yawfloat yaw_cd = 0.0f;bool yaw_relative = false;float yaw_rate_cds = 0.0f;if (!yaw_ignore) {yaw_cd = ToDeg(packet.yaw) * 100.0f;yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;}if (!yaw_rate_ignore) {yaw_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;}// send targets to the appropriate guided mode controllerif (!pos_ignore && !vel_ignore) {// convert Location to vector from ekf origin for posvel controllerif (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {// posvel controller does not support alt-above-terrainbreak;}Vector3f pos_neu_cm;if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {break;}copter.mode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);} else if (pos_ignore && !vel_ignore) {copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);} else if (!pos_ignore && vel_ignore) {copter.mode_guided.set_destination(loc, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);}break;
大概流程就是判断接收到的数据是什么类型，然后进行相应的控制（控制速度，控制航点，还是速度和位置都控制） 。需要修改的地方就显而易见了 。
需要注意的点：
本次实验采用的是脚本的形式完成消息的解析 。在中 不要采用函数，该函数好像用的是, 而不是，导致捕获不到消息 。脚本在处理 的时候，解析出错，报错为中的.pack需要整形的数据，但是发送的不是 。最后采用强制数据类型转换完成 。脚本附赠如下：
【ardupilot之mavlink消息--飞控接收--单向】import timeimport randomimport dronekitimport mathfrom pymavlink import mavutilimport threadingvehicle = dronekit.connect('127.0.0.1:14550',wait_ready=True)master =mavutil.mavlink_connection('udp:127.0.0.1:{}'.format(14551))master.wait_heartbeat()def prearm_check():if vehicle.mode.name != "STABILIZE":vehicle.mode = "STABILIZE"print("waiting for changing the mode to stabilize...")vehicle.wait_for_mode("STABILIZE")print("changing mode to stabilize successfully...")while not vehicle.gps_0.fix_type:time.sleep(0.5)print("gps check passed...")while not vehicle.ekf_ok:time.sleep(0.5)print("ekf check passed...")def goto_position_target_global_int(alocation):"""Send SET_POSITION_TARGET_GLOBAL_INT command to request the vehicle fly to a specified location."""msg = vehicle.message_factory.set_position_target_global_int_encode(0,# time_boot_ms (not used)0, 0,# target system, target componentmavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, # frame0b0000111111111000, # type_mask (only speeds enabled)alocation.lat*1e7, # lat_int - X Position in WGS84 frame in 1e7 * metersalocation.lon*1e7, # lon_int - Y Position in WGS84 frame in 1e7 * metersalocation.alt, # alt - Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT0, # X velocity in NED frame in m/s0, # Y velocity in NED frame in m/s0, # Z velocity in NED frame in m/s0, 0, 0, # afx, afy, afz acceleration (not supported yet, ignored in GCS_Mavlink)0, 0)# yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)# send command to vehiclevehicle.send_mavlink(msg)def goto_position_target_local_ned(vehicle, north, east, down):"""Send SET_POSITION_TARGET_LOCAL_NED command to request the vehicle fly to a specifiedlocation in the North, East, Down frame."""msg = vehicle.message_factory.set_position_target_local_ned_encode(0,# time_boot_ms (not used)0, 0,# target system, target componentmavutil.mavlink.MAV_FRAME_LOCAL_NED,# frame0b0000111111111000,# type_mask (only positions enabled)north, east, down,0, 0, 0,# x, y, z velocity in m/s(not used)0, 0, 0,# x, y, z acceleration (not supported yet, ignored in GCS_Mavlink)0, 0)# yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink)# send command to vehiclevehicle.send_mavlink(msg)def is_fly_land_reset():if vehicle.armed == True:while vehicle.mode != "RTL":vehicle.wait_for_mode("RTL")print("Set mode to RTL...")while vehicle.armed != False:time.sleep(1)vehicle.wait_for_mode("STABILIZE")print("Vehicle reset OK...")def get_location_metres(original_location, dNorth, dEast):earth_radius = 6378137.0# Radius of "spherical" earth# Coordinate offsets in radiansdLat = dNorth / earth_radiusdLon = dEast / (earth_radius * math.cos(math.pi * original_location.lat / 180))# New position in decimal degreesnewlat = original_location.lat + (dLat * 180 / math.pi)newlon = original_location.lon + (dLon * 180 / math.pi)if type(original_location) is dronekit.LocationGlobal:targetlocation = dronekit.LocationGlobal(newlat, newlon, original_location.alt)elif type(original_location) is dronekit.LocationGlobalRelative:targetlocation = dronekit.LocationGlobalRelative(newlat, newlon, original_location.alt)else:raise Exception("Invalid Location object passed")return targetlocationdef goto(dNorth, dEast, gotoFunction=vehicle.simple_goto):currentLocation = vehicle.location.global_relative_frametargetLocation = get_location_metres(currentLocation, dNorth, dEast)# targetDistance = get_distance_metres(currentLocation, targetLocation)gotoFunction(targetLocation)delay_time = 60def time_schedule():while True:if 'i' == input():delay_time= delay_time + 0.1elif 'k' == input():delay_time = delay_time - 0.1else:print("Wrong input... Nothing changed...")if delay_time


上一篇：贵州旅游景点

下一篇：赴美生子论坛