b

App/mainwindow.cpp

@@ -12,22 +12,24 @@ MainWindow::MainWindow(QWidget *parent)
     : QMainWindow(parent)
 {
 
-     //ui->maintoolBar->addAction(ui->action);
-
     dlink = new DLink();
 
+    map = new mapcontrol::OPMapWidget(this);
+
+    map->SetShowHome(false);
+    map->SetShowCompass(false);
+    map->SetUseOpenGL(true);
+
+    //QString maptype = myinifile->ReadIni("FGCS.ini","Map","Type");
+    map->SetMapType(mapcontrol::Helper::MapTypeFromString("BingSatellite"));
+    map->setGeometry(0,
+                     0,
+                     this->width(),
+                     this->height());
 
 
     nav = new QNavigationWidget(this);
-
+    nav->setGeometry(0,0,50,this->height());
-   copk = new Cockpit(this);
-
-   copk->setGeometry(this->width() - copk->width(),0,300,340);
-
-
-   nav->setGeometry(0,0,50,this->height());
-
-
     nav->setRowHeight(50);
     nav->addItem(tr("DRV"));
     nav->addItem(tr("MAP"));
@@ -38,38 +40,15 @@ MainWindow::MainWindow(QWidget *parent)
     nav->addItem(tr("DAT"));
     nav->addItem(tr("REP"));
     nav->addItem(tr("COM"));
-/*
-    connect(nav,SIGNAL(currentItemChanged(const int &index)),
-            this,SLOT(onTabIndexChanged(const int &index)));
-            */
 
     connect(nav,SIGNAL(ItemChanged(int)),
             this,SLOT(onTabIndexChanged(int)));
 
-    //connect(nav,&QNavigationWidget::currentItemChanged,
+    copk = new Cockpit(this);
-      //      this,&MainWindow::onTabIndexChanged);
+    copk->setGeometry(this->width() - copk->width(),0,300,340);
 
 
 
-
-    //node->start();
-
-    qDebug() << "mainwindow " <<  QThread::currentThreadId();
-
-
-    map = new mapcontrol::OPMapWidget(this);
-
-    map->SetShowHome(false);
-    map->SetUseOpenGL(true);
-
-    //QString maptype = myinifile->ReadIni("FGCS.ini","Map","Type");
-    map->SetMapType(mapcontrol::Helper::MapTypeFromString("BingSatellite"));
-    map->setGeometry(nav->width(),
-                     0,
-                     this->width() - nav->width() - copk->width(),
-                     this->height());
-
-
     updateTimer = new QTimer();
 
     connect(updateTimer,&QTimer::timeout,
@@ -94,19 +73,18 @@ MainWindow::~MainWindow()
 
 void MainWindow::resizeEvent(QResizeEvent *e)
 {
-    //qDebug() << e;
     Q_UNUSED(e);
 
-    nav->setGeometry(0,0,50,this->height());
+    map->setGeometry(0,
+                     0,
+                     this->width(),
+                     this->height());
+
+    nav->setGeometry(0,0,nav->width(),this->height());
 
     copk->setGeometry(this->width() - copk->width(),0,copk->width(),copk->height());
 
-    map->setGeometry(nav->width(),
-                     0,
-                     this->width() - nav->width() - copk->width(),
-                     this->height());
 
-    update();
 
 }
 
@@ -174,16 +152,15 @@ void MainWindow::client_triggered()
 
 void MainWindow::updateUI()
 {
-/*
-    copk->setAttitude(dlink->mavlinknode->vehicle.attitude.pitch,
-                      dlink->mavlinknode->vehicle.attitude.roll,
-                      dlink->mavlinknode->vehicle.attitude.yaw);
-                      */
 
-    /*qDebug() << dlink->mavlinknode->vehicle.attitude.pitch
+    copk->setAttitude(dlink->mavlinknode->vehicle.attitude.pitch * 57.3,
-             << dlink->mavlinknode->vehicle.attitude.roll
+                      dlink->mavlinknode->vehicle.attitude.roll * 57.3,
-             << dlink->mavlinknode->vehicle.attitude.yaw;
+                      dlink->mavlinknode->vehicle.attitude.yaw * 57.3);
-             */
+
+
+    copk->setAltitude(dlink->mavlinknode->vehicle.gps_raw_int.alt * 10e-3 );
+    copk->setAirSpeed(dlink->mavlinknode->vehicle.airspeed_autocal.vx);
+
 }
 
 

Cockpit/Cockpit.cpp

@@ -66,7 +66,7 @@ Cockpit::Cockpit(QWidget *parent): QWidget(parent)
     connect(UpdateTimer,SIGNAL(timeout()),
             this,SLOT(UpdateTimeout()));
 
-    UpdateTimer->start(20);//50Hz
+    UpdateTimer->start(50);//50Hz
 
 }
 
@@ -84,7 +84,13 @@ Cockpit::~Cockpit()
 
 void Cockpit::UpdateTimeout(void)
 {
-    update();
+    //如果已经设置了新的数据，那么刷新
+    if(m_State.isUpdate == true)
+    {
+        update();
+        m_State.isUpdate = false;
+    }
+
 }
 
 
@@ -112,6 +118,12 @@ void Cockpit::wheelEvent(QWheelEvent *e)
 
 void Cockpit::setPitch(double Pitch)
 {
+        if(isnan(Pitch))
+        {
+            return;
+        }
+        int n = Pitch/360;
+        Pitch = Pitch - n*360;
         m_State.pitch = Pitch;
         if(m_State.pitch < -90)
         {
@@ -121,10 +133,17 @@ void Cockpit::setPitch(double Pitch)
         {
             m_State.pitch -= 180;
         }
+        m_State.isUpdate = true;
 }
 
 void Cockpit::setRoll(double Roll)
 {
+        if(isnan(Roll))
+        {
+            return;
+        }
+        int n = Roll/360;
+        Roll = Roll - n*360;
         m_State.roll = Roll;
         if(m_State.roll < -180)
         {
@@ -134,15 +153,23 @@ void Cockpit::setRoll(double Roll)
         {
             m_State.roll -= 360;
         }
+        m_State.isUpdate = true;
 }
 
 void Cockpit::setYaw(double Yaw)
 {
+    if(isnan(Yaw))
+    {
+        return;
+    }
+    int n = Yaw/360;
+    Yaw = Yaw - n*360;
     m_State.yaw = Yaw;
     if(m_State.yaw < 0)
     {
         m_State.yaw += 360;
     }
+    m_State.isUpdate = true;
 }
 
 
@@ -158,17 +185,21 @@ void Cockpit::setAttitudeSpeed(double PitchSpeed,double RollSpeed,double YawSpee
     m_State.gy = PitchSpeed;
     m_State.gx = RollSpeed;
     m_State.gz = YawSpeed;
+    m_State.isUpdate = true;
+
 }
 
 
 void Cockpit::setAltitude(double Altitude)
 {
     m_State.altitude = Altitude;
+    m_State.isUpdate = true;
 }
 
 void Cockpit::setAirSpeed(double speed)
 {
     m_State.airspeed =  speed;
+    m_State.isUpdate = true;
 }
 
 void Cockpit::setLed(QColor LED)
@@ -244,7 +275,7 @@ void Cockpit::drawPitch(QPainter *painter)
 
     //俯仰刻度表
     qreal h = 1000.0/45.0;//单位高度
-    for(int i = -15 - m_State.pitch;i<=(15 - m_State.pitch);i+=1)
+    for(int i = -17 - m_State.pitch;i<=(17 - m_State.pitch);i+=1)
     {
 
         QString strText = QString::number(qAbs(i));  //设置当前字体

Cockpit/Cockpit.h

@@ -75,6 +75,8 @@ typedef struct {
 
         quint8 AirspeedFlag;
 
+        bool isUpdate;
+
     }_state;
 
 typedef struct {

MavLinkNode/MavLinkNode.pro

@@ -47,10 +47,6 @@ HEADERS += \
 SOURCES += \
     mavlinknode.cpp
 
-
-
-
-
 #添加mavlink目录
 INCLUDEPATH += $$PWD/../mavlink \
                $$PWD/../mavlink/ardupilotmega \

MavLinkNode/mavlinknode.cpp

@@ -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;
+        switch (msg.msgid) {
-    //vehicle.compid = MAV_COMP_ID_AUTOPILOT1;//msg.compid;
+        //状态
+        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();
-
-
-
 }
-
-
-

MavLinkNode/mavlinknode.h

@@ -104,6 +104,11 @@ private slots:
 
    void MAVLinkRcv_Handler(mavlink_message_t msg);
 
+   void StateParse(mavlink_message_t msg);
+   void ParamParse(mavlink_message_t msg);
+   void CommandParse(mavlink_message_t msg);
+   void MissionParse(mavlink_message_t msg);
+
 protected:
 
    enum SourceType{