hm/gcs-nf
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

b

Browse Source
This commit is contained in:
hm
2020-03-04 15:06:30 +08:00
parent 088ac4549f
commit 70c95f088c
6 changed files with 161 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MavLinkNode/mavlinknode.cpp
+91 -35
View File
@@ -29,7 +29,7 @@ void MavLinkNode::setRunFrq(uint32_t frq)
if((frq != 0)||(frq <= 1000))
{
running_frq = frq;
qDebug() << "set running frquency:" <<frq <<"Hz";
qDebug() << "set Mavlink Node running frquency:" <<frq <<"Hz";
}
}
@@ -202,20 +202,66 @@ void MavLinkNode::Mavlinkparse(quint32 src,QByteArray datagram)
void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg)
{
//vehicle.sysid = msg.sysid;
//vehicle.compid = MAV_COMP_ID_AUTOPILOT1;//msg.compid;
switch (msg.msgid) {
//状态
case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
case MAVLINK_MSG_ID_SYS_STATUS:
case MAVLINK_MSG_ID_HEARTBEAT:
case MAVLINK_MSG_ID_PING:
case MAVLINK_MSG_ID_ATTITUDE:
case MAVLINK_MSG_ID_GPS_RAW_INT:
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
case MAVLINK_MSG_ID_AIRSPEED_AUTOCAL:
case MAVLINK_MSG_ID_RPM:
case MAVLINK_MSG_ID_SCALED_PRESSURE:
case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
case MAVLINK_MSG_ID_BATTERY_STATUS:
case MAVLINK_MSG_ID_VIBRATION:
case MAVLINK_MSG_ID_EngineState:
case MAVLINK_MSG_ID_VFR_HUD:
StateParse(msg);
break;
//航线部分
case MAVLINK_MSG_ID_MISSION_COUNT:
case MAVLINK_MSG_ID_MISSION_CURRENT:
//case MAVLINK_MSG_ID_MISSION_ITEM:
case MAVLINK_MSG_ID_MISSION_ITEM_INT:
//case MAVLINK_MSG_ID_MISSION_REQUEST:
case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
case MAVLINK_MSG_ID_MISSION_ACK:
MissionParse(msg);
break;
//参数
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
case MAVLINK_MSG_ID_PARAM_SET:
case MAVLINK_MSG_ID_PARAM_VALUE:
ParamParse(msg);
break;
//命令
case MAVLINK_MSG_ID_COMMAND_ACK:
CommandParse(msg);
break;
}
}
void MavLinkNode::StateParse(mavlink_message_t msg)
{
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_msg_heartbeat_decode(&msg,&vehicle.heartbeat);
}break;
case MAVLINK_MSG_ID_PING: {
mavlink_msg_ping_decode(&msg,&vehicle.ping);
}break;
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_msg_attitude_decode(&msg,&vehicle.attitude);
qDebug() << "vehicle.attitude.pitch" << vehicle.attitude.pitch;
}break;
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_msg_gps_raw_int_decode(&msg,&vehicle.gps_raw_int);
@@ -226,7 +272,47 @@ void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg)
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_msg_vfr_hud_decode(&msg,&vehicle.vfr_hud);
}break;
}
}
void MavLinkNode::ParamParse(mavlink_message_t msg)
{
switch (msg.msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
mavlink_msg_param_request_list_decode(&msg,&vehicle.param_request_list);
}break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
mavlink_msg_param_request_read_decode(&msg,&vehicle.param_request_read);
}break;
case MAVLINK_MSG_ID_PARAM_SET: {
mavlink_msg_param_set_decode(&msg,&vehicle.param_set);
}break;
case MAVLINK_MSG_ID_PARAM_VALUE: {//分类解码
mavlink_msg_param_value_decode(&msg,&vehicle.param_value);
//要判断是否是全部读取，如果不是那么就不发送那么多
//emit recieveParamater(vehicle.param_value);//发送收到参数
if(vehicle.param_value.param_index < (vehicle.param_value.param_count-1))
{
// Mavlink_param_request_read(vehicle.param_value.param_index + 1);
}
}break;
}
}
void MavLinkNode::CommandParse(mavlink_message_t msg)
{
switch (msg.msgid) {
case MAVLINK_MSG_ID_COMMAND_ACK: {
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&msg,&ack);
}break;
}
}
void MavLinkNode::MissionParse(mavlink_message_t msg)
{
switch (msg.msgid) {
//航线部分
case MAVLINK_MSG_ID_MISSION_COUNT: {//飞控发来计数值，然后开始读取
mavlink_msg_mission_count_decode(&msg,&vehicle.mission_count);
@@ -258,35 +344,5 @@ void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg)
mavlink_msg_mission_request_int_decode(&msg,&vehicle.mission_request_int);
//emit mission_item_request_int(vehicle.mission_request_int.seq);
}break;
case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_msg_mission_ack_decode(&msg,&vehicle.mission_ack);
}break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
mavlink_msg_param_request_list_decode(&msg,&vehicle.param_request_list);
}break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
mavlink_msg_param_request_read_decode(&msg,&vehicle.param_request_read);
}break;
case MAVLINK_MSG_ID_PARAM_SET: {
mavlink_msg_param_set_decode(&msg,&vehicle.param_set);
}break;
case MAVLINK_MSG_ID_PARAM_VALUE: {//分类解码
mavlink_msg_param_value_decode(&msg,&vehicle.param_value);
//要判断是否是全部读取，如果不是那么就不发送那么多
//emit recieveParamater(vehicle.param_value);//发送收到参数
if(vehicle.param_value.param_index < (vehicle.param_value.param_count-1))
{
// Mavlink_param_request_read(vehicle.param_value.param_index + 1);
}
}break;
}
//最后更新一下界面
//emit vehicleUpdate();
}