添加INS，添加舵机指令，修正舵机位置
This commit is contained in:
+63
-21
@@ -1,4 +1,4 @@
|
||||
#include "mainwindow.h"
|
||||
#include "mainwindow.h"
|
||||
#include "QPushButton"
|
||||
#include "QAction"
|
||||
|
||||
@@ -112,8 +112,15 @@ MainWindow::MainWindow(QWidget *parent)
|
||||
connect(toolsui->command,SIGNAL(cmd_long(float,float,float,float,float,float,float,uint16_t,uint8_t)),
|
||||
dlink->mavlinknode->Commander,SLOT(WriteCmd_long(float,float,float,float,float,float,float,uint16_t,uint8_t)),Qt::DirectConnection);
|
||||
|
||||
connect(dlink->mavlinknode->Commander,SIGNAL(commandAccepted(bool,uint16_t,uint8_t)),
|
||||
toolsui->command,SLOT(commandAccepted(bool,uint16_t,uint8_t)),Qt::DirectConnection);
|
||||
|
||||
connect(dlink->mavlinknode,SIGNAL(addVehicles(int,int)),
|
||||
toolsui->command,SLOT(addVehicles(int,int)),Qt::DirectConnection);
|
||||
|
||||
connect(toolsui->command,SIGNAL(WriteCmd(uint8_t,uint8_t,const char*,uint8_t,float)),
|
||||
dlink->mavlinknode->Parameter,SLOT(WriteCmd(uint8_t,uint8_t,const char*,uint8_t,float)),Qt::DirectConnection);
|
||||
|
||||
|
||||
|
||||
|
||||
//this ----- dlink
|
||||
connect(dlink->mavlinknode,SIGNAL(beep()),
|
||||
@@ -930,8 +937,10 @@ void MainWindow::updateUI()//事件驱动式更新数据
|
||||
|
||||
|
||||
bool v28_Low = false,v56_Low = false;
|
||||
v28_Low = ((dlink->mavlinknode->vehicle.bmustate.BAT1_remain_perc * 0.1) < 10)?(1):(0);
|
||||
v56_Low = ((dlink->mavlinknode->vehicle.bmustate.BAT2_remain_perc * 0.1) < 10)?(1):(0);
|
||||
v28_Low = (((float)dlink->mavlinknode->vehicle.bmustate.BAT1_remain_perc * 0.1) < 10)?(false):(true);
|
||||
v56_Low = (((float)dlink->mavlinknode->vehicle.bmustate.BAT2_remain_perc * 0.1) < 10)?(false):(true);
|
||||
|
||||
// qDebug() << "v28_Low" << v28_Low << "v56_Low" << v56_Low;
|
||||
|
||||
toolsui->servosystem->setCheckState(1,v28_Low || v56_Low);
|
||||
toolsui->servosystem->setCheckState(2,v28_Low);
|
||||
@@ -940,9 +949,6 @@ void MainWindow::updateUI()//事件驱动式更新数据
|
||||
toolsui->servosystem->setCheckState(5,(dlink->mavlinknode->vehicle.bmustate.p500w_enabled)?(0):(1));
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
uint32_t health = dlink->mavlinknode->vehicle.sys_status.onboard_control_sensors_health;
|
||||
|
||||
// 0000 0000 0000 0000 0000 0000 0000 0000
|
||||
@@ -962,19 +968,33 @@ void MainWindow::updateUI()//事件驱动式更新数据
|
||||
healthui->setState(6,(dlink->mavlinknode->isCommunicationLost)?(HealthUI::state::failure):(HealthUI::state::success));//DLINK
|
||||
|
||||
healthui->setState(7,getBit(health,13)?(HealthUI::state::success):(HealthUI::state::failure));//AIR
|
||||
healthui->setState(9,getBit(health,15)?(HealthUI::state::success):(HealthUI::state::failure));//FP
|
||||
|
||||
//healthui->setState(9,getBit(health,15)?(HealthUI::state::success):(HealthUI::state::failure));//FP
|
||||
|
||||
|
||||
|
||||
healthui->setState(10,getBit(health,11)?(HealthUI::state::success):(HealthUI::state::failure));//FL
|
||||
|
||||
//dlink->mavlinknode->vehicle.servo_output_raw.servo5_raw
|
||||
uint16_t servoHealt = dlink->mavlinknode->vehicle.servo_output_raw.servo10_raw;
|
||||
|
||||
/*
|
||||
sbus = feedback (ra)
|
||||
sbus = feedback (re)
|
||||
sbus = feedback (ru)
|
||||
sbus = feedback (la)
|
||||
15 sbus = feedback (le)
|
||||
health = 0 le la ru re ra //顺序和上面sbus一样
|
||||
GBIT zero SBIT
|
||||
*/
|
||||
|
||||
//舵机反馈是有符号16b/
|
||||
|
||||
healthui->setState(11,getBit(health,16)?(HealthUI::state::failure):(getBit(servoHealt,3)?(HealthUI::state::warning):(HealthUI::state::success)));//LA
|
||||
healthui->setState(12,getBit(health,17)?(HealthUI::state::failure):(getBit(servoHealt,1)?(HealthUI::state::warning):(HealthUI::state::success)));//RA
|
||||
healthui->setState(13,getBit(health,18)?(HealthUI::state::failure):(getBit(servoHealt,2)?(HealthUI::state::warning):(HealthUI::state::success)));//LE
|
||||
healthui->setState(14,getBit(health,19)?(HealthUI::state::failure):(getBit(servoHealt,4)?(HealthUI::state::warning):(HealthUI::state::success)));//RE
|
||||
healthui->setState(15,getBit(health,20)?(HealthUI::state::failure):(getBit(servoHealt,5)?(HealthUI::state::warning):(HealthUI::state::success)));//RU
|
||||
healthui->setState(11,getBit(health,17)?(getBit(servoHealt,4)?(HealthUI::state::success):(HealthUI::state::warning)):(HealthUI::state::failure));//LA
|
||||
healthui->setState(12,getBit(health,14)?(getBit(servoHealt,1)?(HealthUI::state::success):(HealthUI::state::warning)):(HealthUI::state::failure));//RA
|
||||
healthui->setState(13,getBit(health,18)?(getBit(servoHealt,5)?(HealthUI::state::success):(HealthUI::state::warning)):(HealthUI::state::failure));//LE
|
||||
healthui->setState(14,getBit(health,15)?(getBit(servoHealt,2)?(HealthUI::state::success):(HealthUI::state::warning)):(HealthUI::state::failure));//RE
|
||||
healthui->setState(15,getBit(health,16)?(getBit(servoHealt,3)?(HealthUI::state::success):(HealthUI::state::warning)):(HealthUI::state::failure));//RU
|
||||
|
||||
healthui->setState(16,getBit(health,21)?(HealthUI::state::success):(HealthUI::state::failure));//LT
|
||||
healthui->setState(17,getBit(health,22)?(HealthUI::state::success):(HealthUI::state::failure));//RT
|
||||
@@ -990,7 +1010,7 @@ void MainWindow::updateUI()//事件驱动式更新数据
|
||||
healthui->setState(26,getBit(health,12)?(HealthUI::state::warning):(HealthUI::state::success));//sel
|
||||
healthui->setValueState(26,getBit(health,12)?(tr("连接内置惯导")):(tr("连接SBG")));//sel
|
||||
|
||||
statusui->setState(1,QString::number(dlink->mavlinknode->vehicle.airspeed_autocal.ratio,'f',1),2);
|
||||
statusui->setState(1,QString::number(dlink->mavlinknode->vehicle.airspeed_autocal.ratio,'f',1),0);
|
||||
|
||||
statusui->setState(2,QString::number(dlink->mavlinknode->vehicle.attitude.pitch * 57.3,'f',1),
|
||||
QString::number(dlink->mavlinknode->vehicle.nav_controller_output.nav_pitch * 57.3,'f',1));
|
||||
@@ -998,7 +1018,7 @@ void MainWindow::updateUI()//事件驱动式更新数据
|
||||
statusui->setState(3,QString::number(dlink->mavlinknode->vehicle.attitude.roll * 57.3,'f',1),
|
||||
QString::number(dlink->mavlinknode->vehicle.nav_controller_output.nav_roll * 57.3,'f',1));
|
||||
|
||||
statusui->setState(4,QString::number(dlink->mavlinknode->vehicle.attitude.yaw * 57.3,'f',1),
|
||||
statusui->setState(4,QString::number(dlink->mavlinknode->vehicle.global_position_int.hdg,'f',1),
|
||||
QString::number(dlink->mavlinknode->vehicle.nav_controller_output.nav_bearing,'f',1));
|
||||
|
||||
statusui->setState(5,QString::number(dlink->mavlinknode->vehicle.nav_controller_output.xtrack_error,'f',1),
|
||||
@@ -1023,6 +1043,7 @@ void MainWindow::updateUI()//事件驱动式更新数据
|
||||
statusui->setState(10,QString::number(dlink->mavlinknode->vehicle.global_position_int.vz * 10e-3,'f',1),
|
||||
QString::number(dlink->mavlinknode->vehicle.global_position_int.vz * 10e-3,'f',1));
|
||||
|
||||
|
||||
statusui->setServo(1,(int16_t)dlink->mavlinknode->vehicle.servo_output_raw.servo13_raw,
|
||||
(int16_t)dlink->mavlinknode->vehicle.servo_output_raw.servo3_raw);
|
||||
statusui->setServo(2,(int16_t)dlink->mavlinknode->vehicle.servo_output_raw.servo15_raw,
|
||||
@@ -1035,8 +1056,6 @@ void MainWindow::updateUI()//事件驱动式更新数据
|
||||
(int16_t)dlink->mavlinknode->vehicle.servo_output_raw.servo5_raw);
|
||||
|
||||
|
||||
|
||||
|
||||
statusui->setEngine(1,QString::number(dlink->mavlinknode->vehicle.servo_output_raw.servo6_raw,'f',1));
|
||||
statusui->setEngine(2,QString::number(dlink->mavlinknode->vehicle.turbinstate.RPM_mea,'f',1));
|
||||
statusui->setEngine(3,QString::number(dlink->mavlinknode->vehicle.ccmstate.temp[0] * 0.1,'f',1));
|
||||
@@ -1044,6 +1063,7 @@ void MainWindow::updateUI()//事件驱动式更新数据
|
||||
statusui->setEngine(5,QString::number(dlink->mavlinknode->vehicle.ccmstate.fuel_level,'f',1));
|
||||
statusui->setEngine(6,QString::number(dlink->mavlinknode->vehicle.ccmstate.fuel_level,'f',1));
|
||||
|
||||
|
||||
statusui->setBattery(1,QString::number(dlink->mavlinknode->vehicle.bmustate.BAT1_group_voltage_mv * 0.001,'f',1));
|
||||
statusui->setBattery(2,QString::number(dlink->mavlinknode->vehicle.bmustate.BAT2_group_voltage_mv * 0.001,'f',1));
|
||||
statusui->setBattery(3,QString::number((float)((int16_t)dlink->mavlinknode->vehicle.bmustate.BAT1_group_current_dA) *0.1f,'f',1));
|
||||
@@ -1056,10 +1076,32 @@ void MainWindow::updateUI()//事件驱动式更新数据
|
||||
statusui->setDlink(2,QString::number(dlink->mavlinknode->bitrate));
|
||||
|
||||
|
||||
statusui->setAttitude1(1,QString::number(dlink->mavlinknode->vehicle.ins1.ax,'f',1));
|
||||
statusui->setAttitude1(2,QString::number(dlink->mavlinknode->vehicle.ins1.ay,'f',1));
|
||||
statusui->setAttitude1(3,QString::number(dlink->mavlinknode->vehicle.ins1.az,'f',1));
|
||||
statusui->setAttitude1(4,QString::number(dlink->mavlinknode->vehicle.ins1.gx,'f',1));
|
||||
statusui->setAttitude1(5,QString::number(dlink->mavlinknode->vehicle.ins1.gy,'f',1));
|
||||
statusui->setAttitude1(6,QString::number(dlink->mavlinknode->vehicle.ins1.gz,'f',1));
|
||||
statusui->setAttitude1(7,QString::number(dlink->mavlinknode->vehicle.ins1.roll,'f',1));
|
||||
statusui->setAttitude1(8,QString::number(dlink->mavlinknode->vehicle.ins1.pitch,'f',1));
|
||||
statusui->setAttitude1(9,QString::number(dlink->mavlinknode->vehicle.ins1.yaw,'f',1));
|
||||
|
||||
statusui->setAttitude2(1,QString::number(dlink->mavlinknode->vehicle.ins2.ax,'f',1));
|
||||
statusui->setAttitude2(2,QString::number(dlink->mavlinknode->vehicle.ins2.ay,'f',1));
|
||||
statusui->setAttitude2(3,QString::number(dlink->mavlinknode->vehicle.ins2.az,'f',1));
|
||||
statusui->setAttitude2(4,QString::number(dlink->mavlinknode->vehicle.ins2.gx,'f',1));
|
||||
statusui->setAttitude2(5,QString::number(dlink->mavlinknode->vehicle.ins2.gy,'f',1));
|
||||
statusui->setAttitude2(6,QString::number(dlink->mavlinknode->vehicle.ins2.gz,'f',1));
|
||||
statusui->setAttitude2(7,QString::number(dlink->mavlinknode->vehicle.ins2.roll,'f',1));
|
||||
statusui->setAttitude2(8,QString::number(dlink->mavlinknode->vehicle.ins2.pitch,'f',1));
|
||||
statusui->setAttitude2(9,QString::number(dlink->mavlinknode->vehicle.ins2.yaw,'f',1));
|
||||
|
||||
|
||||
|
||||
statusui->setGPS(1,QString::number(dlink->mavlinknode->vehicle.ins1.lat,'f',8));
|
||||
statusui->setGPS(1,QString::number(dlink->mavlinknode->vehicle.ins1.lon,'f',8));
|
||||
statusui->setGPS(1,QString::number(dlink->mavlinknode->vehicle.ins1.alt,'f',1));
|
||||
statusui->setGPS(1,QString::number(dlink->mavlinknode->vehicle.gps_raw_int.vel,'f',1));
|
||||
statusui->setGPS(1,QString::number(dlink->mavlinknode->vehicle.gps_raw_int.satellites_visible,'f',0));
|
||||
statusui->setGPS(1,QString::number(dlink->mavlinknode->vehicle.gps_raw_int.cog,'f',1));
|
||||
|
||||
/*
|
||||
//实测,r,le,e,la,a
|
||||
@@ -1073,7 +1115,7 @@ void MainWindow::updateUI()//事件驱动式更新数据
|
||||
afb = (1000/2000)
|
||||
0
|
||||
0
|
||||
0
|
||||
healt
|
||||
sbus = feedback (ra)
|
||||
sbus = feedback (re)
|
||||
sbus = feedback (ru)
|
||||
|
||||
Reference in New Issue
Block a user