更改传感器数据来源

App/mainwindow.cpp

@@ -2190,6 +2190,7 @@ void MainWindow::updateVehicle(MavLinkNode::_vehicle vehicle)//事件驱动式
 
     }
 
+/*
     toolsui->senser->setDAS(1,3,QString::number(vehicle.vfr_hud.alt,'f',1));
     toolsui->senser->setDAS(1,4,QString::number(vehicle.vfr_hud.airspeed,'f',1));
     toolsui->senser->setDAS(1,5,QString::number(vehicle.emb_atom_com.Airspeed,'f',1));
@@ -2203,6 +2204,21 @@ void MainWindow::updateVehicle(MavLinkNode::_vehicle vehicle)//事件驱动式
     toolsui->senser->setDAS(2,4,QString::number(vehicle.vfr_hud.airspeed,'f',1));
     toolsui->senser->setDAS(2,7,QString::number(vehicle.scaled_pressure.press_diff,'f',3));
     toolsui->senser->setDAS(2,8,QString::number(vehicle.scaled_pressure.press_abs,'f',3));
+*/
+
+    toolsui->senser->setDAS(1,3,QString::number(qIsNaN(vehicle.vfr_hud.alt)?(0):(vehicle.vfr_hud.alt),'f',1));
+    toolsui->senser->setDAS(1,4,QString::number(qIsNaN(vehicle.vfr_hud.airspeed)?(0):(vehicle.vfr_hud.airspeed),'f',1));
+    toolsui->senser->setDAS(1,5,QString::number(qIsNaN(vehicle.emb_atom_com.Airspeed)?(0):(vehicle.emb_atom_com.Airspeed),'f',1));
+    toolsui->senser->setDAS(1,6,QString::number(qIsNaN(vehicle.emb_atom_com.mach)?(0):(vehicle.emb_atom_com.mach),'f',3));
+    toolsui->senser->setDAS(1,7,QString::number(qIsNaN(vehicle.emb_atom_com.qbar)?(0):(vehicle.emb_atom_com.qbar) * 0.01,'f',3));
+    toolsui->senser->setDAS(1,8,QString::number(qIsNaN(vehicle.emb_atom_com.ps)?(0):(vehicle.emb_atom_com.ps) * 0.01,'f',3));
+
+    toolsui->senser->setDAS(2,1,QString::number(qIsNaN(vehicle.emb_atom_com.alpha)?(0):(vehicle.emb_atom_com.alpha),'f',1));
+    toolsui->senser->setDAS(2,2,QString::number(qIsNaN(vehicle.emb_atom_com.beta)?(0):(vehicle.emb_atom_com.beta),'f',1));
+
+    toolsui->senser->setDAS(2,4,QString::number(qIsNaN(vehicle.vfr_hud.airspeed)?(0):(vehicle.vfr_hud.airspeed),'f',1));
+    toolsui->senser->setDAS(2,7,QString::number(qIsNaN(vehicle.scaled_pressure.press_diff)?(0):(vehicle.scaled_pressure.press_diff),'f',3));
+    toolsui->senser->setDAS(2,8,QString::number(qIsNaN(vehicle.scaled_pressure.press_abs)?(0):(vehicle.scaled_pressure.press_abs),'f',3));
 
 
     quint16 voltages[10];
@@ -2262,7 +2278,6 @@ void MainWindow::updateVehicle(MavLinkNode::_vehicle vehicle)//事件驱动式
     emit setBat(voltages);
 }
 
-
 void MainWindow::ins1_Update(mavlink_ins1_t ins)
 {
     QString bit1;
@@ -2411,8 +2426,6 @@ void MainWindow::ins1_Update(mavlink_ins1_t ins)
     }
 
 
-
-
     toolsui->senser->setINS(1,1,QString::number(ins.satellites_visible));
     toolsui->senser->setINS(1,2,bit1);//bit
     toolsui->senser->setINS(1,3,att1);//bit1
@@ -2422,18 +2435,18 @@ void MainWindow::ins1_Update(mavlink_ins1_t ins)
     toolsui->senser->setINS(1,7,sys1);//sys
     toolsui->senser->setINS(1,8,com1);//com
     toolsui->senser->setINS(1,9,gps1);//gps
-    toolsui->senser->setINS(1,10,QString::number(ins.lon,'f',8));
+    toolsui->senser->setINS(1,10,QString::number(qIsNaN(ins.lon)?(0):(ins.lon),'f',8));
-    toolsui->senser->setINS(1,11,QString::number(ins.lat,'f',8));
+    toolsui->senser->setINS(1,11,QString::number(qIsNaN(ins.lat)?(0):(ins.lat),'f',8));
-    toolsui->senser->setINS(1,12,QString::number(ins.alt,'f',1));
+    toolsui->senser->setINS(1,12,QString::number(qIsNaN(ins.alt)?(0):(ins.alt),'f',1));
     toolsui->senser->setINS(1,13,QString::number(sqrt(pow(ins.v_north,2) +
                                                      pow(ins.v_east,2) +
                                                      pow(ins.v_up,2)),'f',1));
-    toolsui->senser->setINS(1,14,QString::number(ins.roll * 57.3,'f',1));
+    toolsui->senser->setINS(1,14,QString::number(qIsNaN(ins.roll)?(0):(ins.roll) * 57.3,'f',1));
-    toolsui->senser->setINS(1,15,QString::number(ins.pitch * 57.3,'f',1));
+    toolsui->senser->setINS(1,15,QString::number(qIsNaN(ins.pitch)?(0):(ins.pitch) * 57.3,'f',1));
-    toolsui->senser->setINS(1,16,QString::number(to360deg(ins.yaw * 57.3),'f',1));
+    toolsui->senser->setINS(1,16,QString::number(to360deg(qIsNaN(ins.yaw)?(0):(ins.yaw) * 57.3),'f',1));
 
-    qreal east = ins.v_east;
+    qreal east = qIsNaN(ins.v_east)?(0):(ins.v_east);
-    qreal north = ins.v_north;
+    qreal north = qIsNaN(ins.v_north)?(0):(ins.v_north);
     if((east != 0)||
        (north != 0))
     {
@@ -2441,149 +2454,195 @@ void MainWindow::ins1_Update(mavlink_ins1_t ins)
                                                                     north) * 57.3),'f',1));
     }
 
-    toolsui->senser->setINS(1,18,QString::number(ins.gx,'f',1));
+    toolsui->senser->setINS(1,18,QString::number(qIsNaN(ins.gx)?(0):(ins.gx),'f',1));
-    toolsui->senser->setINS(1,19,QString::number(ins.gy,'f',1));
+    toolsui->senser->setINS(1,19,QString::number(qIsNaN(ins.gy)?(0):(ins.gy),'f',1));
-    toolsui->senser->setINS(1,20,QString::number(ins.gz,'f',1));
+    toolsui->senser->setINS(1,20,QString::number(qIsNaN(ins.gz)?(0):(ins.gz),'f',1));
-    toolsui->senser->setINS(1,21,QString::number(ins.ax,'f',1));
+    toolsui->senser->setINS(1,21,QString::number(qIsNaN(ins.ax)?(0):(ins.ax),'f',1));
-    toolsui->senser->setINS(1,22,QString::number(ins.ay,'f',1));
+    toolsui->senser->setINS(1,22,QString::number(qIsNaN(ins.ay)?(0):(ins.ay),'f',1));
-    toolsui->senser->setINS(1,23,QString::number(ins.az,'f',1));
+    toolsui->senser->setINS(1,23,QString::number(qIsNaN(ins.az)?(0):(ins.az),'f',1));
 }
 
 void MainWindow::ins2_Update(mavlink_ins2_t ins)
 {
-
+    QString bit2;
-
+    switch (ins.BIT & 0x0F) {
-    uint8_t com = (ins.com_status & 0x30) >> 4;
-    QString com_str;
-    com_str.clear();
-    switch (com) {
-    case 0:
-        com_str = tr("准备");
-        break;
-    case 1:
-        com_str = tr("对准中");
-        break;
-    case 2:
-        com_str = tr("导航");
-        break;
-    case 3:
-        com_str = tr("对准失败");
-        break;
     default:
-        com_str = tr("未知参数");
+    case 0:
-        break;
+    {
+           bit2.append(tr("未初始化"));
+    }break;
+    case 1:
+    {
+           bit2.append(tr("垂直陀螺"));
+    }break;
+    case 2:
+    {
+           bit2.append(tr("AHRS"));
+    }break;
+    case 3:
+    {
+           bit2.append(tr("速度导航"));
+    }break;
+    case 4:
+    {
+           bit2.append(tr("位置导航"));
+    }break;
     }
 
-//    statusui->setValue(0,3,2,com_str);
+    QString att2;
+    if(ins.BIT & 0x10)
+    {
+        att2.append(tr("正常"));
+    }
+    else{
+        att2.append(tr("无效"));
+    }
 
-    com = (ins.com_status & 0x0E) >> 1;
+    QString heading2;
-    com_str.clear();
+    if(ins.BIT & 0x20)
-    switch (com) {
+    {
+        heading2.append(tr("正常"));
+    }
+    else{
+        heading2.append(tr("无效"));
+    }
+
+    QString spd2;
+    if(ins.BIT & 0x40)
+    {
+        spd2.append(tr("正常"));
+    }
+    else{
+        spd2.append(tr("无效"));
+    }
+
+    QString pos2;
+    if(ins.BIT & 0x80)
+    {
+        pos2.append(tr("正常"));
+    }
+    else{
+        pos2.append(tr("无效"));
+    }
+
+
+
+    QString sys2;
+    switch (ins.sys_status & 0x0F) {
+    default:
     case 0:
-        com_str = tr("未初始化");
+    {
-        break;
+           sys2.append(tr("解算正常"));
+    }break;
     case 1:
-        com_str = tr("INS/大气");
+    {
+           sys2.append(tr("卫星数不足"));
+    }break;
+    case 2:
+    {
+           sys2.append(tr("内部错误"));
+    }break;
+    case 3:
+    {
+           sys2.append(tr("速度超限"));
+    }break;
+    }
+
+
+    QString com2;
+    switch (ins.com_status & 0x0F) {
+    default:
+    case 0:
+    {
+           com2.append(tr("无效"));
+    }break;
+    case 1:
+    {
+           com2.append(tr("未知"));
+    }break;
+    case 2:
+    {
+           com2.append(tr("多普勒"));
+    }break;
+    case 3:
+    {
+           com2.append(tr("微分"));
+    }break;
+    }
+
+
+    QString gps2;
+    switch (ins.gps_status) {
+    case 0:
+        gps2.append(tr("未定位"));
+    case 1:
+        gps2.append(tr("未知"));
         break;
     case 2:
-        com_str = tr("INS/DGPS");
+        gps2.append(tr("单点"));
         break;
     case 3:
-        com_str = tr("INS/GNSS");
+        gps2.append(tr("伪距差分"));
+        break;
+    case 4:
+        gps2.append(tr("SBAS广域差分"));
         break;
     case 5:
-        com_str = tr("GNSS");
+        gps2.append(tr("广域差分"));
+        break;
+    case 6:
+        gps2.append(tr("RTK_FLOAT"));
+        break;
+    case 7:
+        gps2.append(tr("RTK_INT"));
+        break;
+    case 8:
+        gps2.append(tr("PPP_FLOAT"));
+        break;
+    case 9:
+        gps2.append(tr("PPP_INT"));
+        break;
+    case 10:
+        gps2.append(tr("FIXED"));
         break;
     default:
-        com_str = tr("未知参数");
         break;
     }
 
-//    statusui->setValue(0,3,3,com_str);
-
-
-    uint8_t com2 = (ins.com_status & 0x30) >> 4;
-    QString com2_str;
-    com2_str.clear();
-    switch (com2) {
-    case 0:
-        com2_str = tr("准备");
-        break;
-    case 1:
-        com2_str = tr("对准中");
-        break;
-    case 2:
-        com2_str = tr("导航");
-        break;
-    case 3:
-        com2_str = tr("对准失败");
-        break;
-    default:
-        com2_str = tr("未知参数");
-        break;
-    }
-
-
-    uint8_t gps2 = (ins.com_status & 0x0E) >> 1;
-    QString gps2_str;
-    switch (gps2) {
-    case 0:
-        gps2_str = tr("N/A");
-        break;
-    case 1:
-        gps2_str = tr("INS/大气");
-        break;
-    case 2:
-        gps2_str = tr("INS/DGPS");
-        break;
-    case 3:
-        gps2_str = tr("INS/GNSS");
-        break;
-    case 5:
-        gps2_str = tr("GNSS");
-        break;
-    default:
-        gps2_str = tr("未知参数");
-        break;
-    }
-
-
-
     toolsui->senser->setINS(2,1,QString::number(ins.satellites_visible));
-    toolsui->senser->setINS(2,2,(ins.sys_status & 0x80)?(tr("有效")):(tr("无效")));
+    toolsui->senser->setINS(2,2,bit2);
-    toolsui->senser->setINS(2,3,(ins.sys_status & 0x10)?(tr("有效")):(tr("无效")));//bit1
+    toolsui->senser->setINS(2,3,att2);//bit1
-    toolsui->senser->setINS(2,4,(ins.sys_status & 0x02)?(tr("有效")):(tr("无效")));//bit2
+    toolsui->senser->setINS(2,4,heading2);//bit2
-    toolsui->senser->setINS(2,5,(ins.sys_status & 0x40)?(tr("有效")):(tr("无效")));//bit3
+    toolsui->senser->setINS(2,5,spd2);//bit3
-    toolsui->senser->setINS(2,6,(ins.sys_status & 0x80)?(tr("有效")):(tr("无效")));//bit4
+    toolsui->senser->setINS(2,6,pos2);//bit4
-    //toolsui->senser->setINS(2,7,sys2);
+    toolsui->senser->setINS(2,7,sys2);
-    toolsui->senser->setINS(2,8,com2_str);
+    toolsui->senser->setINS(2,8,com2);
-    toolsui->senser->setINS(2,9,gps2_str);
+    toolsui->senser->setINS(2,9,gps2);
-    toolsui->senser->setINS(2,10,QString::number(ins.lon,'f',8));
+    toolsui->senser->setINS(2,10,QString::number(qIsNaN(ins.lon)?(0):(ins.lon),'f',8));
-    toolsui->senser->setINS(2,11,QString::number(ins.lat,'f',8));
+    toolsui->senser->setINS(2,11,QString::number(qIsNaN(ins.lat)?(0):(ins.lat),'f',8));
-    toolsui->senser->setINS(2,12,QString::number(ins.alt,'f',1));
+    toolsui->senser->setINS(2,12,QString::number(qIsNaN(ins.alt)?(0):(ins.alt),'f',1));
-    toolsui->senser->setINS(2,13,QString::number(sqrt(pow(ins.v_north,2) +
+    toolsui->senser->setINS(2,13,QString::number(sqrt(pow(qIsNaN(ins.v_north)?(0):(ins.v_north),2) +
-                                                     pow(ins.v_east,2) +
+                                                     pow(qIsNaN(ins.v_east)?(0):(ins.v_east),2) +
-                                                     pow(ins.v_up,2)),'f',1));
+                                                     pow(qIsNaN(ins.v_up)?(0):(ins.v_up),2)),'f',1));
-    toolsui->senser->setINS(2,14,QString::number(ins.roll * 57.3,'f',1));
+    toolsui->senser->setINS(2,14,QString::number(qIsNaN(ins.roll)?(0):(ins.roll) * 57.3,'f',1));
-    toolsui->senser->setINS(2,15,QString::number(ins.pitch * 57.3,'f',1));
+    toolsui->senser->setINS(2,15,QString::number(qIsNaN(ins.pitch)?(0):(ins.pitch) * 57.3,'f',1));
-    toolsui->senser->setINS(2,16,QString::number(to360deg(ins.yaw * 57.3),'f',1));
+    toolsui->senser->setINS(2,16,QString::number(to360deg(qIsNaN(ins.yaw)?(0):(ins.yaw) * 57.3),'f',1));
 
-    qreal east = ins.v_east;
+    double east2 = qIsNaN(ins.v_east)?(0):(ins.v_east);
-    qreal north = ins.v_north;
+    double north2 = qIsNaN(ins.v_north)?(0):(ins.v_north);
-    if((east != 0)||
+    if((east2 != 0)&&
-       (north != 0))
+       (north2 != 0))
     {
-        toolsui->senser->setINS(2,17,QString::number(to360deg(atan2(east,
+        toolsui->senser->setINS(2,17,QString::number(to360deg(atan2(east2,north2) * 57.3),'f',1));
-                                                                    north) * 57.3),'f',1));
+
     }
 
-
+    toolsui->senser->setINS(2,18,QString::number(qIsNaN(ins.gx)?(0):(ins.gx),'f',1));
-    toolsui->senser->setINS(2,18,QString::number(ins.gx,'f',1));
+    toolsui->senser->setINS(2,19,QString::number(qIsNaN(ins.gy)?(0):(ins.gy),'f',1));
-    toolsui->senser->setINS(2,19,QString::number(ins.gy,'f',1));
+    toolsui->senser->setINS(2,20,QString::number(qIsNaN(ins.gz)?(0):(ins.gz),'f',1));
-    toolsui->senser->setINS(2,20,QString::number(ins.gz,'f',1));
+    toolsui->senser->setINS(2,21,QString::number(qIsNaN(ins.ax)?(0):(ins.ax),'f',1));
-    toolsui->senser->setINS(2,21,QString::number(ins.ax,'f',1));
+    toolsui->senser->setINS(2,22,QString::number(qIsNaN(ins.ay)?(0):(ins.ay),'f',1));
-    toolsui->senser->setINS(2,22,QString::number(ins.ay,'f',1));
+    toolsui->senser->setINS(2,23,QString::number(qIsNaN(ins.az)?(0):(ins.az),'f',1));
-    toolsui->senser->setINS(2,23,QString::number(ins.az,'f',1));
 }