QGC 地面站中获取电压完整教程(QGC中无法获取APM电压)
Posted 火山上的企鹅
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了QGC 地面站中获取电压完整教程(QGC中无法获取APM电压)相关的知识,希望对你有一定的参考价值。
文章目录
关于QGC地面站其它文章请点击这里: QGC地面站
一、演示
老规先演示,看效果:
PX4 中:
数据列表中的 voltage 为电池组中获取的,PX4 中可以获取到
APM 中:
数据列表中的 voltage 为电池组中获取的,APM 中获取不到
二、简介
起因是在 APM 中无法在BATTERY_STATUS 帧中获取电压信息
而在 SYS_STATUS 系统状态中存在电压信息,于是乎想调用 SYS_STATUS 中的 voltage_battery
同样 PX4 中也可这样操作, 你如果不想改地面站,那就改飞控的 BATTERY_STATUS 数据源吧~
三、代码修改
1. 后台数据获取
● Vehicle.cc
//92:定义字段,将通过 "myVoltage" 找到 voltage 的数据
const char* Vehicle::_myVoltageFactName = "myVoltage";
//149:初始化 _myVoltageFact
, _myVoltageFact (0, _myVoltageFactName, FactMetaData::valueTypeDouble)
//297
, _myVoltageFact (0, _myVoltageFactName, FactMetaData::valueTypeDouble)
//404
_addFact(&_myVoltageFact, _myVoltageFactName);
//1339 在 _handleSysStatus(mavlink_message_t& message) 函数中获取电压
double voltage = static_cast<double>(sysStatus.voltage_battery)/1000;
_myVoltageFact.setRawValue(voltage);
● 2. Vehicle.h
//293
Q_PROPERTY(Fact* myVoltage READ myVoltage CONSTANT)
//609
Fact* myVoltage () return &_myVoltageFact;
//1217
Fact _myVoltageFact;
//1262
static const char* _myVoltageFactName;
2. json 文件
"name": "myVoltage",
"shortDesc": "Current voltage",
"type": "double",
"decimalPlaces": 2,
"units": "v"
3. 前台显示
● 在 FlyViewWidgetLayer.qml 最后调用:
VoltageTest
anchors.top: parent.top
anchors.topMargin: 50
anchors.horizontalCenter: parent.horizontalCenter
● VoltageTest
可以参考此文 Qt QML 基于画布的扇形的进度条(Canvas)
import QtQuick 2.12
import QGroundControl.Vehicle 1.0
import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Controls 1.0
import QGroundControl.FactSystem 1.0
import QGroundControl.FactControls 1.0
Rectangle
id: valuesRoot
width: rowRoot.width + 10
height: rowRoot.height + 10
color: "black"
///----内部使用:
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.offlineEditingVehicle
///---另外个版本获取电压
property var voltageVal: _activeVehicle ? _activeVehicle.voltage0 : 0
property var voltageMax: 4.2 * 6 //6s
property var voltageMin: 3.7 * 6 //6s
///--只读常量
readonly property color _themeColor: "#00EC00"
readonly property real _mar: 5
readonly property real defaultWidgetsWidth: (ScreenTools.isMobile ? 50*2 : 50)
readonly property real defaultWidgetsHeight: (ScreenTools.isMobile ? 35*2 : 35)
readonly property real _pointSize: 12
///--获取模型的颜色
function getModelColor(fact)
//voltage 22.2~25.2
if(fact > voltageMin + (voltageMax-voltageMin) * 0.3) return _themeColor
if(fact > voltageMin + (voltageMax-voltageMin) * 0.1) return "yellow"
if(fact >= voltageMin + (voltageMax-voltageMin) * 0.0) return "red"
return _themeColor
//电压显示
Row
id: rowRoot
anchors.centerIn: parent
spacing: 5
property Fact fact: _activeVehicle.getFact("myVoltage");
Column
spacing: 0
anchors.verticalCenter: parent.verticalCenter
QGCLabel
id: sgLabel
color: getModelColor(rowRoot.fact.enumOrValueString)
horizontalAlignment: Text.AlignHCenter
text: qsTr("电压")
font.pointSize: _pointSize
font.bold: true
QGCLabel
horizontalAlignment: Text.AlignHCenter
color: sgLabel.color
text: rowRoot.fact.valueEqualsDefault ? " " : rowRoot.fact.value.toFixed(1) + rowRoot.fact.units
font.pointSize: _pointSize
font.bold: true
Loader
id: loaderDash
anchors.verticalCenter: parent.verticalCenter
sourceComponent: canvasDash
Component.onCompleted:
loaderDash.item._text = "v"
///--更新值和颜色
Timer
id: timer
interval: 1000
running: true
repeat: true
onTriggered:
var val = isNaN(rowRoot.fact.enumOrValueString) ? 0.0 : rowRoot.fact.enumOrValueString //初步判断是否有效
val = (val<0) ? 0 : val //初步判断是否为0
val= (val - voltageMin) / (voltageMax - voltageMin) * 100
loaderDash.item._value = (val<0) ? 0 : val
loaderDash.item._color = sgLabel.color
///------------------ 动态小仪表 ------------------
///电压和油门
Component
id: canvasDash
Item
width: defaultWidgetsWidth
height: defaultWidgetsHeight
anchors.centerIn: parent
property real _value : 0
property real _angle: (_value * (180-10) / 100 + (180+10))
property string _text : ""
property color _color: '#01E9A9'
property color _backgroundColor: "white"
on_ValueChanged: canvas.requestPaint();
Canvas ///画布
id: canvas
width: parent.width //40
height: width/2 //20
anchors.centerIn: parent
contextType: "2d";
function paintGimbalYaw(ctx,x,y,r,angle1,angle2,color)
ctx.fillStyle = color
ctx.save();
ctx.beginPath();
ctx.moveTo(x,y);
ctx.arc(x,y,r,angle1*Math.PI/180,angle2*Math.PI/180);
ctx.closePath();
ctx.fill()
ctx.restore();
onPaint:
var ctx = getContext("2d"); ///画师
paintGimbalYaw(ctx,canvas.width/2, canvas.height, canvas.width/2, 180, 360, _backgroundColor)//'#005840')
paintGimbalYaw(ctx,canvas.width/2, canvas.height, canvas.width/2, 180, _angle, _color)
paintGimbalYaw(ctx,canvas.width/2, canvas.height, canvas.width/2*0.85, 0, 360, "#142c29")
paintGimbalYaw(ctx,canvas.width/2, canvas.height, canvas.width/2*0.7, 180, 360, _backgroundColor) //'#005840')
paintGimbalYaw(ctx,canvas.width/2, canvas.height, canvas.width/2*0.7, 180,_angle, _color)
paintGimbalYaw(ctx,canvas.width/2, canvas.height, canvas.width/2*0.6, 0, 360, "#142c29")
//文字
QGCLabel
id: txt_progress
visible: _text != ""
anchors.bottom: parent.bottom
anchors.bottomMargin: ScreenTools.isMobile ? -7 : -2
anchors.horizontalCenter: parent.horizontalCenter
font.pointSize: 11
text: _text //"V"
color: _color
font.bold: true
如果多个数据,加入Repeater-model 这将是个很好的模型框架
4. APM 中电压校准
就是此处的电压:
// property Fact vehicleVoltage: _batteryFactGroup.voltage
property Fact vehicleVoltage: controller.vehicle.myVoltage
PX4 中没有修改校准,可以自己动手,其实一样
5. 工程文件部分
● qgroundcontrol.qrc
<file alias="QGroundControl/FlightDisplay/VoltageTest.qml">src/FlightDisplay/VoltageTest.qml</file>
● FlightDisplay\\qmldir
VoltageTest 1.0 VoltageTest.qml
四、其它方法介绍(PX4)
在 v4.1 后的版本中,是动态实例化的电池组,咱们也从电池组中快速获取电压,只贴出关键代码:
vehicle.cc
//构造函数中开定时器不断调用 getVoltage0()
connect(&_upVoltageTimer, &QTimer::timeout, this, &Vehicle::getVoltage0);
_upVoltageTimer.start(1000);
bool Vehicle::getVoltage0(void)
if(_batteryFactGroupListModel.count() < 1)
qDebug() << ("Valtage invalid!!!");
return false;
QmlObjectListModel* batteriesList = batteries();
VehicleBatteryFactGroup * groop = batteriesList->value<VehicleBatteryFactGroup*>(0);
_voltage0 = groop->voltage()->enumOrValueString();
emit voltage0Changed(_voltage0);
return true;
//start_cch_20210714
void Vehicle::setVoltage0(QString voltage0)
if(_voltage0 != voltage0)
_voltage0 = voltage0;
emit voltage0Changed(_voltage0);
前台调用:
property var voltageVal: _activeVehicle ? _activeVehicle.voltage0 : 0
以上是关于QGC 地面站中获取电压完整教程(QGC中无法获取APM电压)的主要内容,如果未能解决你的问题,请参考以下文章
QGC 地面站中获取电压完整教程(QGC中无法获取APM电压)
QGC地面站软件名和Logo修改(QGroundControl)