Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
/*!
* @file
* @brief Camera Controller
* @author Gus Grubba <mavlink@grubba.com>
*
*/
#include "QGCApplication.h"
#include "QGCCameraManager.h"
QGC_LOGGING_CATEGORY(CameraManagerLog, "CameraManagerLog")
//-----------------------------------------------------------------------------
QGCCameraManager::QGCCameraManager(Vehicle *vehicle)
: _vehicle(vehicle)
, _vehicleReadyState(false)
, _cameraCount(0)
, _currentTask(0)
{
qCDebug(CameraManagerLog) << "QGCCameraManager Created";
connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &QGCCameraManager::_vehicleReady);
connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &QGCCameraManager::_mavlinkMessageReceived);
}
//-----------------------------------------------------------------------------
QGCCameraManager::~QGCCameraManager()
{
}
//-----------------------------------------------------------------------------
QString
QGCCameraManager::controllerSource()
{
return QStringLiteral("/qml/CameraControl.qml");
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_vehicleReady(bool ready)
{
qCDebug(CameraManagerLog) << "_vehicleReady(" << ready << ")";
if(ready) {
if(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle() == _vehicle) {
_vehicleReadyState = true;
}
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_mavlinkMessageReceived(const mavlink_message_t& message)
{
switch (message.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
_handleHeartbeat(message);
break;
case MAVLINK_MSG_ID_CAMERA_INFORMATION:
_handleCameraInfo(message);
break;
case MAVLINK_MSG_ID_CAMERA_SETTINGS:
_handleCameraSettings(message);
break;
case MAVLINK_MSG_ID_PARAM_EXT_ACK:
_handleParamAck(message);
break;
case MAVLINK_MSG_ID_PARAM_EXT_VALUE:
_handleParamValue(message);
break;
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_handleHeartbeat(const mavlink_message_t &message)
{
mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(&message, &heartbeat);
//-- If this heartbeat is from a different node within the vehicle
if(_vehicleReadyState && _vehicle->id() == message.sysid && _vehicle->defaultComponentId() != message.compid) {
if(!_cameraInfoRequested[message.compid]) {
_cameraInfoRequested[message.compid] = true;
//-- Request camera info
_requestCameraInfo(message.compid);
}
}
}
//-----------------------------------------------------------------------------
QGCCameraControl*
QGCCameraManager::_findCamera(int id)
{
for(int i = 0; i < _cameras.count(); i++) {
if(_cameras[i]) {
QGCCameraControl* pCamera = qobject_cast<QGCCameraControl*>(_cameras[i]);
if(pCamera) {
if(pCamera->compID() == id) {
return pCamera;
}
} else {
qCritical() << "Null QGCCameraControl instance";
}
}
}
qWarning() << "Camera id not found:" << id;
return NULL;
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_handleCameraInfo(const mavlink_message_t& message)
{
mavlink_camera_information_t info;
mavlink_msg_camera_information_decode(&message, &info);
qCDebug(CameraManagerLog) << "_handleCameraInfo:" << (const char*)(void*)&info.model_name[0] << (const char*)(void*)&info.vendor_name[0];
_cameraCount = info.camera_count;
QGCCameraControl* pCamera = _vehicle->firmwarePlugin()->createCameraControl(&info, _vehicle, message.compid, this);
if(pCamera) {
_cameras.append(pCamera);
emit camerasChanged();
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_handleCameraSettings(const mavlink_message_t& message)
{
QGCCameraControl* pCamera = _findCamera(message.compid);
if(pCamera) {
mavlink_camera_settings_t settings;
mavlink_msg_camera_settings_decode(&message, &settings);
pCamera->handleSettings(settings);
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_handleParamAck(const mavlink_message_t& message)
{
QGCCameraControl* pCamera = _findCamera(message.compid);
if(pCamera) {
mavlink_param_ext_ack_t ack;
mavlink_msg_param_ext_ack_decode(&message, &ack);
pCamera->handleParamAck(ack);
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_handleParamValue(const mavlink_message_t& message)
{
QGCCameraControl* pCamera = _findCamera(message.compid);
if(pCamera) {
mavlink_param_ext_value_t value;
mavlink_msg_param_ext_value_decode(&message, &value);
pCamera->handleParamValue(value);
}
}
//-----------------------------------------------------------------------------
void
QGCCameraManager::_requestCameraInfo(int compID)
{
qCDebug(CameraManagerLog) << "_requestCameraInfo(" << compID << ")";
if(_vehicle) {
_vehicle->sendMavCommand(
compID, // target component
MAV_CMD_REQUEST_CAMERA_INFORMATION, // command id
false, // showError
0, // Camera ID (0 for all, 1 for first, 2 for second, etc.)
1); // Do Request
}
}