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

修改NAN

Browse Source
This commit is contained in:
hm
2021-11-11 15:00:56 +08:00
parent 67d6b2c518
commit 27eee67444
4 changed files with 137 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
App/mainwindow.cpp
+105 -102
View File
@@ -399,9 +399,14 @@ MainWindow::MainWindow(QWidget *parent)
connect(dlink,SIGNAL(PortConnected(QVariant,QVariant,QVariant,QVariant,QVariant,QVariant,QVariant,QVariant)),
setting->index0->link,SIGNAL(PortConnect(QVariant,QVariant,QVariant,QVariant,QVariant,QVariant,QVariant,QVariant)));
/*
connect(dlink->mavlinknode,SIGNAL(state_updated()),
this,SLOT(updateUI()));
*/
qRegisterMetaType<MavLinkNode::_vehicle>("MavLinkNode::_vehicle");
connect(dlink->mavlinknode,SIGNAL(signal_vehicle(MavLinkNode::_vehicle)),
this,SLOT(updateVehicle(MavLinkNode::_vehicle)));
connect(setting->index0->link,SIGNAL(connectSignal(QVariant,QVariant,QVariant,QVariant,QVariant,QVariant,QVariant,QVariant)),
@@ -1130,18 +1135,17 @@ void MainWindow::update_servo_output_raw(mavlink_servo_output_raw_t servo)
}
// 16~20Hz左右 运行频率可能太高（30fps）
void MainWindow::updateUI()//事件驱动式更新数据
void MainWindow::updateVehicle(MavLinkNode::_vehicle vehicle)//事件驱动式更新数据
{
//多次重复进入？
//问题：不适合调用式，要使用信号，因为读取法会在读取过程中，数被修改，导致出现NAN，导致计算出错
qDebug() << "updateUI";
qDebug() << "updateVehicle";
//qDebug()<<"updeta";
//设置舵机显示
//update_servo_output_raw(dlink->mavlinknode->vehicleList.value(currentUAV).servo_output_raw);
//update_servo_output_raw(vehicle.servo_output_raw);
static uint32_t custommode_old = 0;
static uint8_t state_old = 0;
@@ -1165,74 +1169,99 @@ void MainWindow::updateUI()//事件驱动式更新数据
return;
}
if(!dlink->mavlinknode->vehicleList.keys().contains(currentUAV))//滤掉不存在的无人机
qDebug() << "4";
double lat = (double)(vehicle.gps_raw_int.lat * 10e-8);
double lng = (double)(vehicle.gps_raw_int.lon * 10e-8);
if(((lat > -90)&&(lat < 90))&&((lng > -180)&&(lng < 180)))
{
map->setUAVPos(vehicle.sysid,
vehicle.compid,
(double)(vehicle.gps_raw_int.lat * 10e-8),
(double)(vehicle.gps_raw_int.lon * 10e-8),
(double)(vehicle.gps_raw_int.alt * 10e-4));
}
map->setUAVHeading(vehicle.sysid,
vehicle.compid,
vehicle.attitude.yaw * 57.3);
qDebug() << "5";
if(vehicle.sysid != currentUAV)//滤掉不存在的无人机
{
return;
}
qDebug() << "0";
copk->setAttitude(dlink->mavlinknode->vehicleList.value(currentUAV).attitude.pitch * 57.3,
dlink->mavlinknode->vehicleList.value(currentUAV).attitude.roll * 57.3,
dlink->mavlinknode->vehicleList.value(currentUAV).attitude.yaw * 57.3);
copk->setAttitude(vehicle.attitude.pitch * 57.3,
vehicle.attitude.roll * 57.3,
vehicle.attitude.yaw * 57.3);
copk->setAltitude(dlink->mavlinknode->vehicleList.value(currentUAV).global_position_int.alt * 10e-4);
copk->setAltitudeTarget(dlink->mavlinknode->vehicleList.value(currentUAV).global_position_int.alt * 10e-4
+dlink->mavlinknode->vehicleList.value(currentUAV).nav_controller_output.alt_error);
copk->setAltitude(vehicle.global_position_int.alt * 10e-4);
copk->setAltitudeTarget(vehicle.global_position_int.alt * 10e-4
+vehicle.nav_controller_output.alt_error);
switch (copk->AltitudeFlag()) {
default:
case 0://绝对
copk->setHeight(dlink->mavlinknode->vehicleList.value(currentUAV).global_position_int.alt * 10e-4);
copk->setHeight(vehicle.global_position_int.alt * 10e-4);
break;
case 1://相对
copk->setHeight(dlink->mavlinknode->vehicleList.value(currentUAV).global_position_int.relative_alt * 10e-4);
copk->setHeight(vehicle.global_position_int.relative_alt * 10e-4);
break;
case 2://气压
copk->setHeight(dlink->mavlinknode->vehicleList.value(currentUAV).vfr_hud.alt);
copk->setHeight(vehicle.vfr_hud.alt);
break;
}
copk->setAirSpeed(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.Airspeed,5);//真空速
copk->setAirSpeedTarget(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.Airspeed
+dlink->mavlinknode->vehicleList.value(currentUAV).nav_controller_output.aspd_error,5);
copk->setAirSpeed(vehicle.emb_atom_com.Airspeed,5);//真空速
copk->setAirSpeedTarget(vehicle.emb_atom_com.Airspeed
+vehicle.nav_controller_output.aspd_error,5);
//c t g m
switch (copk->AirSpeedFlag()) {
default:
case 0:
copk->setSpeed(dlink->mavlinknode->vehicleList.value(currentUAV).vfr_hud.airspeed,copk->AirSpeedFlag());//表速
copk->setSpeed(vehicle.vfr_hud.airspeed,copk->AirSpeedFlag());//表速
break;
case 1:
copk->setSpeed(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.Airspeed,copk->AirSpeedFlag());//真空速
copk->setSpeed(vehicle.emb_atom_com.Airspeed,copk->AirSpeedFlag());//真空速
break;
case 2:
copk->setSpeed(dlink->mavlinknode->vehicleList.value(currentUAV).gps_raw_int.vel * 0.01,copk->AirSpeedFlag());//地速
copk->setSpeed(vehicle.gps_raw_int.vel * 0.01,copk->AirSpeedFlag());//地速
break;
case 3:
copk->setSpeed(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.mach,copk->AirSpeedFlag());//马赫
copk->setSpeed(vehicle.emb_atom_com.mach,copk->AirSpeedFlag());//马赫
break;
}
copk->setAOA(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.alpha);
copk->setOL(dlink->mavlinknode->vehicleList.value(currentUAV).ins1.az/(-9.8));
copk->setAOA(vehicle.emb_atom_com.alpha);
copk->setOL(vehicle.ins1.az/(-9.8));
copk->setVerticalSpeed(-dlink->mavlinknode->vehicleList.value(currentUAV).global_position_int.vz * 10e-3);//速度朝下为正
copk->setVerticalSpeed(-vehicle.global_position_int.vz * 10e-3);//速度朝下为正
copk->setAlt_err(dlink->mavlinknode->vehicleList.value(currentUAV).nav_controller_output.alt_error);//高度差 飞机在航线下面为正
copk->setXTrack(dlink->mavlinknode->vehicleList.value(currentUAV).nav_controller_output.xtrack_error);//侧偏距 飞机在航线右侧为正
copk->setAlt_err(vehicle.nav_controller_output.alt_error);//高度差 飞机在航线下面为正
copk->setXTrack(vehicle.nav_controller_output.xtrack_error);//侧偏距 飞机在航线右侧为正
copk->setRollTarget(dlink->mavlinknode->vehicleList.value(currentUAV).nav_controller_output.nav_roll);//
copk->setPitchTarget(dlink->mavlinknode->vehicleList.value(currentUAV).nav_controller_output.nav_pitch);//
copk->setYawTarget(dlink->mavlinknode->vehicleList.value(currentUAV).nav_controller_output.nav_bearing);//
copk->setRollTarget(vehicle.nav_controller_output.nav_roll);//
copk->setPitchTarget(vehicle.nav_controller_output.nav_pitch);//
copk->setYawTarget(vehicle.nav_controller_output.nav_bearing);//
qDebug() << "1";
@@ -1240,14 +1269,14 @@ void MainWindow::updateUI()//事件驱动式更新数据
gps_str.clear();
switch (dlink->mavlinknode->vehicleList.value(currentUAV).gps_raw_int.fix_type) {
switch (vehicle.gps_raw_int.fix_type) {
case 0:
case 1:
gps_str.append(tr("Unlocated"));
break;
case 2:
case 3:
gps_str.append(tr("%1D").arg(dlink->mavlinknode->vehicleList.value(currentUAV).gps_raw_int.fix_type));
gps_str.append(tr("%1D").arg(vehicle.gps_raw_int.fix_type));
break;
case 4:
gps_str.append(tr("DGPS"));
@@ -1259,11 +1288,11 @@ void MainWindow::updateUI()//事件驱动式更新数据
gps_str.append(tr("INT"));
break;
default:
gps_str.append(tr("GPS %1").arg(dlink->mavlinknode->vehicleList.value(currentUAV).gps_raw_int.fix_type));
gps_str.append(tr("GPS %1").arg(vehicle.gps_raw_int.fix_type));
break;
}
copk->setGPS(gps_str);
copk->setSVN(dlink->mavlinknode->vehicleList.value(currentUAV).gps_raw_int.satellites_visible);
copk->setSVN(vehicle.gps_raw_int.satellites_visible);
qDebug() << "2";
@@ -1272,7 +1301,7 @@ void MainWindow::updateUI()//事件驱动式更新数据
uint8_t state = 0;
state = (dlink->mavlinknode->vehicleList.value(currentUAV).heartbeat.base_mode&MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED);
state = (vehicle.heartbeat.base_mode&MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED);
if(state != state_old)
{
@@ -1314,7 +1343,7 @@ void MainWindow::updateUI()//事件驱动式更新数据
}
copk->setARM(arm_str);
uint32_t custommode = dlink->mavlinknode->vehicleList.value(currentUAV).heartbeat.custom_mode;
uint32_t custommode = vehicle.heartbeat.custom_mode;
if(custommode != custommode_old)
{
@@ -1391,7 +1420,7 @@ void MainWindow::updateUI()//事件驱动式更新数据
QString state_str;
switch (dlink->mavlinknode->vehicleList.value(currentUAV).extended_sys_state.landed_state) {
switch (vehicle.extended_sys_state.landed_state) {
default:
case MAV_LANDED_STATE_UNDEFINED:
state_str = tr("undef");
@@ -1424,38 +1453,11 @@ void MainWindow::updateUI()//事件驱动式更新数据
mode_str.append(tr("flight mode"));
TTSsay(mode_str);
}
qDebug() << "4";
//经纬度大于正常值，将舍弃
for(QHash<int,MavLinkNode::_vehicle>::const_iterator ite = dlink->mavlinknode->vehicleList.constBegin();ite != dlink->mavlinknode->vehicleList.constEnd();++ite)
{
uint64_t time = ((uint64_t)vehicle.ins2.time)% 1000000;
MavLinkNode::_vehicle v = ite.value();
double lat = (double)(v.gps_raw_int.lat * 10e-8);
double lng = (double)(v.gps_raw_int.lon * 10e-8);
if(((lat > -90)&&(lat < 90))&&((lng > -180)&&(lng < 180)))
{
map->setUAVPos(v.sysid,
v.compid,
(double)(v.gps_raw_int.lat * 10e-8),
(double)(v.gps_raw_int.lon * 10e-8),
(double)(v.gps_raw_int.alt * 10e-4));
}
map->setUAVHeading(v.sysid,
v.compid,
v.attitude.yaw * 57.3);
}
qDebug() << "5";
uint64_t time = ((uint64_t)dlink->mavlinknode->vehicleList.value(currentUAV).ins2.time)% 1000000;
menuBarUI->setTargetAlt(dlink->mavlinknode->vehicleList.value(currentUAV).sys_status.load);
menuBarUI->setTargetAlt(vehicle.sys_status.load);
uint8_t hour = time / 10000;
@@ -1471,14 +1473,14 @@ void MainWindow::updateUI()//事件驱动式更新数据
menuBarUI->setTagetAirspeed(tim_str);
menuBarUI->setX(dlink->mavlinknode->vehicleList.value(currentUAV).nav_controller_output.xtrack_error);
menuBarUI->setX(vehicle.nav_controller_output.xtrack_error);
menuBarUI->setwp_Dist(((float)dlink->mavlinknode->vehicleList.value(currentUAV).nav_controller_output.wp_dist) * 0.001);
menuBarUI->setwp_Dist(((float)vehicle.nav_controller_output.wp_dist) * 0.001);
qDebug() << "6";
uint32_t health = dlink->mavlinknode->vehicleList.value(currentUAV).sys_status.onboard_control_sensors_health;
uint32_t enabled= dlink->mavlinknode->vehicleList.value(currentUAV).sys_status.onboard_control_sensors_enabled;
uint32_t health = vehicle.sys_status.onboard_control_sensors_health;
uint32_t enabled= vehicle.sys_status.onboard_control_sensors_enabled;
qDebug() << "6" << health << enabled;
@@ -1530,77 +1532,78 @@ void MainWindow::updateUI()//事件驱动式更新数据
//========================0
statusui->setValue(0,0,mode_str);
//========================1
qDebug() << dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.alpha
<< dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.beta;
qDebug() << vehicle.emb_atom_com.alpha
<< vehicle.emb_atom_com.beta;
statusui->setValue(1,0,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.alpha,'f',1));
statusui->setValue(1,0,QString::number(vehicle.emb_atom_com.alpha,'f',1));
qDebug() << "653";
//statusui->setValue(1,1,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.beta,'f',1));
statusui->setValue(1,1,QString::number(vehicle.emb_atom_com.beta,'f',1));
qDebug() << "654";
statusui->setValue(1,2,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).attitude.roll * 57.3,'f',1));
statusui->setValue(1,2,QString::number(vehicle.attitude.roll * 57.3,'f',1));
qDebug() << "655";
statusui->setValue(1,3,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).attitude.pitch * 57.3,'f',1));
statusui->setValue(1,3,QString::number(vehicle.attitude.pitch * 57.3,'f',1));
qDebug() << "656";
statusui->setValue(1,4,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).gps_raw_int.cog * 0.01,'f',1));
statusui->setValue(1,4,QString::number(vehicle.gps_raw_int.cog * 0.01,'f',1));
qDebug() << "66";
statusui->setValue(1,5,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).gps_raw_int.alt * 10e-4,'f',1));
statusui->setValue(1,5,QString::number(vehicle.gps_raw_int.alt * 10e-4,'f',1));
qDebug() << "661";
//statusui->setValue(1,6,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).vfr_hud.airspeed,'f',1));
statusui->setValue(1,6,QString::number(vehicle.vfr_hud.airspeed,'f',1));
qDebug() << "662";
//statusui->setValue(1,7,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.Airspeed,'f',1));
statusui->setValue(1,7,QString::number(vehicle.emb_atom_com.Airspeed,'f',1));
qDebug() << "663";
statusui->setValue(1,8,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).gps_raw_int.vel * 10e-3,'f',1));
statusui->setValue(1,8,QString::number(vehicle.gps_raw_int.vel * 10e-3,'f',1));
qDebug() << "664";
statusui->setValue(1,9,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.mach,'f',2));
statusui->setValue(1,9,QString::number(vehicle.emb_atom_com.mach,'f',2));
qDebug() << "665";
statusui->setValue(1,10,QString::number(-dlink->mavlinknode->vehicleList.value(currentUAV).global_position_int.vz * 10e-3,'f',1));
statusui->setValue(1,10,QString::number(-vehicle.global_position_int.vz * 10e-3,'f',1));
//========================4
qDebug() << "67";
QString throwkey;
throwkey.append((dlink->mavlinknode->vehicleList.value(currentUAV).rpm.rpm1 == 1)?(tr("投放|")):(tr("锁定|")));
throwkey.append((dlink->mavlinknode->vehicleList.value(currentUAV).rpm.rpm2 == 1)?(tr("投放")):(tr("锁定")));
throwkey.append((vehicle.rpm.rpm1 == 1)?(tr("投放|")):(tr("锁定|")));
throwkey.append((vehicle.rpm.rpm2 == 1)?(tr("投放")):(tr("锁定")));
statusui->setValue(4,0,throwkey);
throwkey.clear();
throwkey.append((dlink->mavlinknode->vehicleList.value(currentUAV).rpm.rpm4 == 0)?(tr("保险|")):(tr("解除|")));
throwkey.append((dlink->mavlinknode->vehicleList.value(currentUAV).rpm.rpm5 == 0)?(tr("工作")):(tr("上锁")));
throwkey.append((vehicle.rpm.rpm4 == 0)?(tr("保险|")):(tr("解除|")));
throwkey.append((vehicle.rpm.rpm5 == 0)?(tr("工作")):(tr("上锁")));
statusui->setValue(4,1,throwkey);
qDebug() << "68";
//========================5 battery
statusui->setValue(5,0,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).sys_status.voltage_battery * 0.001,'f',1));
statusui->setValue(5,1,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).battery_status.voltages[2] * 0.001,'f',1));
statusui->setValue(5,0,QString::number(vehicle.sys_status.voltage_battery * 0.001,'f',1));
statusui->setValue(5,1,QString::number(vehicle.battery_status.voltages[2] * 0.001,'f',1));
//========================6 dlink
}
qDebug() << "7";
toolsui->senser->setDAS(1,3,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).vfr_hud.alt,'f',1));
toolsui->senser->setDAS(1,4,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).vfr_hud.airspeed,'f',1));
toolsui->senser->setDAS(1,5,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.Airspeed,'f',1));
toolsui->senser->setDAS(1,6,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.mach,'f',3));
toolsui->senser->setDAS(1,7,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.qbar * 0.01,'f',3));
toolsui->senser->setDAS(1,8,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.ps * 0.01,'f',3));
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));
toolsui->senser->setDAS(1,6,QString::number(vehicle.emb_atom_com.mach,'f',3));
toolsui->senser->setDAS(1,7,QString::number(vehicle.emb_atom_com.qbar * 0.01,'f',3));
toolsui->senser->setDAS(1,8,QString::number(vehicle.emb_atom_com.ps * 0.01,'f',3));
toolsui->senser->setDAS(2,1,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.alpha,'f',1));
toolsui->senser->setDAS(2,2,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.beta,'f',1));
//toolsui->senser->setDAS(2,3,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).emb_atom_com.));
toolsui->senser->setDAS(2,4,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).vfr_hud.airspeed,'f',1));
toolsui->senser->setDAS(2,7,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).scaled_pressure.press_diff,'f',3));
toolsui->senser->setDAS(2,8,QString::number(dlink->mavlinknode->vehicleList.value(currentUAV).scaled_pressure.press_abs,'f',3));
toolsui->senser->setDAS(2,1,QString::number(vehicle.emb_atom_com.alpha,'f',1));
toolsui->senser->setDAS(2,2,QString::number(vehicle.emb_atom_com.beta,'f',1));
//toolsui->senser->setDAS(2,3,QString::number(vehicle.emb_atom_com.));
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));
qDebug() << "updateUI end";
}
void MainWindow::ins1_Update(mavlink_ins1_t ins)
{
App/mainwindow.h
+1 -2
View File
@@ -107,8 +107,7 @@ protected slots:
private slots:
void onTabIndexChanged(const int &index);
void updateUI();
void updateVehicle(MavLinkNode::_vehicle vehicle);
void TotalDistance(double value);
MavLinkNode/mavlinknode.cpp
+2
View File
@@ -1138,6 +1138,8 @@ void MavLinkNode::StatusParse(mavlink_message_t msg)
vehicleList.insert(msg.sysid,vehicle);//直接覆盖
emit signal_vehicle(vehicle);
mutex.unlock();
emit state_updated();
MavLinkNode/mavlinknode.h
+29 -2
View File
@@ -169,10 +169,37 @@ public:
signals:
void signal_servo_output_raw(mavlink_servo_output_raw_t servo);
void signal_autopilot_version();
void signal_sys_status();
void signal_heartbeat();
void signal_ping();
void signal_attitude();
void signal_ins1(mavlink_ins1_t ins);
void signal_ins2(mavlink_ins2_t ins);
void signal_gps_raw_int();
void signal_global_position_int();
void signal_servo_output_raw(mavlink_servo_output_raw_t servo);
void signal_rc_channels_raw();
void signal_nav_controller_output();
void signal_airspeed_autocal();
void signal_rpm();
void signal_scaled_pressure();
void signal_extended_sys_state();
void signal_battery_status();
void signal_vibration();
void signal_enginestate();
void signal_vfr_hud();
void signal_aoa_ssa();
void signal_emb_atom_com();
void signal_turbinstate();
void signal_bmustate();
void signal_ccmstate();
void signal_serial_control();
void signal_vehicle(MavLinkNode::_vehicle vehicle);
void updateDlink(float rssi,uint64_t in,uint64_t out);